Make the manual mode work properly without calling process

kodestarter · Nov 20, 2014 · c82079c · c82079c
1 parent 936a9fb
commit c82079c
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 13 deletions.
diff --git a/Mirobot.cpp b/Mirobot.cpp
@@ -14,6 +14,19 @@ Mirobot::Mirobot(){
   lastLedChange = millis();
 }
 
+void Mirobot::setup(){
+  HotStepper::setup(TIMER1INT);
+  // Set up the pen arm servo
+  pinMode(SERVO_PIN, OUTPUT);
+  // Set up the collision sensor inputs and state
+  pinMode(LEFT_COLLIDE_SENSOR, INPUT_PULLUP);
+  pinMode(RIGHT_COLLIDE_SENSOR, INPUT_PULLUP);
+  collideState = NORMAL;
+  //setPenState(UP);
+  // Set up the status LED
+  pinMode(STATUS_LED, OUTPUT);
+}
+
 void Mirobot::setup(Stream &s){
   HotStepper::setup(TIMER1INT);
   // Set up the pen arm servo
@@ -105,10 +118,12 @@ void Mirobot::right(int angle){
 
 void Mirobot::penup(){
   setPenState(UP);
+  wait();
 }
 
 void Mirobot::pendown(){
   setPenState(DOWN);
+  wait();
 }
 
 void Mirobot::pause(){
@@ -152,13 +167,13 @@ boolean Mirobot::ready(){
   return (motor1.ready() && motor2.ready() && !servo_pulses_left);
 }
 
-void Mirobot::setBlocking(boolean val){
-  blocking = val;
-}
-
 void Mirobot::wait(){
   if(blocking){
-    while(!ready()){}
+    while(!ready()){
+      if(servo_pulses_left){
+        servoHandler();
+      }
+    }
   }
 }
 

diff --git a/Mirobot.h b/Mirobot.h
@@ -47,6 +47,7 @@ struct HwVersion {
 class Mirobot {
   public:
     Mirobot();
+    void setup();
     void setup(Stream &s);
     void forward(int distance);
     void back(int distance);
@@ -63,9 +64,9 @@ class Mirobot {
     void beep(int);
     void setHwVersion(char&);
     boolean ready();
-    void setBlocking(boolean val);
     void process();
     HwVersion hwVersion;
+    boolean blocking;
   private:
     void wait();
     void followHandler();
@@ -78,7 +79,6 @@ class Mirobot {
     mainState_t mainState;
     collideState_t collideState;
     unsigned long lastLedChange;
-    boolean blocking;
     Mirobot& self() { return *this; }
     penState_t penState;
     void setPenState(penState_t);

diff --git a/examples/manual_example/manual_example.ino b/examples/manual_example/manual_example.ino
@@ -1,15 +1,12 @@
-#define HOTSTEPPER_TIMER1
-#include <stdlib.h>
-#include <HotStepper.h>
 #include <Mirobot.h>
 #include <EEPROM.h>
 
-Mirobot mirobot;
-
 /*
 This sketch shows how you can program Mirobot directly in the Arduino environment.
 */
 
+Mirobot mirobot;
+
 void setup(){
   mirobot.setup();
 }
@@ -33,4 +30,4 @@ void loop(){
   mirobot.penup();
 
   delay(10000);
-}
+}