File content as of revision 3:c9e09e968552:

#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;
float TRspeed = .35;
float TLspeed = .35;

int Instr = 0;
int Rtot=0;
int Ltot=0;
char c;
int targets = 0;
float 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() {
    c = pc.getc();
  //  printf("%c",pic);
  //  pc.printf("%c",pic);
  //pc.printf("Hello world2 \n \r");
    led = !led;
    if (c == '0') { //Stop
        Instr = 0;   
    } else if (c == '4') { //reverse
        Instr = 1;
    } else if (c == '2') { //Left
        Instr = 2;
    } else if (c == '3') { //Right
        Instr = 3;
    } else if (c == '1') { //forward
        Instr = 4;
    } else if (c == 'A') { //Rotate + take pic
        Instr = 5;
    } else if (c == 'D') { //move foward set amount
    ;}
}
 
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) { //move backwards (forward, we turned our bot around)
            TError = Ltot - Rtot;
            TError = (float) TError +.05;
           Rspeed = abs(Rspeed) + (float)TError /100;
           Lspeed = abs(Lspeed);
           if(Rspeed < 0)
           RMotor.speed(0);
           if(Lspeed < 0)
           LMotor.speed(0); //sets speed to 0 because motor cannot go from negative to positive without hitting 0
           
        } else if (Instr == 2) {        //turn Left 
             TError = Ltot - Rtot;
            if(TError > 0) TRspeed = abs(TRspeed)+.01;
            else if (TError == 0) TRspeed = TRspeed;
            else TRspeed = abs(TRspeed) - .01;
           // Rspeed = Rspeed + .0008;
            LMotor.speed(0);
            RMotor.speed(0);
            TLspeed = -abs(TLspeed); //Lwheel must move in reverse for left turn
            TRspeed = abs(TRspeed); //Rwheel must move forward for left turn
            
        } else if (Instr == 3) {        // turn right
               TError = Ltot - Rtot;
            if(TError > 0) TRspeed = abs(TRspeed)+.01;
            else if (TError == 0) TRspeed = TRspeed;
            else TRspeed = abs(TRspeed) - .01;
           // Rspeed = Rspeed + .0008;
                LMotor.speed(0);
                RMotor.speed(0);
            TRspeed = -abs(TRspeed); 
            TLspeed = abs(TLspeed);
        } else if (Instr == 4) {        // go forward (reverse, we turned our bot around)
                TError = Ltot - Rtot; 
                Rspeed = abs(Rspeed) + (float) TError / 100;
                Rspeed = -abs(Rspeed);
                Lspeed = -abs(Lspeed);
                LMotor.speed(0);
                RMotor.speed(0);
            }
        else if (Instr == 5) { //Rotate + Pic
        

        LMotor.speed(0);
        RMotor.speed(0);
        wait(.2);
        while(Rtot < 50 | Ltot < 50){
        LMotor.speed(.4);
        RMotor.speed(-.4);}
        LMotor.speed(0);
        RMotor.speed(0);
        Rtot = 0;
        Ltot = 0;
        wait(1);
        pc.printf("P\n"); //commands pi to take pic
        Instr = 0; //turns status to stop after search
        }
        else if (Instr == 6) { //code for nudging forward
          ; } 
 
       
if (Rtot >10 || Ltot > 10){
                Rtot = 0;
                Ltot = 0;}
                
if (Instr == 2 | Instr == 3){
    LMotor.speed(TLspeed);
    RMotor.speed(TRspeed);}
else{     
    LMotor.speed(Lspeed);
    RMotor.speed(Rspeed);}
Lcount = 0; //Restart the counters
Rcount = 0;

wait(.02);
}
}