main.cpp

#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);
    }
}