Diff: main.cpp

Revision:

0:1dadb580b75f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 06 04:17:08 2019 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+#include "Motor.h"
+
+Motor leftWheel(p23, p16, p15); //pwm, fwd(Digital), rev(Digital)
+Motor rightWheel(p24, p18, p17); //pwm, fwd(Digital), rev(Digital)
+DigitalIn fwd(p5);
+DigitalIn rev(p6);
+DigitalIn turnL(p7);
+DigitalIn turnR(p8);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+
+void moveForward(float time)
+{
+    leftWheel.speed(0.6);
+    rightWheel.speed(0.6);
+    wait(time);
+    leftWheel.speed(0.0);
+    rightWheel.speed(0.0);
+}
+
+void moveBackward(float time)
+{
+    leftWheel.speed(-0.6);
+    rightWheel.speed(-0.6);
+    wait(time);
+    leftWheel.speed(0.0);
+    rightWheel.speed(0.0);
+}
+
+void turnLeft(float time)
+{
+    leftWheel.speed(-0.6);
+    rightWheel.speed(0.6);
+    wait(time);
+    leftWheel.speed(0.0);
+    rightWheel.speed(0.0);
+}
+
+void turnRight(float time)
+{
+    leftWheel.speed(0.6);
+    rightWheel.speed(-0.6);
+    wait(time);
+    leftWheel.speed(0.0);
+    rightWheel.speed(0.0);
+}
+
+int main()
+{
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+    leftWheel.speed(0.0);
+    rightWheel.speed(0.0);
+    
+    while (1)
+    {
+        if (fwd)
+        {
+            led2 = 1;
+            led3 = 1;
+            moveForward(0.2);
+            led2 = 0;
+            led3 = 0;
+        }
+        if (rev)
+        {
+            led1 = 1;
+            led4 = 1;
+            moveBackward(0.2);
+            led1 = 0;
+            led4 = 0;
+        }
+        if (turnL)
+        {
+            led1 = 1;
+            led2 = 1;
+            turnLeft(0.2);
+            led1 = 0;
+            led2 = 0;
+        }
+        if (turnR)
+        {
+            led3 = 1;
+            led4 = 1;
+            turnRight(0.2);
+            led3 = 0;
+            led4 = 0;
+        }
+    }
+}