Diff: main.cpp

Revision:

0:bdcbec496e71

Child:

1:02b46ad7dcac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 05 02:39:17 2013 +0000
@@ -0,0 +1,102 @@
+#include "mbed.h"
+
+DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
+
+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;
+        } else
+        if (speed > 0) {
+            m1p = speed;
+            m1a = 1;
+            m1b = 0;
+        } else
+        if (speed < 0) {
+            m1p = - speed;
+            m1a = 0;
+            m1b = 1;
+        } else {
+            m1p = 1;
+            m1a = 0;
+            m1b = 0;
+        }
+    } else
+    if (num == 2) {
+        if (brake) {
+            m2p = 1;
+            m2a = 0;
+            m2b = 0;
+        } else
+        if (speed > 0) {
+            m2p = speed;
+            m2a = 1;
+            m2b = 0;
+        } else
+        if (speed < 0) {
+            m2p = - speed;
+            m2a = 0;
+            m2b = 1;
+        } else {
+            m2p = 1;
+            m2a = 0;
+            m2b = 0;
+        }
+    }
+}
+
+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));
+    }
+}
+
+int main() {
+    float f;
+
+    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);
+        }
+        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;
+    }
+}