File content as of revision 0:4d45673148c2:

#include "mbed.h" 
#include "Motor.h"
#include "PID.h"
#include <stdlib.h>
#include "rtos.h"

#define PI 3.1415926
#define threshold 27
//objects:
Motor rightMotor(p24, p28, p27); // pwm, fwd(in1), rev(in2) Motor B (right wheel)
Motor leftMotor(p22, p29, p30); // pwm, fwd(in1), rev(in2) Motor A (left wheel)
Timer timer;
Serial xbee(p13, p14);
Serial pc(USBTX,USBRX);
Mutex wheelMutex;
Mutex timermutex; 

//functions
void go_fwd(float);
void turn_R(float);
void turn_L(float);
void stop(float);
void tunePID(void);
void go_fwd_thread(void const *argument);

//globals:
int leftPrevPulses  = 0;
int leftActPulses=0; //pulses from left motor
float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per second
int rightPrevPulses = 0;
int rightActPulses=0; //pulses from right motor
float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
int turnDistance     = 0; //Number of pulses to travel.
int distanceR = 0; // in mm
int distanceL = 0; // in mm
int distanceRold = 0; // in mm
int distanceLold = 0; // in mm
bool go;
float go_time = 0.0;

class Counter {     //interrupt driven rotation counter to be used with the feedback control
    public:
    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
    }
 
    void increment() {
        _count++;
    }
 
    int read() {
        return _count;
    }
 
private:
    InterruptIn _interrupt;
    volatile int _count;
};


Counter leftPulses(p9), rightPulses (p10);
PID leftPid(0.4620, 0.1, 0.1, 0.01);  //Kc, Ti, Td
PID rightPid(0.4620, 0.1, 0.1, 0.01); //Kc, Ti, Td
//PID rightPid(0.515, 0.1, 0.0, 0.01); //Kc, Ti, Td

int main()
{
    tunePID();
    xbee.baud(19200);
    leftPid.setSetPoint(975);  //set velocity goals for PID
    rightPid.setSetPoint(990);
    wait(1);
    Thread fwdThread(go_fwd_thread);
    go = false;
    char tag;
    char turnDirectionBuf[5];
    float turnDirection; 
    char distToTravelBuf[5];
    float distToTravel;
    while(1)
    {
        tag = xbee.getc();
        
        if(tag == '*')
        {
            //pc.printf("tag=%c\r\n",tag);
            distToTravelBuf[0] = xbee.getc();
            distToTravelBuf[1] = xbee.getc();
            distToTravelBuf[2] = xbee.getc();
            distToTravelBuf[3] = xbee.getc();
            distToTravelBuf[4] = xbee.getc();
            //pc.printf("dist = %s\r\n",distToTravelBuf);
            distToTravel = atof(distToTravelBuf);
            //pc.printf("traveldist=%f\r\n",distToTravel);
            //go_fwd(distToTravel);
            timermutex.lock();
            go_time = distToTravel;
            timermutex.unlock();
            go = true;
                                    
        }
        else if(tag == 'R')
        {
            //pc.printf("tag=%c\r\n",tag);
            turnDirectionBuf[0] = xbee.getc();
            turnDirectionBuf[1] = xbee.getc();
            turnDirectionBuf[2] = xbee.getc();
            turnDirectionBuf[3] = xbee.getc();
            turnDirectionBuf[4] = xbee.getc();
            //pc.printf("turn right %s\n\r",turnDirectionBuf);
            turnDirection = atof(turnDirectionBuf);
            //pc.printf("turn right %f\n\r",turnDirection);
            stop(1);
            turn_R(turnDirection);
        }
        else if(tag == 'L')
        {
            //pc.printf("tag=%c\r\n",tag);
            turnDirectionBuf[0] = xbee.getc();
            turnDirectionBuf[1] = xbee.getc();
            turnDirectionBuf[2] = xbee.getc();
            turnDirectionBuf[3] = xbee.getc();
            turnDirectionBuf[4] = xbee.getc();
            //pc.printf("turn left %s\n\r",turnDirectionBuf);
            turnDirection = atof(turnDirectionBuf);
            //pc.printf("turn left %f\n\r",turnDirection);
            stop(1);
            turn_L(turnDirection);
        }   
    }
} 

void tunePID(void){
    leftPid.setInputLimits(0, 3000);    
    leftPid.setOutputLimits(0.0, 0.4);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 3000);
    rightPid.setOutputLimits(0.0, 0.4);
    rightPid.setMode(AUTO_MODE);
}

//void go_fwd(float time)
//{
void go_fwd_thread(void const *argument) {
    while(1){
        if(!go) {Thread::yield();}
        while (go) {
            wheelMutex.lock();

    timer.start();
    while(timer.read_ms() < go_time){
    leftActPulses=leftPulses.read();
    leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
    leftPrevPulses = leftActPulses;
    rightActPulses=rightPulses.read();
    rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
    rightPrevPulses = rightActPulses;    
    leftPid.setProcessValue(fabs(leftVelocity));
    leftMotor.speed(leftPid.compute());
    rightPid.setProcessValue(fabs(rightVelocity));
    rightMotor.speed(rightPid.compute());
    }
    timer.stop();
    timer.reset();
    go = false;
    timermutex.lock();
    go_time = 0.0;
    timermutex.unlock();
    wheelMutex.unlock();
    }
 
  }
}


void stop(float t)
{
    wheelMutex.lock();
    
    leftMotor.speed(0.0);    
    rightMotor.speed(0.0);
    //wait(t);
    wheelMutex.unlock();
}




void turn_R(float deg)
{
    wheelMutex.lock();
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    turnDistance=rightActPulses+(int)deg;     
    while (rightActPulses<turnDistance) { //turn approximately half a revolution
    leftMotor.speed(0.35);  //rotate to the right
    rightMotor.speed(-0.35);  //open loop, because the PID class can't handle negative values
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    wait(0.005);
    }
    wheelMutex.unlock();
    stop(1);
}
    
void turn_L(float deg)
{
    wheelMutex.lock();
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    turnDistance=leftActPulses+(int)deg;     
    while (leftActPulses<turnDistance) { //turn approximately half a revolution
    leftMotor.speed(-0.35);  //rotate to the right
    rightMotor.speed(0.35);  //open loop, because the PID class can't handle negative values
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    wait(0.005);
    }
    wheelMutex.unlock();
    stop(1);
}