James Nagendran / Mbed 2 deprecated 4180_final_receiver

Receiver code for SLVM

Dependencies:   mbed-rtos mbed

main.cpp@0:fd289b2e6b74, 2014-12-09 (annotated)

Committer:
jnagendran3
Date:
Tue Dec 09 01:15:37 2014 +0000
Revision:
0:fd289b2e6b74
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jnagendran3 0:fd289b2e6b74 1 #include "mbed.h"
jnagendran3 0:fd289b2e6b74 2 #include "rtos.h"
jnagendran3 0:fd289b2e6b74 3 #include "Robot.h"
jnagendran3 0:fd289b2e6b74 4
jnagendran3 0:fd289b2e6b74 5 DigitalOut led1(LED1);
jnagendran3 0:fd289b2e6b74 6 DigitalOut led2(LED2);
jnagendran3 0:fd289b2e6b74 7 DigitalOut led3(LED3);
jnagendran3 0:fd289b2e6b74 8 DigitalOut led4(LED4);
jnagendran3 0:fd289b2e6b74 9
jnagendran3 0:fd289b2e6b74 10 //RFID data from other MBed
jnagendran3 0:fd289b2e6b74 11 DigitalIn rfIdBit1(p21);
jnagendran3 0:fd289b2e6b74 12 DigitalIn rfIdBit0(p22);
jnagendran3 0:fd289b2e6b74 13 InterruptIn rfIdInterrupt(p23);
jnagendran3 0:fd289b2e6b74 14
jnagendran3 0:fd289b2e6b74 15 //Robot code
jnagendran3 0:fd289b2e6b74 16 Serial device(p9, p10);
jnagendran3 0:fd289b2e6b74 17 Serial rob(p13, p14);
jnagendran3 0:fd289b2e6b74 18 Robot TheRobot(&rob, &led1);
jnagendran3 0:fd289b2e6b74 19
jnagendran3 0:fd289b2e6b74 20
jnagendran3 0:fd289b2e6b74 21 //Debug
jnagendran3 0:fd289b2e6b74 22 Serial pc(USBTX, USBRX); // tx, rx
jnagendran3 0:fd289b2e6b74 23 Mutex pc_mutex;
jnagendran3 0:fd289b2e6b74 24
jnagendran3 0:fd289b2e6b74 25 //Global Variables
jnagendran3 0:fd289b2e6b74 26 float biasRate = 0.1;
jnagendran3 0:fd289b2e6b74 27 float updateRate = 2.0;
jnagendran3 0:fd289b2e6b74 28 float speed_modifier=1;
jnagendran3 0:fd289b2e6b74 29 float current_speed=0;
jnagendran3 0:fd289b2e6b74 30 int p1xdir=1,p2xdir=-1; //Direction
jnagendran3 0:fd289b2e6b74 31 int speed_limit=0;
jnagendran3 0:fd289b2e6b74 32
jnagendran3 0:fd289b2e6b74 33 void limit_detect(void const *args)
jnagendran3 0:fd289b2e6b74 34 {
jnagendran3 0:fd289b2e6b74 35 while(1)
jnagendran3 0:fd289b2e6b74 36 {
jnagendran3 0:fd289b2e6b74 37 if(current_speed<speed_limit) speed_modifier=1.0;
jnagendran3 0:fd289b2e6b74 38 else speed_modifier = speed_limit / current_speed;
jnagendran3 0:fd289b2e6b74 39 Thread::wait(20);
jnagendran3 0:fd289b2e6b74 40 }
jnagendran3 0:fd289b2e6b74 41 }
jnagendran3 0:fd289b2e6b74 42
jnagendran3 0:fd289b2e6b74 43 void SetSpeed(void const *args)
jnagendran3 0:fd289b2e6b74 44 {
jnagendran3 0:fd289b2e6b74 45 while(1)
jnagendran3 0:fd289b2e6b74 46 {
jnagendran3 0:fd289b2e6b74 47 //Set bits
jnagendran3 0:fd289b2e6b74 48 int RfidFlag = -1;
jnagendran3 0:fd289b2e6b74 49 int V1 = rfIdBit1? 1:0;
jnagendran3 0:fd289b2e6b74 50 int V0 = rfIdBit0? 1:0;
jnagendran3 0:fd289b2e6b74 51
jnagendran3 0:fd289b2e6b74 52 //Set flag
jnagendran3 0:fd289b2e6b74 53 RfidFlag = V1 <<1;
jnagendran3 0:fd289b2e6b74 54 RfidFlag |= V0;
jnagendran3 0:fd289b2e6b74 55 if(RfidFlag == 0) continue;
jnagendran3 0:fd289b2e6b74 56
jnagendran3 0:fd289b2e6b74 57 //Debug
jnagendran3 0:fd289b2e6b74 58 //pc_mutex.lock();
jnagendran3 0:fd289b2e6b74 59 //pc.printf("%d , %d ___ %d\n",rfIdBit1? 1:0, rfIdBit0?1:0, RfidFlag);
jnagendran3 0:fd289b2e6b74 60 //pc_mutex.unlock();
jnagendran3 0:fd289b2e6b74 61
jnagendran3 0:fd289b2e6b74 62 //Set speed limit based on RfidFlag
jnagendran3 0:fd289b2e6b74 63 switch(RfidFlag)
jnagendran3 0:fd289b2e6b74 64 {
jnagendran3 0:fd289b2e6b74 65 case 0:
jnagendran3 0:fd289b2e6b74 66 speed_limit = 0;
jnagendran3 0:fd289b2e6b74 67 break;
jnagendran3 0:fd289b2e6b74 68 case 1:
jnagendran3 0:fd289b2e6b74 69 speed_limit = 0;
jnagendran3 0:fd289b2e6b74 70 break;
jnagendran3 0:fd289b2e6b74 71 case 2:
jnagendran3 0:fd289b2e6b74 72 speed_limit = 10;
jnagendran3 0:fd289b2e6b74 73 break;
jnagendran3 0:fd289b2e6b74 74 case 3:
jnagendran3 0:fd289b2e6b74 75 speed_limit = 60;
jnagendran3 0:fd289b2e6b74 76 break;
jnagendran3 0:fd289b2e6b74 77 default:
jnagendran3 0:fd289b2e6b74 78 speed_limit = 0;
jnagendran3 0:fd289b2e6b74 79 break;
jnagendran3 0:fd289b2e6b74 80
jnagendran3 0:fd289b2e6b74 81 }
jnagendran3 0:fd289b2e6b74 82 TheRobot.SetSelectMotorVelocity(0, 0);
jnagendran3 0:fd289b2e6b74 83 wait_ms(5);
jnagendran3 0:fd289b2e6b74 84 TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);
jnagendran3 0:fd289b2e6b74 85 Thread::wait(500);
jnagendran3 0:fd289b2e6b74 86 }
jnagendran3 0:fd289b2e6b74 87 }
jnagendran3 0:fd289b2e6b74 88
jnagendran3 0:fd289b2e6b74 89 int main()
jnagendran3 0:fd289b2e6b74 90 {
jnagendran3 0:fd289b2e6b74 91 //Start thread
jnagendran3 0:fd289b2e6b74 92 Thread Poll_RFID(SetSpeed);
jnagendran3 0:fd289b2e6b74 93
jnagendran3 0:fd289b2e6b74 94
jnagendran3 0:fd289b2e6b74 95 char y1, y2, y3;
jnagendran3 0:fd289b2e6b74 96 while(1)
jnagendran3 0:fd289b2e6b74 97 {
jnagendran3 0:fd289b2e6b74 98 y3=device.getc();
jnagendran3 0:fd289b2e6b74 99 y2=y3 & 0x0F;
jnagendran3 0:fd289b2e6b74 100 y1=(y3 & 0xF0)>>4;
jnagendran3 0:fd289b2e6b74 101
jnagendran3 0:fd289b2e6b74 102 if(y1==0x0C) p1xdir=-1;
jnagendran3 0:fd289b2e6b74 103 else if (y1==0x03) p1xdir=1;
jnagendran3 0:fd289b2e6b74 104 else p1xdir=0;
jnagendran3 0:fd289b2e6b74 105
jnagendran3 0:fd289b2e6b74 106 if(y2==0x0C) p2xdir=-1;
jnagendran3 0:fd289b2e6b74 107 else if (y2==0x03) p2xdir=1;
jnagendran3 0:fd289b2e6b74 108 else p2xdir=0;
jnagendran3 0:fd289b2e6b74 109
jnagendran3 0:fd289b2e6b74 110 //TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);
jnagendran3 0:fd289b2e6b74 111
jnagendran3 0:fd289b2e6b74 112 /*txflag=txchar & 0xC0;
jnagendran3 0:fd289b2e6b74 113 switch(txflag)
jnagendran3 0:fd289b2e6b74 114 {
jnagendran3 0:fd289b2e6b74 115 case 0x00:
jnagendran3 0:fd289b2e6b74 116 JS1x=(int)txchar & 0x3F;
jnagendran3 0:fd289b2e6b74 117 break;
jnagendran3 0:fd289b2e6b74 118 case 0xC0:
jnagendran3 0:fd289b2e6b74 119 JS2x=(int)txchar & 0x3F;
jnagendran3 0:fd289b2e6b74 120 break;
jnagendran3 0:fd289b2e6b74 121 default:
jnagendran3 0:fd289b2e6b74 122 break;
jnagendran3 0:fd289b2e6b74 123 }*/
jnagendran3 0:fd289b2e6b74 124
jnagendran3 0:fd289b2e6b74 125
jnagendran3 0:fd289b2e6b74 126
jnagendran3 0:fd289b2e6b74 127 //Break info into direction and magnitude
jnagendran3 0:fd289b2e6b74 128 /*if(JS1x>31){p1xdir=-1; p1x=JS1x-32;}
jnagendran3 0:fd289b2e6b74 129 else {p1xdir=1; p1x=31-JS1x;}
jnagendran3 0:fd289b2e6b74 130 if(JS2x>31){p2xdir=1; p2x=JS2x-32;}
jnagendran3 0:fd289b2e6b74 131 else {p2xdir=-1; p2x=31-JS2x;}
jnagendran3 0:fd289b2e6b74 132 */
jnagendran3 0:fd289b2e6b74 133
jnagendran3 0:fd289b2e6b74 134 //current_speed=sqrt((float)p1x*(float)p1x+(float)p2x*(float)p2x);
jnagendran3 0:fd289b2e6b74 135
jnagendran3 0:fd289b2e6b74 136
jnagendran3 0:fd289b2e6b74 137 //PMW output based on modifier
jnagendran3 0:fd289b2e6b74 138 //pmwL = p1xdir * (speed_modifier * p1x=31-JS;) * 2;
jnagendran3 0:fd289b2e6b74 139 //pmwR = p2xdir * (speed_modifier * p2x) * 2;
jnagendran3 0:fd289b2e6b74 140
jnagendran3 0:fd289b2e6b74 141 //TheRobot.SetSelectMotorVelocity(pmwL, pmwR);
jnagendran3 0:fd289b2e6b74 142 //TheRobot.WaitForAck();
jnagendran3 0:fd289b2e6b74 143 //TheRobot.SetSelectMotorVelocity(2, 60);
jnagendran3 0:fd289b2e6b74 144 //wait(5);
jnagendran3 0:fd289b2e6b74 145 //TheRobot.SetSelectMotorVelocity(1, 15);
jnagendran3 0:fd289b2e6b74 146 //wait(5);
jnagendran3 0:fd289b2e6b74 147 //TheRobot.SetSelectMotorVelocity(2, pmwR);
jnagendran3 0:fd289b2e6b74 148
jnagendran3 0:fd289b2e6b74 149 //DEBUG
jnagendran3 0:fd289b2e6b74 150 //if(JS1x>16) led1=1;
jnagendran3 0:fd289b2e6b74 151 //else led1=0;
jnagendran3 0:fd289b2e6b74 152 /*if(JS1x>32) led2=1;
jnagendran3 0:fd289b2e6b74 153 else led2=0;
jnagendran3 0:fd289b2e6b74 154 if(JS2x>16) led3=1;
jnagendran3 0:fd289b2e6b74 155 else led3=0;
jnagendran3 0:fd289b2e6b74 156 if(JS2x>32) led4=1;
jnagendran3 0:fd289b2e6b74 157 else led4=0;*/
jnagendran3 0:fd289b2e6b74 158 }
jnagendran3 0:fd289b2e6b74 159 }
jnagendran3 0:fd289b2e6b74 160