Diff: main.cpp

Revision:

8:e282aa7c1fe9

Parent:

7:ad8295723268

--- a/main.cpp	Wed Jul 16 10:20:45 2014 +0000
+++ b/main.cpp	Tue Oct 06 20:40:58 2015 +0000
@@ -1,19 +1,71 @@
 #include "mbed.h"
 
-PwmOut r(LED_RED);
-PwmOut g(LED_GREEN);
+Serial pc(USBTX,USBRX);
+
+PwmOut M1(PTD0);
+PwmOut M2(PTD1);
+PwmOut M3(PTD2);
+PwmOut M4(PTD3);
+
+//LED Outs
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+
+DigitalIn motion(D2);
 
 int main()
 {
-    r.period(0.001f);
-    g.period(0.001f);
-
+    pc.printf("Loading...\n\r");
+    pc.printf("%c\n\r",motion);
+     M1.period(0.0025f);
+    M2.period(0.0025f);
+    M3.period(0.0025f);
+    M4.period(0.0025f);
+//Motor Off
+    M1.write(0.0f);
+    M2.write(0.0f);
+    M3.write(0.0f);
+    M4.write(0.00f);
+    led_red = 1;
+    led_green = 0;
+    wait (2.0f);
+//Wait for Motion         
     while (true) {
-        for (float i = 0.0f; i < 1.0f ; i += 0.001f) {
-            float p = 3 * i;
-            r = 1.0f - ((p < 1.0f) ? 1.0f - p : (p > 2.0f) ? p - 2.0f : 0.0f);
-            g = 1.0f - ((p < 1.0f) ? p : (p > 2.0f) ? 0.0f : 2.0f - p);
-            wait (0.0025f);
-        }
+    led_red = 1;
+    led_green = 0;
+    pc.printf("Motors off; Waiting for motion...\n\r");
+//Motor Off
+    M1.write(0.0f);
+    M2.write(0.0f);
+    M3.write(0.0f);
+    M4.write(0.0f);
+    wait (.0025f);
+          if(motion){
+            pc.printf("Motors On");
+            led_red = 0;
+            led_green = 1;
+            M1.write(0.5f);  //Anything less than 50% duty cycle did not spin my motors.
+            M2.write(0.5f);
+            M3.write(0.5f);
+            M4.write(0.5f);
+            wait (.0025f);             
+            }
+            //RAMP DOWN SEQUENCE to avoid ESC protection
+            M1.write(0.25f);
+            M2.write(0.25f);
+            M3.write(0.25f);
+            M4.write(0.25f);
+            wait (.0025f);
+            M1.write(0.15f);
+            M2.write(0.15f);
+            M3.write(0.15f);
+            M4.write(0.15f);
+            wait (.0025f);
+            M1.write(0.0f);
+            M2.write(0.0f);
+            M3.write(0.0f);
+            M4.write(0.0f);
+            wait (.0025f);
+          }
     }
-}
+