Diff: main.cpp

Revision:

8:840b3b80cfa2

Parent:

4:81cea7a352b0

Child:

9:ada1a8a81354

--- a/main.cpp	Thu Mar 26 22:33:50 2015 +0000
+++ b/main.cpp	Mon May 22 21:17:29 2017 +0000
@@ -1,12 +1,79 @@
 #include "mbed.h"
 
-DigitalOut myled(LED1);
+DigitalOut drive_led(LED2);
+PwmOut motorA(PTD0);
+PwmOut motorB(PTD5);
+DigitalOut dirB1(PTA13);
+DigitalOut dirB2(PTC9);
+DigitalOut dirA2(PTC8);
+DigitalOut dirA1(PTA5);
+
+/* set pulse width for motor drive signals */
+void drive_init(void)
+{
+    motorA.period(0.01f);
+    motorB.period(0.01f);
+}
 
-int main() {
-    while(1) {
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
+/* set signals to move motors */
+/* bias min c. 25, max 100 */
+void move(int bias_a, int bias_b, int milliseconds)
+{
+    if ((bias_a == 0) && (bias_b == 0))
+        drive_led = 1;
+    else
+        drive_led = 0;
+    if (bias_a < 0)
+    {
+        dirA1 = 0;
+        dirA2 = 1;
+    }
+    else if (bias_a > 0)
+    {
+        dirA1 = 1;
+        dirA2 = 0;
+    }
+    else
+    {
+        dirA1 = 1;
+        dirA2 = 1;
+    }
+    if (bias_b < 0)
+    {
+        dirB1 = 1;
+        dirB2 = 0;
+    }
+    else if (bias_b > 0)
+    {
+        dirB1 = 0;
+        dirB2 = 1;
+    }
+    else
+    {
+        dirB1 = 1;
+        dirB2 = 1;
+    }
+    motorA.write((float)abs(bias_a) / 100);
+    motorB.write((float)abs(bias_b) / 100);
+    wait_ms(milliseconds);
+}
+
+int main()
+{    
+    drive_init();
+    
+    move(0, 0, 500);
+    move(40, 40, 500);
+    move(0, 100, 300);
+    move(40, 40, 500);
+    move(0, 100, 300);
+    move(40, 40, 500);
+    move(0, 100, 300);
+    move(40, 40, 500);
+    move(0, 100, 300);
+
+    while(1)
+    {
+        move(0, 0, 500);
     }
 }