File content as of revision 2:9a3221b22855:

#include "mbed.h"
#include "Motor.h"
Serial pc(USBTX, USBRX);

 
InterruptIn Rencoder(p30);
InterruptIn Lencoder(p29);

DigitalOut led(LED1);
DigitalOut flash(LED4);

Motor LMotor(p22, p23, p24); // pwmA, fwd, rev,  
Motor RMotor(p21, p26, p25); // pwmB, fwd, rev, 

int Rcount = 0;
int Lcount = 0;
int Error = 0;
float Rspeed = .4;
float Lspeed = .4;
int Instr = 5;
int Rtot=0;
int Ltot=0;
char c;
int targets = 0;
int TError = 0;
char V;
char M;
int current = 1;

Ticker Sampler;

 
void RSample() {
    Rtot++;
// pc.printf("Rcount: %d\n\r",Rcount);  
}

void LSample() {
    Ltot++;
}

void callback() {
    // Note: you need to actually read from the serial to clear the RX interrupt
    //printf("%c\n",pc.getc());
    //printf("Hello world");
    c = pc.getc();
    led = !led;
    if (c == '0') { //Stop
        Instr = 0;   
    } else if (c == '1') { //Forward
        Instr = 1;
    }else if (c == '2') { //Left
        Instr = 2;
    } else if (c == '3') { //Right
        Instr = 3;
    } else if (c == '4') { //REverse
        Instr = 4;
    }
}
 
int main() {
    pc.attach(&callback);
    
    Rencoder.mode(PullUp);
    Rencoder.rise(&RSample);
    Lencoder.mode(PullUp);
    Lencoder.rise(&LSample);    
    
    while(1) { 
    
       //printf("Instr = %d\n", Instr);
       
       if (Instr == 0) {
            Rspeed = 0;
            Lspeed = 0;
        } else if (Instr == 1) {
            TError = Ltot - Rtot;
            if(TError > 0) Rspeed = Rspeed+.01;
            else if (TError == 0) Rspeed = Rspeed;
            else Rspeed = Rspeed - .01;
            Rspeed = Rspeed + .0008;
        } else if (Instr == 2) {
             TError = Ltot - Rtot;
            if(TError > 0) Rspeed = Rspeed+.01;
            else if (TError == 0) Rspeed = Rspeed;
            else Rspeed = Rspeed - .01;
            Rspeed = Rspeed + .0008;
            LMotor.speed(0);
            RMotor.speed(0);
            if (Lspeed >0){
                Lspeed = -Lspeed;
             }
            
            
        } else if (Instr == 3) {
               TError = Ltot - Rtot;
            if(TError > 0) Rspeed = abs(Rspeed)+.01;
            else if (TError == 0) Rspeed = Rspeed;
            else Rspeed = abs(Rspeed) - .01;
            Rspeed = Rspeed + .0008;
                LMotor.speed(0);
                RMotor.speed(0);
                if (Rspeed >0){
                    Rspeed = -Rspeed;
                }
            
        } else if (Instr == 4) {
              TError = Ltot - Rtot;
            if(TError > 0) Rspeed = abs(Rspeed)+.0075;
            else if (TError == 0) Rspeed = Rspeed;
            else Rspeed = abs(Rspeed) - .005;
            Rspeed = Rspeed + .00075;
                if (Rspeed > 0){
                Rspeed = -Rspeed;
                }
                if (Lspeed > 0){
                Lspeed = -Lspeed;
                }
                LMotor.speed(0);
                RMotor.speed(0);
        }
        else if (Instr == 5) {
        
        for (current < targets+1){
        LMotor.speed(0);
        RMotor.speed(0);
        wait(.2);
        while(Rtot < 55 | Ltot < 55){
        LMotor.speed(.3);
        RMotor.speed(-.3);}
        LMotor.speed(0);
        RMotor.speed(0);
        pc.printf( "RTot: %d\n\r",Rtot);
        pc.printf("LTot: %d\n\r",Ltot); 
        Rtot = 0;
        Ltot = 0;
        wait(15); // TAKE PICTURE
        while (V == 0){ // V = whether picture is valid or not
        wait(10);// Continue to take pictures)
        }
            if (M == 1){  // if match found drives forward
            LMotor.speed(Lspeed);
            RMotor.speed(Rspeed);
            wait(1);
            LMotor.speed(0);
            RMotor.speed(0);
            wait(1);//snap tons of photos
            LMotor.speed(-Lspeed);
            RMotor.speed(-Rspeed);
            wait(1);
            current = targets+1; //if a match is found, stops the search
            }
            else current++;
        }
        Instr = 0; //turns status to stop after search
        /*
    switch (Instr){
    
    // stop instruction
        case 0:
        Rspeed = 0;
        Lspeed = 0;
        break;
    // forward instruction
        case 1 :  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
        Error =  Lcount - Rcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 10);
        break;
        
    // Left turn Instruction
        case 2:   
        Error =  Lcount - Rcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 10);
        LMotor.speed(0);
        RMotor.speed(0);
        if (Lspeed >0){
        Lspeed = -Lspeed;
        }
        break;
        
    // Right turn Instruction
        case 3:   
        Error =  Lcount - Rcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 10);
        LMotor.speed(0);
        RMotor.speed(0);
        if (Rspeed >0){
        Rspeed = -Rspeed;}
        break;
        
    // reverse instruction
        case 4:  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
        Error =  Rcount - Lcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 10);
        if (Rspeed > 0){
        Rspeed = -Rspeed;
        }
        if (Lspeed > 0){
        Lspeed = -Lspeed;
        }
        LMotor.speed(0);
        RMotor.speed(0);
        break;
       }
       
       */
if (Rtot >10 || Ltot > 10){
                Rtot = 0;
                Ltot = 0;}
LMotor.speed(Lspeed);
RMotor.speed(Rspeed);
Lcount = 0; //Restart the counters
Rcount = 0;

wait(.02);
}
}