Allow specifying a program for the two motors

to follow sequentially.
akkana · May 30, 2013 · f108a15 · f108a15
1 parent 3611b6c
commit f108a15
Showing 1 changed file with 45 additions and 10 deletions.
diff --git a/motor-tests/halfbridge/halfbridge.ino b/motor-tests/halfbridge/halfbridge.ino
@@ -2,28 +2,63 @@
 
 #include <HalfBridge.h>
 
+#define DEBUG 1
+
 Motor motors[2] = { Motor(9, 2, 3), Motor(10, 4, 5) };
 
+#define FAST 230
+#define SLOW 180
+
+#define MAXSTEPS 14
+#ifdef NOTDEF
+int program[MAXSTEPS][2] = { {0, 0}, {SLOW, SLOW}, {SLOW, SLOW}, {SLOW, 0},
+                             {SLOW, -SLOW}, {SLOW, -SLOW}, {0, 0},
+                             {SLOW, SLOW}, {FAST, FAST}, {SLOW, SLOW},
+                             {0, 0}, {-SLOW, -SLOW}, {0, -SLOW},
+                             {0, 0} };
+#endif
+
+int program[][2] = { {0, 0}, {SLOW, SLOW}, {SLOW, SLOW}, {SLOW, 0},
+                             {0, 0}, {0, -SLOW},
+                             {0, 0} };
+
+int NUMSTEPS = ((sizeof program) / (sizeof *program));
+
+int curstep = 0;
+
 void setup()
 {
     motors[0].init();
     motors[1].init();
-}
 
-#define FAST 200
-#define SLOW 130
+#ifdef DEBUG
+    Serial.begin(9600);
+    Serial.print(NUMSTEPS);
+    Serial.println(" steps in program");
+#endif
+
+    delay(8000);
+}
 
 void loop()
 {
-    if (motors[0].mSpeed == 0 ) {
-        motors[0].setSpeed(SLOW);
-        motors[1].setSpeed(-SLOW);
-    } else if (abs(motors[0].mSpeed) >= FAST) {
+    if (curstep < NUMSTEPS) {
+        motors[0].setSpeed(program[curstep][0]);
+        motors[1].setSpeed(program[curstep][1]);
+
+#ifdef DEBUG
+        Serial.print("Setting speeds: ");
+        Serial.print(program[curstep][0]);
+        Serial.print(", ");
+        Serial.print(program[curstep][1]);
+        Serial.println(".");
+#endif
+
+        ++curstep;
+    }
+    else {
         motors[0].setSpeed(0);
         motors[1].setSpeed(0);
-    } else {
-        motors[0].setSpeed(FAST);
-        motors[1].setSpeed(FAST);
     }
 
     delay(2000);