Selen Krgo
/
CyberPiranhaPlant
ECE 3882 Plant Project
main.cpp@0:423866e831fb, 2019-11-14 (annotated)
- Committer:
- skrgo3
- Date:
- Thu Nov 14 05:14:10 2019 +0000
- Revision:
- 0:423866e831fb
Final1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
skrgo3 | 0:423866e831fb | 1 | #include "mbed.h" |
skrgo3 | 0:423866e831fb | 2 | #include "pins.h" |
skrgo3 | 0:423866e831fb | 3 | #include "Servo.h" |
skrgo3 | 0:423866e831fb | 4 | |
skrgo3 | 0:423866e831fb | 5 | Serial pc(USBTX, USBRX); |
skrgo3 | 0:423866e831fb | 6 | AnalogIn photocell1(p18); |
skrgo3 | 0:423866e831fb | 7 | AnalogIn photocell2(p17); |
skrgo3 | 0:423866e831fb | 8 | AnalogIn photocell3(p20); |
skrgo3 | 0:423866e831fb | 9 | AnalogIn photocell4(p19); |
skrgo3 | 0:423866e831fb | 10 | |
skrgo3 | 0:423866e831fb | 11 | float s1; |
skrgo3 | 0:423866e831fb | 12 | float s2; |
skrgo3 | 0:423866e831fb | 13 | float s3; |
skrgo3 | 0:423866e831fb | 14 | float s4; |
skrgo3 | 0:423866e831fb | 15 | |
skrgo3 | 0:423866e831fb | 16 | void moveUp () { |
skrgo3 | 0:423866e831fb | 17 | if (servoControl < 1.0) |
skrgo3 | 0:423866e831fb | 18 | servoControl = servoControl + .01; |
skrgo3 | 0:423866e831fb | 19 | } |
skrgo3 | 0:423866e831fb | 20 | |
skrgo3 | 0:423866e831fb | 21 | void moveDown () { |
skrgo3 | 0:423866e831fb | 22 | if (servoControl > 0.0) |
skrgo3 | 0:423866e831fb | 23 | servoControl = servoControl - .01; |
skrgo3 | 0:423866e831fb | 24 | } |
skrgo3 | 0:423866e831fb | 25 | |
skrgo3 | 0:423866e831fb | 26 | void moveLeft () { |
skrgo3 | 0:423866e831fb | 27 | if (driverDir == 1) |
skrgo3 | 0:423866e831fb | 28 | driverDir = 0; |
skrgo3 | 0:423866e831fb | 29 | for (int i = 0; i < 50; i++) { |
skrgo3 | 0:423866e831fb | 30 | driverStep = !driverStep; |
skrgo3 | 0:423866e831fb | 31 | wait(0.0005); |
skrgo3 | 0:423866e831fb | 32 | driverStep = !driverStep; |
skrgo3 | 0:423866e831fb | 33 | wait(0.0005); |
skrgo3 | 0:423866e831fb | 34 | } |
skrgo3 | 0:423866e831fb | 35 | } |
skrgo3 | 0:423866e831fb | 36 | |
skrgo3 | 0:423866e831fb | 37 | void moveRight () { |
skrgo3 | 0:423866e831fb | 38 | if (driverDir == 0) |
skrgo3 | 0:423866e831fb | 39 | driverDir = 1; |
skrgo3 | 0:423866e831fb | 40 | for (int i = 0; i < 50; i++) { |
skrgo3 | 0:423866e831fb | 41 | driverStep = !driverStep; |
skrgo3 | 0:423866e831fb | 42 | wait(0.0005); |
skrgo3 | 0:423866e831fb | 43 | driverStep = !driverStep; |
skrgo3 | 0:423866e831fb | 44 | wait(0.0005); |
skrgo3 | 0:423866e831fb | 45 | } |
skrgo3 | 0:423866e831fb | 46 | } |
skrgo3 | 0:423866e831fb | 47 | |
skrgo3 | 0:423866e831fb | 48 | void readSensors () { |
skrgo3 | 0:423866e831fb | 49 | s1 = photocell1; |
skrgo3 | 0:423866e831fb | 50 | s2 = photocell2; |
skrgo3 | 0:423866e831fb | 51 | s3 = photocell3; |
skrgo3 | 0:423866e831fb | 52 | s4 = photocell4; |
skrgo3 | 0:423866e831fb | 53 | } |
skrgo3 | 0:423866e831fb | 54 | |
skrgo3 | 0:423866e831fb | 55 | int main () { |
skrgo3 | 0:423866e831fb | 56 | while(1) { |
skrgo3 | 0:423866e831fb | 57 | readSensors (); |
skrgo3 | 0:423866e831fb | 58 | if (s1 - s2 > .05 && s3 - s4 > .05) { |
skrgo3 | 0:423866e831fb | 59 | moveUp (); |
skrgo3 | 0:423866e831fb | 60 | moveLeft (); |
skrgo3 | 0:423866e831fb | 61 | } else if (s2 - s1 > .05 && s3 - s4 > .05) { |
skrgo3 | 0:423866e831fb | 62 | moveUp (); |
skrgo3 | 0:423866e831fb | 63 | moveRight (); |
skrgo3 | 0:423866e831fb | 64 | } else if (s2 - s1 > .05 && s4 - s3 > .05) { |
skrgo3 | 0:423866e831fb | 65 | moveDown (); |
skrgo3 | 0:423866e831fb | 66 | moveRight (); |
skrgo3 | 0:423866e831fb | 67 | } else if (s1 - s2 > .05 && s4 - s3 > .05) { |
skrgo3 | 0:423866e831fb | 68 | moveDown (); |
skrgo3 | 0:423866e831fb | 69 | moveLeft (); |
skrgo3 | 0:423866e831fb | 70 | } else if (s1 - s2 > .05) { |
skrgo3 | 0:423866e831fb | 71 | moveLeft (); |
skrgo3 | 0:423866e831fb | 72 | } else if (s2 - s1 > .05) { |
skrgo3 | 0:423866e831fb | 73 | moveRight (); |
skrgo3 | 0:423866e831fb | 74 | } else if (s4 - s3 > .05) { |
skrgo3 | 0:423866e831fb | 75 | moveDown (); |
skrgo3 | 0:423866e831fb | 76 | } else if (s3 - s4 > .05) { |
skrgo3 | 0:423866e831fb | 77 | moveUp (); |
skrgo3 | 0:423866e831fb | 78 | } |
skrgo3 | 0:423866e831fb | 79 | wait(.05); |
skrgo3 | 0:423866e831fb | 80 | } |
skrgo3 | 0:423866e831fb | 81 | } |