Diff: main.cpp

Revision:

1:ed4310cb2e3d

Parent:

0:bdcbec496e71

--- a/main.cpp	Tue Feb 05 02:39:17 2013 +0000
+++ b/main.cpp	Tue Feb 05 04:10:19 2013 +0000
@@ -4,14 +4,13 @@
 
 DigitalOut m1a(p29), m1b(p30), m2a(p27), m2b(p28);
 PwmOut m1p(p26), m2p(p25);
-PwmOut m3p(p23), m4p(p24);
 
 void motor (int num, float speed, int brake) {
     if (num == 1) {
         if (brake) {
             m1p = 1;
-            m1a = 0;
-            m1b = 0;
+            m1a = 1;
+            m1b = 1;
         } else
         if (speed > 0) {
             m1p = speed;
@@ -31,8 +30,8 @@
     if (num == 2) {
         if (brake) {
             m2p = 1;
-            m2a = 0;
-            m2b = 0;
+            m2a = 1;
+            m2b = 1;
         } else
         if (speed > 0) {
             m2p = speed;
@@ -51,52 +50,58 @@
     }
 }
 
-void servo (int num, float deg) {
-    if (num == 3) {
-        m3p.period_ms(20);
-        m3p.pulsewidth_us(1500 + (int)(500.0 * deg));
-    } else
-    if (num == 4) {
-        m4p.period_ms(20);
-        m4p.pulsewidth_us(1500 + (int)(500.0 * deg));
+void stepper (int n) {
+    switch (n) {
+    case 0:
+        motor(1, 1, 0);
+        motor(2, 0, 0);
+        led1 = 1;
+        break;
+    case 1:
+        motor(1, 1, 0);
+        motor(2, 1, 0);
+        led1 = 0;
+        break;
+    case 2:
+        motor(1, 0, 0);
+        motor(2, 1, 0);
+        led2 = 1;
+        break;
+    case 3:
+        motor(1, -1, 0);
+        motor(2, 1, 0);
+        led2 = 0;
+        break;
+    case 4:
+        motor(1, -1, 0);
+        motor(2, 0, 0);
+        led3 = 1;
+        break;
+    case 5:
+        motor(1, -1, 0);
+        motor(2, -1, 0);
+        led3 = 0;
+        break;
+    case 6:
+        motor(1, 0, 0);
+        motor(2, -1, 0);
+        led4 = 1;
+        break;
+    case 7:
+        motor(1, 1, 0);
+        motor(2, -1, 0);
+        led4 = 0;
+        break;
     }
 }
 
 int main() {
-    float f;
+    int i;
 
     while(1) {
-        led1 = 1;
-        for (f = 0; f < 1; f += 0.02) {
-            motor(1, f, 0);
-            motor(2, f, 0);
-            servo(3, f);
-            servo(4, f);
-            wait(0.1);
+        for (i = 0; i < 8; i ++) {
+            stepper(i);
+            wait(0.002);
         }
-        led1 = 0;
-        led2 = 1;
-        for (f = 1; f > -1; f -= 0.02) {
-            motor(1, f, 0);
-            motor(2, f, 0);
-            servo(3, f);
-            servo(4, f);
-            wait(0.1);
-        }
-        led2 = 0;
-        led3 = 1;
-        for (f = -1; f < 0; f += 0.02) {
-            motor(1, f, 0);
-            motor(2, f, 0);
-            servo(3, f);
-            servo(4, f);
-            wait(0.1);
-        }
-        led3 = 0;
-        led4 = 1;
-        motor(1, 0, 1);
-        motor(2, 0, 1);
-        wait(1);
-        led4 = 0;
     }
 }