File content as of revision 0:fd289b2e6b74:

#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 = -1;
      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 =  0;
              break;
          case 2:
              speed_limit = 10;
              break;
          case 3:
              speed_limit = 60;
              break;
          default:
              speed_limit =  0;
              break;
           
    }
    TheRobot.SetSelectMotorVelocity(0, 0);
    wait_ms(5);
    TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);
    Thread::wait(500);
   }
}

int main() 
{
    //Start thread
    Thread Poll_RFID(SetSpeed);
    
    
    char y1, y2, y3;
    while(1) 
    {
        y3=device.getc();
        y2=y3 & 0x0F;
        y1=(y3 & 0xF0)>>4;

        if(y1==0x0C) p1xdir=-1;
        else if (y1==0x03) p1xdir=1;
        else p1xdir=0;

        if(y2==0x0C) p2xdir=-1;
        else if (y2==0x03) p2xdir=1;
        else p2xdir=0;
    
       //TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);

        /*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=31-JS;) * 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;*/
    }
}