main.cpp

00001 #include "mbed.h"
00002 #include "pins.h"
00003 #include "Servo.h"
00004  
00005 Serial pc(USBTX, USBRX);
00006 AnalogIn photocell1(p18);
00007 AnalogIn photocell2(p17);
00008 AnalogIn photocell3(p20);
00009 AnalogIn photocell4(p19);
00010 
00011 float s1;
00012 float s2;
00013 float s3;
00014 float s4;
00015 
00016 void moveUp () {
00017     if (servoControl < 1.0)
00018         servoControl = servoControl + .01;
00019 }
00020 
00021 void moveDown () {
00022     if (servoControl > 0.0)
00023         servoControl = servoControl - .01;
00024 }
00025 
00026 void moveLeft () {
00027     if (driverDir == 1)
00028         driverDir = 0;
00029     for (int i = 0; i < 50; i++) {
00030         driverStep = !driverStep;
00031         wait(0.0005);
00032         driverStep = !driverStep;
00033         wait(0.0005);
00034     }
00035 }
00036 
00037 void moveRight () {
00038     if (driverDir == 0)
00039         driverDir = 1;
00040     for (int i = 0; i < 50; i++) {
00041         driverStep = !driverStep;
00042         wait(0.0005);
00043         driverStep = !driverStep;
00044         wait(0.0005);
00045     }
00046 }
00047 
00048 void readSensors () {
00049     s1 = photocell1;
00050     s2 = photocell2;
00051     s3 = photocell3;
00052     s4 = photocell4;
00053 }
00054 
00055 int main () {
00056     while(1) {
00057         readSensors ();
00058         if (s1 - s2 > .05 && s3 - s4 > .05) {
00059             moveUp ();
00060             moveLeft ();
00061         } else if (s2 - s1 > .05 && s3 - s4 > .05) {
00062             moveUp ();
00063             moveRight ();
00064         } else if (s2 - s1 > .05 && s4 - s3 > .05) {
00065             moveDown ();
00066             moveRight ();
00067         } else if (s1 - s2 > .05 && s4 - s3 > .05) {
00068             moveDown ();
00069             moveLeft ();
00070         } else if (s1 - s2 > .05) {
00071             moveLeft ();
00072         } else if (s2 - s1 > .05) {
00073             moveRight ();
00074         } else if (s4 - s3 > .05) {
00075             moveDown ();
00076         } else if (s3 - s4 > .05) {
00077             moveUp ();
00078         }
00079         wait(.05);
00080     }
00081 }