Guillaume Mourozeau / Mbed 2 deprecated Lab4_BluetoothRobot

Bluetooth-controlled Robot with RFID switching mode

Dependencies:   Motor mbed-rtos mbed

main.cpp@0:64f17142aa89, 2016-03-13 (annotated)

Committer:
GuillaumeMrzo
Date:
Sun Mar 13 03:51:00 2016 +0000
Revision:
0:64f17142aa89
Bluetooth-controlled Robot with RFID switching mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GuillaumeMrzo 0:64f17142aa89 1 #include "mbed.h"
GuillaumeMrzo 0:64f17142aa89 2 #include "Motor.h"
GuillaumeMrzo 0:64f17142aa89 3 #include "rtos.h"
GuillaumeMrzo 0:64f17142aa89 4 #include "stdio.h"
GuillaumeMrzo 0:64f17142aa89 5 #include "MFRC522.h"
GuillaumeMrzo 0:64f17142aa89 6
GuillaumeMrzo 0:64f17142aa89 7 Motor m_r(p24, p17, p18); // pwm, fwd, rev
GuillaumeMrzo 0:64f17142aa89 8 Motor m_l(p25, p20, p19); // pwm, fwd, rev
GuillaumeMrzo 0:64f17142aa89 9
GuillaumeMrzo 0:64f17142aa89 10 MFRC522 RfChip(p5, p6, p7, p8, p9);
GuillaumeMrzo 0:64f17142aa89 11
GuillaumeMrzo 0:64f17142aa89 12 AnalogIn IR_right(p15);
GuillaumeMrzo 0:64f17142aa89 13 AnalogIn IR_left(p16);
GuillaumeMrzo 0:64f17142aa89 14 DigitalOut ledL(LED1);
GuillaumeMrzo 0:64f17142aa89 15 DigitalOut led2(LED2);
GuillaumeMrzo 0:64f17142aa89 16 DigitalOut led3(LED3);
GuillaumeMrzo 0:64f17142aa89 17 DigitalOut ledR(LED4);
GuillaumeMrzo 0:64f17142aa89 18
GuillaumeMrzo 0:64f17142aa89 19 RawSerial pc(USBTX, USBRX);
GuillaumeMrzo 0:64f17142aa89 20 RawSerial blue(p28,p27);
GuillaumeMrzo 0:64f17142aa89 21 DigitalOut led1(LED1);
GuillaumeMrzo 0:64f17142aa89 22 DigitalOut led4(LED4);
GuillaumeMrzo 0:64f17142aa89 23
GuillaumeMrzo 0:64f17142aa89 24 char bnum=0;
GuillaumeMrzo 0:64f17142aa89 25 char bhit=0;
GuillaumeMrzo 0:64f17142aa89 26
GuillaumeMrzo 0:64f17142aa89 27 // Flags for the buttons
GuillaumeMrzo 0:64f17142aa89 28 int flagFW, flagRight, flagLeft, flagRev;
GuillaumeMrzo 0:64f17142aa89 29 int flag1, flag2, flag3, flag4;
GuillaumeMrzo 0:64f17142aa89 30 int flag_autom;
GuillaumeMrzo 0:64f17142aa89 31
GuillaumeMrzo 0:64f17142aa89 32 // IR detectors
GuillaumeMrzo 0:64f17142aa89 33 float ainR, ainL;
GuillaumeMrzo 0:64f17142aa89 34
GuillaumeMrzo 0:64f17142aa89 35 // Speed of the robot
GuillaumeMrzo 0:64f17142aa89 36 float s_max = 0.5;
GuillaumeMrzo 0:64f17142aa89 37 float s_min = 0.0;
GuillaumeMrzo 0:64f17142aa89 38
GuillaumeMrzo 0:64f17142aa89 39 // mutex for the flag that switches mode
GuillaumeMrzo 0:64f17142aa89 40 Mutex autom_mutex;
GuillaumeMrzo 0:64f17142aa89 41
GuillaumeMrzo 0:64f17142aa89 42 // ID of tags - We only compare the 1st element
GuillaumeMrzo 0:64f17142aa89 43 int white_tag = 115;
GuillaumeMrzo 0:64f17142aa89 44 int blue_tag = 106;
GuillaumeMrzo 0:64f17142aa89 45 int tag;
GuillaumeMrzo 0:64f17142aa89 46
GuillaumeMrzo 0:64f17142aa89 47
GuillaumeMrzo 0:64f17142aa89 48 // ------------------- Thread RFID ------------------------//
GuillaumeMrzo 0:64f17142aa89 49 void thread_RFID(void const *args){
GuillaumeMrzo 0:64f17142aa89 50 while (true) {
GuillaumeMrzo 0:64f17142aa89 51
GuillaumeMrzo 0:64f17142aa89 52 // Look for new cards
GuillaumeMrzo 0:64f17142aa89 53 if ( ! RfChip.PICC_IsNewCardPresent()){
GuillaumeMrzo 0:64f17142aa89 54 Thread::wait(200);
GuillaumeMrzo 0:64f17142aa89 55 continue;
GuillaumeMrzo 0:64f17142aa89 56 }
GuillaumeMrzo 0:64f17142aa89 57
GuillaumeMrzo 0:64f17142aa89 58 // Select one of the cards
GuillaumeMrzo 0:64f17142aa89 59 if ( ! RfChip.PICC_ReadCardSerial()){
GuillaumeMrzo 0:64f17142aa89 60 Thread::wait(200);
GuillaumeMrzo 0:64f17142aa89 61 continue;
GuillaumeMrzo 0:64f17142aa89 62 }
GuillaumeMrzo 0:64f17142aa89 63
GuillaumeMrzo 0:64f17142aa89 64 led2 = 0;
GuillaumeMrzo 0:64f17142aa89 65
GuillaumeMrzo 0:64f17142aa89 66 // Print Card UID
GuillaumeMrzo 0:64f17142aa89 67 pc.printf("Card UID: ");
GuillaumeMrzo 0:64f17142aa89 68 for (uint8_t i = 0; i < RfChip.uid.size; i++){
GuillaumeMrzo 0:64f17142aa89 69 //pc.printf(" %X02", RfChip.uid.uidByte[i]);
GuillaumeMrzo 0:64f17142aa89 70 pc.printf(" %i", RfChip.uid.uidByte[i]);
GuillaumeMrzo 0:64f17142aa89 71
GuillaumeMrzo 0:64f17142aa89 72 // Get the 1st element of the ID
GuillaumeMrzo 0:64f17142aa89 73 if(i == 0){
GuillaumeMrzo 0:64f17142aa89 74 tag = RfChip.uid.uidByte[i];
GuillaumeMrzo 0:64f17142aa89 75 }
GuillaumeMrzo 0:64f17142aa89 76 }
GuillaumeMrzo 0:64f17142aa89 77
GuillaumeMrzo 0:64f17142aa89 78 pc.printf("\n\r");
GuillaumeMrzo 0:64f17142aa89 79 pc.printf("ID = %i\n\r", tag);
GuillaumeMrzo 0:64f17142aa89 80
GuillaumeMrzo 0:64f17142aa89 81 if(tag == white_tag){
GuillaumeMrzo 0:64f17142aa89 82 autom_mutex.lock();
GuillaumeMrzo 0:64f17142aa89 83 flag_autom = 0;
GuillaumeMrzo 0:64f17142aa89 84 autom_mutex.unlock();
GuillaumeMrzo 0:64f17142aa89 85 pc.printf("Controlled mode\n\r");
GuillaumeMrzo 0:64f17142aa89 86 }
GuillaumeMrzo 0:64f17142aa89 87 else if(tag == blue_tag){
GuillaumeMrzo 0:64f17142aa89 88 autom_mutex.lock();
GuillaumeMrzo 0:64f17142aa89 89 flag_autom = 1;
GuillaumeMrzo 0:64f17142aa89 90 autom_mutex.unlock();
GuillaumeMrzo 0:64f17142aa89 91 pc.printf("Automatic mode\n\r");
GuillaumeMrzo 0:64f17142aa89 92 }
GuillaumeMrzo 0:64f17142aa89 93
GuillaumeMrzo 0:64f17142aa89 94
GuillaumeMrzo 0:64f17142aa89 95 // Print Card type
GuillaumeMrzo 0:64f17142aa89 96 uint8_t piccType = RfChip.PICC_GetType(RfChip.uid.sak);
GuillaumeMrzo 0:64f17142aa89 97 pc.printf("PICC Type: %s \n\r", RfChip.PICC_GetTypeName(piccType));
GuillaumeMrzo 0:64f17142aa89 98
GuillaumeMrzo 0:64f17142aa89 99 Thread::wait(10);
GuillaumeMrzo 0:64f17142aa89 100 }
GuillaumeMrzo 0:64f17142aa89 101 }
GuillaumeMrzo 0:64f17142aa89 102
GuillaumeMrzo 0:64f17142aa89 103
GuillaumeMrzo 0:64f17142aa89 104 // ------------------- Thread Motor ------------------------//
GuillaumeMrzo 0:64f17142aa89 105 void thread_Motor(void const *args){
GuillaumeMrzo 0:64f17142aa89 106 while (true) {
GuillaumeMrzo 0:64f17142aa89 107
GuillaumeMrzo 0:64f17142aa89 108 m_r.speed(0);
GuillaumeMrzo 0:64f17142aa89 109 m_l.speed(0);
GuillaumeMrzo 0:64f17142aa89 110
GuillaumeMrzo 0:64f17142aa89 111 ainR = IR_right;
GuillaumeMrzo 0:64f17142aa89 112 ainL = IR_left;
GuillaumeMrzo 0:64f17142aa89 113
GuillaumeMrzo 0:64f17142aa89 114 int autom;
GuillaumeMrzo 0:64f17142aa89 115 autom_mutex.lock();
GuillaumeMrzo 0:64f17142aa89 116 autom = flag_autom;
GuillaumeMrzo 0:64f17142aa89 117 autom_mutex.unlock();
GuillaumeMrzo 0:64f17142aa89 118
GuillaumeMrzo 0:64f17142aa89 119 // ------- Control mode ------------------
GuillaumeMrzo 0:64f17142aa89 120 if(autom == 0){
GuillaumeMrzo 0:64f17142aa89 121
GuillaumeMrzo 0:64f17142aa89 122 ledL = led2 = led3 = ledR = 0;
GuillaumeMrzo 0:64f17142aa89 123
GuillaumeMrzo 0:64f17142aa89 124 // Push FW button 1st
GuillaumeMrzo 0:64f17142aa89 125 while(flagFW == 1){
GuillaumeMrzo 0:64f17142aa89 126 ledL =1;
GuillaumeMrzo 0:64f17142aa89 127 led2 = led3 = ledR = 0;
GuillaumeMrzo 0:64f17142aa89 128
GuillaumeMrzo 0:64f17142aa89 129 if(flagLeft == 1){
GuillaumeMrzo 0:64f17142aa89 130 led2 = 1;
GuillaumeMrzo 0:64f17142aa89 131 m_r.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 132 m_l.speed(0);
GuillaumeMrzo 0:64f17142aa89 133 }
GuillaumeMrzo 0:64f17142aa89 134 if(flagRight == 1){
GuillaumeMrzo 0:64f17142aa89 135 led3 = 1;
GuillaumeMrzo 0:64f17142aa89 136 m_r.speed(0);
GuillaumeMrzo 0:64f17142aa89 137 m_l.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 138 }
GuillaumeMrzo 0:64f17142aa89 139 if(flagRev == 1){
GuillaumeMrzo 0:64f17142aa89 140 ledR=1;
GuillaumeMrzo 0:64f17142aa89 141 m_r.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 142 m_l.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 143 }
GuillaumeMrzo 0:64f17142aa89 144 else{
GuillaumeMrzo 0:64f17142aa89 145 m_r.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 146 m_l.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 147 }
GuillaumeMrzo 0:64f17142aa89 148 }
GuillaumeMrzo 0:64f17142aa89 149
GuillaumeMrzo 0:64f17142aa89 150 // Push Rev button 1st
GuillaumeMrzo 0:64f17142aa89 151 while(flagRev == 1){
GuillaumeMrzo 0:64f17142aa89 152
GuillaumeMrzo 0:64f17142aa89 153 if(flagLeft == 1){
GuillaumeMrzo 0:64f17142aa89 154 m_r.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 155 m_l.speed(-s_min);
GuillaumeMrzo 0:64f17142aa89 156 }
GuillaumeMrzo 0:64f17142aa89 157 if(flagRight == 1){
GuillaumeMrzo 0:64f17142aa89 158 m_r.speed(-s_min);
GuillaumeMrzo 0:64f17142aa89 159 m_l.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 160 }
GuillaumeMrzo 0:64f17142aa89 161 if(flagFW == 1){
GuillaumeMrzo 0:64f17142aa89 162 m_r.speed(-s_min);
GuillaumeMrzo 0:64f17142aa89 163 m_l.speed(-s_min);
GuillaumeMrzo 0:64f17142aa89 164 }
GuillaumeMrzo 0:64f17142aa89 165 else{
GuillaumeMrzo 0:64f17142aa89 166 m_r.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 167 m_l.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 168 }
GuillaumeMrzo 0:64f17142aa89 169 }
GuillaumeMrzo 0:64f17142aa89 170
GuillaumeMrzo 0:64f17142aa89 171 // Push Left button 1st
GuillaumeMrzo 0:64f17142aa89 172 while(flagLeft == 1){
GuillaumeMrzo 0:64f17142aa89 173
GuillaumeMrzo 0:64f17142aa89 174 if(flagFW == 1){
GuillaumeMrzo 0:64f17142aa89 175 m_r.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 176 m_l.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 177 }
GuillaumeMrzo 0:64f17142aa89 178 if(flagRight == 1){
GuillaumeMrzo 0:64f17142aa89 179 m_r.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 180 m_l.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 181 }
GuillaumeMrzo 0:64f17142aa89 182 if(flagRev == 1){
GuillaumeMrzo 0:64f17142aa89 183 m_r.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 184 m_l.speed(-s_min);
GuillaumeMrzo 0:64f17142aa89 185 }
GuillaumeMrzo 0:64f17142aa89 186 else{
GuillaumeMrzo 0:64f17142aa89 187 m_r.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 188 m_l.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 189 }
GuillaumeMrzo 0:64f17142aa89 190 }
GuillaumeMrzo 0:64f17142aa89 191
GuillaumeMrzo 0:64f17142aa89 192 // Push Right button 1st
GuillaumeMrzo 0:64f17142aa89 193 while(flagRight == 1){
GuillaumeMrzo 0:64f17142aa89 194
GuillaumeMrzo 0:64f17142aa89 195 if(flagFW == 1){
GuillaumeMrzo 0:64f17142aa89 196 m_r.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 197 m_l.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 198 }
GuillaumeMrzo 0:64f17142aa89 199 if(flagLeft == 1){
GuillaumeMrzo 0:64f17142aa89 200 m_r.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 201 m_l.speed(s_min);
GuillaumeMrzo 0:64f17142aa89 202 }
GuillaumeMrzo 0:64f17142aa89 203 if(flagRev == 1){
GuillaumeMrzo 0:64f17142aa89 204 m_r.speed(-s_min);
GuillaumeMrzo 0:64f17142aa89 205 m_l.speed(-s_min);
GuillaumeMrzo 0:64f17142aa89 206 }
GuillaumeMrzo 0:64f17142aa89 207 else{
GuillaumeMrzo 0:64f17142aa89 208 m_r.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 209 m_l.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 210 }
GuillaumeMrzo 0:64f17142aa89 211 }
GuillaumeMrzo 0:64f17142aa89 212 }
GuillaumeMrzo 0:64f17142aa89 213
GuillaumeMrzo 0:64f17142aa89 214 // ------- Automatic mode ------------------
GuillaumeMrzo 0:64f17142aa89 215 while(autom == 1) {
GuillaumeMrzo 0:64f17142aa89 216
GuillaumeMrzo 0:64f17142aa89 217 //int autom;
GuillaumeMrzo 0:64f17142aa89 218 autom_mutex.lock();
GuillaumeMrzo 0:64f17142aa89 219 autom = flag_autom;
GuillaumeMrzo 0:64f17142aa89 220 autom_mutex.unlock();
GuillaumeMrzo 0:64f17142aa89 221
GuillaumeMrzo 0:64f17142aa89 222 // No obstacle --> FW with
GuillaumeMrzo 0:64f17142aa89 223 if((IR_right < 0.2) && (IR_left < 0.2)){
GuillaumeMrzo 0:64f17142aa89 224 m_r.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 225 m_l.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 226 ledR = 0;
GuillaumeMrzo 0:64f17142aa89 227 ledL = 0;
GuillaumeMrzo 0:64f17142aa89 228 }
GuillaumeMrzo 0:64f17142aa89 229
GuillaumeMrzo 0:64f17142aa89 230 // Obstacle on the right --> turn left and reduce speed
GuillaumeMrzo 0:64f17142aa89 231 else if((IR_right >= 0.2) && (IR_left < 0.2)){
GuillaumeMrzo 0:64f17142aa89 232 m_r.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 233 m_l.speed(0);
GuillaumeMrzo 0:64f17142aa89 234 ainR = IR_right;
GuillaumeMrzo 0:64f17142aa89 235 ainL = IR_left;
GuillaumeMrzo 0:64f17142aa89 236 printf("Right: %0.2f - Left: %0.2f\n", ainR, ainL);
GuillaumeMrzo 0:64f17142aa89 237 ledR = 1;
GuillaumeMrzo 0:64f17142aa89 238 ledL = 0;
GuillaumeMrzo 0:64f17142aa89 239 }
GuillaumeMrzo 0:64f17142aa89 240
GuillaumeMrzo 0:64f17142aa89 241 // Obstacle on the left --> turn right and reduce speed
GuillaumeMrzo 0:64f17142aa89 242 else if((IR_right < 0.2) && (IR_left >= 0.2)){
GuillaumeMrzo 0:64f17142aa89 243 m_r.speed(0);
GuillaumeMrzo 0:64f17142aa89 244 m_l.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 245 ainR = IR_right;
GuillaumeMrzo 0:64f17142aa89 246 ainL = IR_left;
GuillaumeMrzo 0:64f17142aa89 247 printf("Right: %0.2f - Left: %0.2f\n", ainR, ainL);
GuillaumeMrzo 0:64f17142aa89 248 ledR = 0;
GuillaumeMrzo 0:64f17142aa89 249 ledL = 1;
GuillaumeMrzo 0:64f17142aa89 250 }
GuillaumeMrzo 0:64f17142aa89 251
GuillaumeMrzo 0:64f17142aa89 252 // Obstacle in front --> stop, move back and turn
GuillaumeMrzo 0:64f17142aa89 253 else if((IR_right >= 0.2) && (IR_left >= 0.2)){
GuillaumeMrzo 0:64f17142aa89 254 m_r.speed(0);
GuillaumeMrzo 0:64f17142aa89 255 m_l.speed(0);
GuillaumeMrzo 0:64f17142aa89 256 wait(0.5); // Stop for 0.5s
GuillaumeMrzo 0:64f17142aa89 257 ainR = IR_right;
GuillaumeMrzo 0:64f17142aa89 258 ainL = IR_left;
GuillaumeMrzo 0:64f17142aa89 259 printf("Right: %0.2f - Left: %0.2f\n", ainR, ainL);
GuillaumeMrzo 0:64f17142aa89 260 ledR = 1;
GuillaumeMrzo 0:64f17142aa89 261 ledL = 1;
GuillaumeMrzo 0:64f17142aa89 262
GuillaumeMrzo 0:64f17142aa89 263 if(IR_left > IR_right){
GuillaumeMrzo 0:64f17142aa89 264 m_r.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 265 m_l.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 266 Thread::wait(500); // Back for 0.5s
GuillaumeMrzo 0:64f17142aa89 267
GuillaumeMrzo 0:64f17142aa89 268 m_r.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 269 m_l.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 270 Thread::wait(500); // turn for 0.5s
GuillaumeMrzo 0:64f17142aa89 271 }
GuillaumeMrzo 0:64f17142aa89 272 else{
GuillaumeMrzo 0:64f17142aa89 273 m_r.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 274 m_l.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 275 Thread::wait(500); // Back for 0.5s
GuillaumeMrzo 0:64f17142aa89 276
GuillaumeMrzo 0:64f17142aa89 277 m_r.speed(s_max);
GuillaumeMrzo 0:64f17142aa89 278 m_l.speed(-s_max);
GuillaumeMrzo 0:64f17142aa89 279 Thread::wait(1000); // turn for 1s
GuillaumeMrzo 0:64f17142aa89 280 }
GuillaumeMrzo 0:64f17142aa89 281 }
GuillaumeMrzo 0:64f17142aa89 282 }
GuillaumeMrzo 0:64f17142aa89 283
GuillaumeMrzo 0:64f17142aa89 284 Thread::wait(10);
GuillaumeMrzo 0:64f17142aa89 285 }
GuillaumeMrzo 0:64f17142aa89 286 }
GuillaumeMrzo 0:64f17142aa89 287
GuillaumeMrzo 0:64f17142aa89 288
GuillaumeMrzo 0:64f17142aa89 289 // -------------------- Callback Blue ---------------------- //
GuillaumeMrzo 0:64f17142aa89 290 void blue_recv(){
GuillaumeMrzo 0:64f17142aa89 291 led1 = !led1;
GuillaumeMrzo 0:64f17142aa89 292 while(blue.readable()) {
GuillaumeMrzo 0:64f17142aa89 293 if (blue.getc()=='!') {
GuillaumeMrzo 0:64f17142aa89 294 if (blue.getc()=='B') { //button data packet
GuillaumeMrzo 0:64f17142aa89 295 bnum = blue.getc(); //button number
GuillaumeMrzo 0:64f17142aa89 296 bhit = blue.getc(); //1=hit, 0=release
GuillaumeMrzo 0:64f17142aa89 297 pc.putc(bnum);
GuillaumeMrzo 0:64f17142aa89 298 pc.putc(bhit);
GuillaumeMrzo 0:64f17142aa89 299 pc.printf("\n");
GuillaumeMrzo 0:64f17142aa89 300
GuillaumeMrzo 0:64f17142aa89 301 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
GuillaumeMrzo 0:64f17142aa89 302 switch (bnum) {
GuillaumeMrzo 0:64f17142aa89 303 case '1': //number button 1
GuillaumeMrzo 0:64f17142aa89 304 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 305 flag1 = 1;
GuillaumeMrzo 0:64f17142aa89 306 s_max = 0.3;
GuillaumeMrzo 0:64f17142aa89 307 pc.printf("Speed max 1: %.2f\n", s_max);
GuillaumeMrzo 0:64f17142aa89 308 } else {
GuillaumeMrzo 0:64f17142aa89 309 flag1 = 0;
GuillaumeMrzo 0:64f17142aa89 310 }
GuillaumeMrzo 0:64f17142aa89 311 break;
GuillaumeMrzo 0:64f17142aa89 312 case '2': //number button 2
GuillaumeMrzo 0:64f17142aa89 313 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 314 flag2 = 1;
GuillaumeMrzo 0:64f17142aa89 315 s_max = 0.6;
GuillaumeMrzo 0:64f17142aa89 316 pc.printf("Speed max 2: %.2f\n", s_max);
GuillaumeMrzo 0:64f17142aa89 317 } else {
GuillaumeMrzo 0:64f17142aa89 318 flag2 = 0;
GuillaumeMrzo 0:64f17142aa89 319 }
GuillaumeMrzo 0:64f17142aa89 320 break;
GuillaumeMrzo 0:64f17142aa89 321 case '3': //number button 3
GuillaumeMrzo 0:64f17142aa89 322 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 323 flag3 = 1;
GuillaumeMrzo 0:64f17142aa89 324 s_max = 0.8;
GuillaumeMrzo 0:64f17142aa89 325 pc.printf("Speed max 3: %.2f\n", s_max);
GuillaumeMrzo 0:64f17142aa89 326 } else {
GuillaumeMrzo 0:64f17142aa89 327 flag3 = 0;
GuillaumeMrzo 0:64f17142aa89 328 }
GuillaumeMrzo 0:64f17142aa89 329 break;
GuillaumeMrzo 0:64f17142aa89 330 case '4': //number button 4
GuillaumeMrzo 0:64f17142aa89 331 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 332 flag4 = 1;
GuillaumeMrzo 0:64f17142aa89 333 s_max = 1.0;
GuillaumeMrzo 0:64f17142aa89 334 pc.printf("Speed max 4: %.2f\n", s_max);
GuillaumeMrzo 0:64f17142aa89 335 } else {
GuillaumeMrzo 0:64f17142aa89 336 flag4 = 4;
GuillaumeMrzo 0:64f17142aa89 337 }
GuillaumeMrzo 0:64f17142aa89 338 break;
GuillaumeMrzo 0:64f17142aa89 339 case '5': //button 5 up arrow
GuillaumeMrzo 0:64f17142aa89 340 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 341 flagFW = 1;
GuillaumeMrzo 0:64f17142aa89 342 } else {
GuillaumeMrzo 0:64f17142aa89 343 flagFW = 0;
GuillaumeMrzo 0:64f17142aa89 344 }
GuillaumeMrzo 0:64f17142aa89 345 break;
GuillaumeMrzo 0:64f17142aa89 346 case '6': //button 6 down arrow
GuillaumeMrzo 0:64f17142aa89 347 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 348 flagRev = 1;
GuillaumeMrzo 0:64f17142aa89 349 } else {
GuillaumeMrzo 0:64f17142aa89 350 flagRev = 0;
GuillaumeMrzo 0:64f17142aa89 351 }
GuillaumeMrzo 0:64f17142aa89 352 break;
GuillaumeMrzo 0:64f17142aa89 353 case '7': //button 7 left arrow
GuillaumeMrzo 0:64f17142aa89 354 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 355 flagLeft = 1;
GuillaumeMrzo 0:64f17142aa89 356 } else {
GuillaumeMrzo 0:64f17142aa89 357 flagLeft = 0;
GuillaumeMrzo 0:64f17142aa89 358 }
GuillaumeMrzo 0:64f17142aa89 359 break;
GuillaumeMrzo 0:64f17142aa89 360 case '8': //button 8 right arrow
GuillaumeMrzo 0:64f17142aa89 361 if (bhit=='1') {
GuillaumeMrzo 0:64f17142aa89 362 flagRight = 1;
GuillaumeMrzo 0:64f17142aa89 363 } else {
GuillaumeMrzo 0:64f17142aa89 364 flagRight = 0;
GuillaumeMrzo 0:64f17142aa89 365 }
GuillaumeMrzo 0:64f17142aa89 366 break;
GuillaumeMrzo 0:64f17142aa89 367 default:
GuillaumeMrzo 0:64f17142aa89 368 break;
GuillaumeMrzo 0:64f17142aa89 369 }
GuillaumeMrzo 0:64f17142aa89 370 }
GuillaumeMrzo 0:64f17142aa89 371 }
GuillaumeMrzo 0:64f17142aa89 372 }
GuillaumeMrzo 0:64f17142aa89 373 }
GuillaumeMrzo 0:64f17142aa89 374 }
GuillaumeMrzo 0:64f17142aa89 375
GuillaumeMrzo 0:64f17142aa89 376
GuillaumeMrzo 0:64f17142aa89 377 // -------------------- Callback PC ---------------------- //
GuillaumeMrzo 0:64f17142aa89 378 void pc_recv(){
GuillaumeMrzo 0:64f17142aa89 379 led4 = !led4;
GuillaumeMrzo 0:64f17142aa89 380 while(pc.readable()) {
GuillaumeMrzo 0:64f17142aa89 381 blue.putc(pc.getc());
GuillaumeMrzo 0:64f17142aa89 382 }
GuillaumeMrzo 0:64f17142aa89 383 }
GuillaumeMrzo 0:64f17142aa89 384
GuillaumeMrzo 0:64f17142aa89 385
GuillaumeMrzo 0:64f17142aa89 386 // -------------------- Main ---------------------- //
GuillaumeMrzo 0:64f17142aa89 387 int main(){
GuillaumeMrzo 0:64f17142aa89 388 pc.baud(9600);
GuillaumeMrzo 0:64f17142aa89 389 blue.baud(9600);
GuillaumeMrzo 0:64f17142aa89 390
GuillaumeMrzo 0:64f17142aa89 391 pc.attach(&pc_recv, Serial::RxIrq);
GuillaumeMrzo 0:64f17142aa89 392 blue.attach(&blue_recv, Serial::RxIrq);
GuillaumeMrzo 0:64f17142aa89 393
GuillaumeMrzo 0:64f17142aa89 394 RfChip.PCD_Init();
GuillaumeMrzo 0:64f17142aa89 395
GuillaumeMrzo 0:64f17142aa89 396 flag_autom = 0;
GuillaumeMrzo 0:64f17142aa89 397 pc.printf("hello babe\n");
GuillaumeMrzo 0:64f17142aa89 398
GuillaumeMrzo 0:64f17142aa89 399 Thread t1(thread_RFID);
GuillaumeMrzo 0:64f17142aa89 400 Thread t2(thread_Motor);
GuillaumeMrzo 0:64f17142aa89 401
GuillaumeMrzo 0:64f17142aa89 402 while(1) {
GuillaumeMrzo 0:64f17142aa89 403 Thread::wait(500); // wait 0.5s
GuillaumeMrzo 0:64f17142aa89 404 }
GuillaumeMrzo 0:64f17142aa89 405 }