Selen Krgo
/
CyberPiranhaPlant
ECE 3882 Plant Project
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