James Nagendran
/
4180_final_receiver
Receiver code for SLVM
Diff: main.cpp
- Revision:
- 0:fd289b2e6b74
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Dec 09 01:15:37 2014 +0000 @@ -0,0 +1,160 @@ +#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;*/ + } +} +