File content as of revision 0:354a8831107d:

#include "mbed.h"
#include "rtos.h"
#include "Robot.h"

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//RFID data from other MBed
DigitalIn rfIdBit1(p21);
DigitalIn rfIdBit0(p22);
InterruptIn rfIdInterrupt(p23);

//Robot code
Serial device(p9, p10);
Serial rob(p13, p14);
Robot TheRobot(&rob, &led1);



//Debug
Serial pc(USBTX, USBRX); // tx, rx
Mutex pc_mutex;

//Global Variables
float biasRate = 0.1;
float updateRate = 2.0;
float speed_modifier=1;
float current_speed=0;
int p1xdir=1,p2xdir=-1; //Direction
int speed_limit=0;

void limit_detect(void const *args)
{
    while(1)
    {
        if(current_speed<speed_limit) speed_modifier=1.0;
        else speed_modifier = speed_limit / current_speed;
        Thread::wait(20);
    }
}

void SetSpeed(void const *args)
{
   while(1)
   {
      //Set bits
      int  RfidFlag = 0;
      int V1 = rfIdBit1? 1:0;
      int V0 = rfIdBit0? 1:0;

      //Set flag
      RfidFlag = V1 <<1;
      RfidFlag |= V0;
      if(RfidFlag == 0) continue;
  
      //Debug
      //pc_mutex.lock();
      pc.printf("%d , %d    ___ %d\n",rfIdBit1? 1:0, rfIdBit0?1:0, RfidFlag);
      //pc_mutex.unlock();
   
      //Set speed limit based on RfidFlag
      switch(RfidFlag)
      {
          case 0:
              speed_limit =  0;
              break;
          case 1:
              speed_limit =  10;
              p1xdir = 1;
              p2xdir = -1;
              break;
          case 2:
              speed_limit = 10;
              p1xdir = -1;
              p2xdir = 1;
              break;
          case 3:
              speed_limit = 60;
              p1xdir = 1;
              p2xdir = 1;
              break;
          default:
              break;
           
    }
    TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);

    Thread::wait(500);
   }
}

int main() 
{
    //Start thread
    Thread limit_change(limit_detect);
    //Thread Poll_IMU(CalcSpeed);
    Thread Poll_RFID(SetSpeed);

    int JS1x=0, JS2x=0; //Translated binary values from joystick
    int p1x=0,p2x=0; //PWM power
    char txchar=0x00;
    char txflag=0x00;
    device.baud(600);
    //TheRobot.SetMotorVelocity(30);
    while(1) 
    {
        txchar=device.getc();
        txflag=txchar & 0xC0;
        switch(txflag)
        {
            case 0x00:
                JS1x=(int)txchar & 0x3F;
                break;
            case 0xC0:
                JS2x=(int)txchar & 0x3F;
                break;
            default:
                break;
        }
        
    
    
        //Break info into direction and magnitude
        //if(JS1x>31){p1xdir=-1; p1x=JS1x-32;}
        //else {p1xdir=1; p1x=31-JS1x;}
        //if(JS2x>31){p2xdir=1; p2x=JS2x-32;}
        //else {p2xdir=-1; p2x=31-JS2x;}
        
        
        current_speed=sqrt((float)p1x*(float)p1x+(float)p2x*(float)p2x);
        
        
        //PMW output based on modifier
        //pmwL = p1xdir * (speed_modifier * p1x) * 2;
        //pmwR = p2xdir * (speed_modifier * p2x) * 2;
        
        //TheRobot.SetSelectMotorVelocity(pmwL, pmwR);
        //TheRobot.WaitForAck();
        //TheRobot.SetSelectMotorVelocity(2, 60);
        //wait(5);
        //TheRobot.SetSelectMotorVelocity(1, 15);
        //wait(5);
        //TheRobot.SetSelectMotorVelocity(2, pmwR);
        
        //DEBUG
        //if(JS1x>16) led1=1;
        //else led1=0;
        /*if(JS1x>32) led2=1;
        else led2=0;
        if(JS2x>16) led3=1;
        else led3=0;
        if(JS2x>32) led4=1;
        else led4=0;*/
    }
}