Diff: main.cpp

Revision:

8:ed9204f71104

Parent:

4:81cea7a352b0

--- a/main.cpp	Thu Mar 26 22:33:50 2015 +0000
+++ b/main.cpp	Mon Mar 12 23:01:33 2018 +0000
@@ -1,12 +1,54 @@
 #include "mbed.h"
+#include "Servo.h"
+ 
+Servo myservo(PTB0);
 
-DigitalOut myled(LED1);
+DigitalOut myled(LED3);
+PwmOut motorA(PTD0);
+PwmOut motorB(PTD5);
+DigitalOut dirB1(PTA13);
+DigitalOut dirB2(PTC9);
+DigitalOut dirA2(PTC8);
+DigitalOut dirA1(PTA5);
+
+float p = 0;
 
 int main() {
+
     while(1) {
+        myservo = p;
+        p += 0.05;
+        if (p > 1)
+          p = 0;
         myled = 1;
-        wait(0.2);
+        dirB1 = 1;
+        dirB2 = 0;
+        dirA1 = 0;
+        dirA2 = 1;
+        motorA.period(0.01f);
+        motorA.write(0.0f);
+        motorB.period(0.01f);
+        motorB.write(0.0f);
+        wait(1);
         myled = 0;
-        wait(0.2);
+        motorA.write(0.5f);
+        motorB.write(0.5f);
+        wait(1);
+        
+        myled = 1;
+        dirB1 = 0;
+        dirB2 = 1;
+        dirA1 = 1;
+        dirA2 = 0;
+        motorA.period(0.01f);
+        motorA.write(0.0f);
+        motorB.period(0.01f);
+        motorB.write(0.0f);
+        wait(1);
+        myled = 0;
+        motorA.write(0.5f);
+        motorB.write(0.5f);
+        wait(1);
+
     }
 }