Diff: main.cpp

Revision:

0:423866e831fb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 14 05:14:10 2019 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include "pins.h"
+#include "Servo.h"
+ 
+Serial pc(USBTX, USBRX);
+AnalogIn photocell1(p18);
+AnalogIn photocell2(p17);
+AnalogIn photocell3(p20);
+AnalogIn photocell4(p19);
+
+float s1;
+float s2;
+float s3;
+float s4;
+
+void moveUp () {
+    if (servoControl < 1.0)
+        servoControl = servoControl + .01;
+}
+
+void moveDown () {
+    if (servoControl > 0.0)
+        servoControl = servoControl - .01;
+}
+
+void moveLeft () {
+    if (driverDir == 1)
+        driverDir = 0;
+    for (int i = 0; i < 50; i++) {
+        driverStep = !driverStep;
+        wait(0.0005);
+        driverStep = !driverStep;
+        wait(0.0005);
+    }
+}
+
+void moveRight () {
+    if (driverDir == 0)
+        driverDir = 1;
+    for (int i = 0; i < 50; i++) {
+        driverStep = !driverStep;
+        wait(0.0005);
+        driverStep = !driverStep;
+        wait(0.0005);
+    }
+}
+
+void readSensors () {
+    s1 = photocell1;
+    s2 = photocell2;
+    s3 = photocell3;
+    s4 = photocell4;
+}
+
+int main () {
+    while(1) {
+        readSensors ();
+        if (s1 - s2 > .05 && s3 - s4 > .05) {
+            moveUp ();
+            moveLeft ();
+        } else if (s2 - s1 > .05 && s3 - s4 > .05) {
+            moveUp ();
+            moveRight ();
+        } else if (s2 - s1 > .05 && s4 - s3 > .05) {
+            moveDown ();
+            moveRight ();
+        } else if (s1 - s2 > .05 && s4 - s3 > .05) {
+            moveDown ();
+            moveLeft ();
+        } else if (s1 - s2 > .05) {
+            moveLeft ();
+        } else if (s2 - s1 > .05) {
+            moveRight ();
+        } else if (s4 - s3 > .05) {
+            moveDown ();
+        } else if (s3 - s4 > .05) {
+            moveUp ();
+        }
+        wait(.05);
+    }
+}
\ No newline at end of file