Guillaume Mourozeau / Mbed 2 deprecated Project_mbedRobot

1st Version of LiveXboxRobot project. Robot side.

Dependencies:   Motor mbed-rtos mbed

main.cpp@0:398cc0dda9e7, 2016-04-28 (annotated)

Committer:
GuillaumeMrzo
Date:
Thu Apr 28 12:01:17 2016 +0000
Revision:
0:398cc0dda9e7
1st Version of LiveXboxRobot project. Robot side.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GuillaumeMrzo 0:398cc0dda9e7 1 #include "mbed.h"
GuillaumeMrzo 0:398cc0dda9e7 2 #include "Motor.h"
GuillaumeMrzo 0:398cc0dda9e7 3 #include "rtos.h"
GuillaumeMrzo 0:398cc0dda9e7 4 #include "stdio.h"
GuillaumeMrzo 0:398cc0dda9e7 5
GuillaumeMrzo 0:398cc0dda9e7 6 // Motors and speed
GuillaumeMrzo 0:398cc0dda9e7 7 Motor m_r(p24, p17, p18); // pwm, fwd, rev
GuillaumeMrzo 0:398cc0dda9e7 8 Motor m_l(p25, p20, p19); // pwm, fwd, rev
GuillaumeMrzo 0:398cc0dda9e7 9 float s_max = 0.5;
GuillaumeMrzo 0:398cc0dda9e7 10 float s_min = 0.0;
GuillaumeMrzo 0:398cc0dda9e7 11
GuillaumeMrzo 0:398cc0dda9e7 12 // LEDS
GuillaumeMrzo 0:398cc0dda9e7 13 DigitalOut led1(LED1);
GuillaumeMrzo 0:398cc0dda9e7 14 DigitalOut led2(LED2);
GuillaumeMrzo 0:398cc0dda9e7 15 DigitalOut led3(LED3);
GuillaumeMrzo 0:398cc0dda9e7 16 DigitalOut led4(LED4);
GuillaumeMrzo 0:398cc0dda9e7 17
GuillaumeMrzo 0:398cc0dda9e7 18 // Serial ports
GuillaumeMrzo 0:398cc0dda9e7 19 RawSerial pc(USBTX, USBRX);
GuillaumeMrzo 0:398cc0dda9e7 20 RawSerial xbee(p9, p10);
GuillaumeMrzo 0:398cc0dda9e7 21 char X;
GuillaumeMrzo 0:398cc0dda9e7 22 Mutex xbee_mutex;
GuillaumeMrzo 0:398cc0dda9e7 23 DigitalOut rst1(p8);
GuillaumeMrzo 0:398cc0dda9e7 24
GuillaumeMrzo 0:398cc0dda9e7 25 // Flags for the buttons
GuillaumeMrzo 0:398cc0dda9e7 26 int flagFW, flagRight, flagLeft, flagRev;
GuillaumeMrzo 0:398cc0dda9e7 27 int flag1, flag2, flag3, flag4;
GuillaumeMrzo 0:398cc0dda9e7 28 int flag_control;
GuillaumeMrzo 0:398cc0dda9e7 29
GuillaumeMrzo 0:398cc0dda9e7 30 // IR detectors
GuillaumeMrzo 0:398cc0dda9e7 31 AnalogIn IR_right(p15);
GuillaumeMrzo 0:398cc0dda9e7 32 AnalogIn IR_left(p16);
GuillaumeMrzo 0:398cc0dda9e7 33 float dist = 0.2;
GuillaumeMrzo 0:398cc0dda9e7 34 int iR, iL;
GuillaumeMrzo 0:398cc0dda9e7 35
GuillaumeMrzo 0:398cc0dda9e7 36
GuillaumeMrzo 0:398cc0dda9e7 37
GuillaumeMrzo 0:398cc0dda9e7 38
GuillaumeMrzo 0:398cc0dda9e7 39 // ------------------- Thread Motor ------------------------//
GuillaumeMrzo 0:398cc0dda9e7 40 void thread_Motor(void const *args){
GuillaumeMrzo 0:398cc0dda9e7 41 while (true) {
GuillaumeMrzo 0:398cc0dda9e7 42
GuillaumeMrzo 0:398cc0dda9e7 43 m_r.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 44 m_l.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 45
GuillaumeMrzo 0:398cc0dda9e7 46 int control = 0;
GuillaumeMrzo 0:398cc0dda9e7 47
GuillaumeMrzo 0:398cc0dda9e7 48 iR = (int)(100*IR_right);
GuillaumeMrzo 0:398cc0dda9e7 49 iL = (int)(100*IR_left);
GuillaumeMrzo 0:398cc0dda9e7 50
GuillaumeMrzo 0:398cc0dda9e7 51
GuillaumeMrzo 0:398cc0dda9e7 52 xbee.putc('R');
GuillaumeMrzo 0:398cc0dda9e7 53 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 54 xbee.putc(iR);
GuillaumeMrzo 0:398cc0dda9e7 55 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 56
GuillaumeMrzo 0:398cc0dda9e7 57 xbee.putc('L');
GuillaumeMrzo 0:398cc0dda9e7 58 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 59 xbee.putc(iL);
GuillaumeMrzo 0:398cc0dda9e7 60 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 61
GuillaumeMrzo 0:398cc0dda9e7 62
GuillaumeMrzo 0:398cc0dda9e7 63 // ------- Keyboard-GUI mode ------------------
GuillaumeMrzo 0:398cc0dda9e7 64 if(control == 0){
GuillaumeMrzo 0:398cc0dda9e7 65
GuillaumeMrzo 0:398cc0dda9e7 66 led1 = led2 = led3 = led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 67
GuillaumeMrzo 0:398cc0dda9e7 68 // Push FW button 1st
GuillaumeMrzo 0:398cc0dda9e7 69 while(flagFW == 1){
GuillaumeMrzo 0:398cc0dda9e7 70 /*
GuillaumeMrzo 0:398cc0dda9e7 71 iR = (int)(100*IR_right);
GuillaumeMrzo 0:398cc0dda9e7 72 iL = (int)(100*IR_left);
GuillaumeMrzo 0:398cc0dda9e7 73
GuillaumeMrzo 0:398cc0dda9e7 74
GuillaumeMrzo 0:398cc0dda9e7 75 xbee.putc('R');
GuillaumeMrzo 0:398cc0dda9e7 76 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 77 xbee.putc(iR);
GuillaumeMrzo 0:398cc0dda9e7 78 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 79
GuillaumeMrzo 0:398cc0dda9e7 80 xbee.putc('L');
GuillaumeMrzo 0:398cc0dda9e7 81 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 82 xbee.putc(iL);
GuillaumeMrzo 0:398cc0dda9e7 83 Thread::wait(100); */
GuillaumeMrzo 0:398cc0dda9e7 84
GuillaumeMrzo 0:398cc0dda9e7 85
GuillaumeMrzo 0:398cc0dda9e7 86 led2 = led3 = 1;
GuillaumeMrzo 0:398cc0dda9e7 87 led1 = led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 88
GuillaumeMrzo 0:398cc0dda9e7 89 if(flagLeft == 1){
GuillaumeMrzo 0:398cc0dda9e7 90 led1 = 1;
GuillaumeMrzo 0:398cc0dda9e7 91 m_l.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 92 pc.printf("left");
GuillaumeMrzo 0:398cc0dda9e7 93
GuillaumeMrzo 0:398cc0dda9e7 94 // No obstacle
GuillaumeMrzo 0:398cc0dda9e7 95 if((IR_right < dist) && (IR_left < dist)){
GuillaumeMrzo 0:398cc0dda9e7 96 m_r.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 97 }
GuillaumeMrzo 0:398cc0dda9e7 98 // Obstacle on the left
GuillaumeMrzo 0:398cc0dda9e7 99 else if((IR_left >= dist)){
GuillaumeMrzo 0:398cc0dda9e7 100 m_r.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 101 }
GuillaumeMrzo 0:398cc0dda9e7 102 }
GuillaumeMrzo 0:398cc0dda9e7 103 if(flagRight == 1){
GuillaumeMrzo 0:398cc0dda9e7 104 led4 = 1;
GuillaumeMrzo 0:398cc0dda9e7 105 m_r.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 106 pc.printf("right");
GuillaumeMrzo 0:398cc0dda9e7 107
GuillaumeMrzo 0:398cc0dda9e7 108 // No obstacle
GuillaumeMrzo 0:398cc0dda9e7 109 if((IR_right < dist) && (IR_left < dist)){
GuillaumeMrzo 0:398cc0dda9e7 110 m_l.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 111 }
GuillaumeMrzo 0:398cc0dda9e7 112 // Obstacle on the right
GuillaumeMrzo 0:398cc0dda9e7 113 else if((IR_right >= dist)){
GuillaumeMrzo 0:398cc0dda9e7 114 m_l.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 115 }
GuillaumeMrzo 0:398cc0dda9e7 116 }
GuillaumeMrzo 0:398cc0dda9e7 117 if(flagRev == 1){
GuillaumeMrzo 0:398cc0dda9e7 118 led1 = led2 = led3 = led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 119 m_r.speed(s_min);
GuillaumeMrzo 0:398cc0dda9e7 120 m_l.speed(s_min);
GuillaumeMrzo 0:398cc0dda9e7 121 }
GuillaumeMrzo 0:398cc0dda9e7 122 else{
GuillaumeMrzo 0:398cc0dda9e7 123 // No obstacle
GuillaumeMrzo 0:398cc0dda9e7 124 if((IR_right < dist) && (IR_left < dist)){
GuillaumeMrzo 0:398cc0dda9e7 125 m_r.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 126 m_l.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 127 }
GuillaumeMrzo 0:398cc0dda9e7 128
GuillaumeMrzo 0:398cc0dda9e7 129 // Obstacle on the right --> turn left and reduce speed -----------------------------------
GuillaumeMrzo 0:398cc0dda9e7 130 else if((IR_right >= dist) && (IR_left < dist)){
GuillaumeMrzo 0:398cc0dda9e7 131 m_r.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 132 m_l.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 133 }
GuillaumeMrzo 0:398cc0dda9e7 134
GuillaumeMrzo 0:398cc0dda9e7 135 // Obstacle on the left --> turn right and reduce speed
GuillaumeMrzo 0:398cc0dda9e7 136 else if((IR_right < dist) && (IR_left >= dist)){
GuillaumeMrzo 0:398cc0dda9e7 137 m_r.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 138 m_l.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 139 }
GuillaumeMrzo 0:398cc0dda9e7 140
GuillaumeMrzo 0:398cc0dda9e7 141 // Obstacle in front --> stop ------------------------------------------
GuillaumeMrzo 0:398cc0dda9e7 142 else if((IR_right >= dist) && (IR_left >= dist)){
GuillaumeMrzo 0:398cc0dda9e7 143 m_r.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 144 m_l.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 145 }
GuillaumeMrzo 0:398cc0dda9e7 146 }
GuillaumeMrzo 0:398cc0dda9e7 147 }
GuillaumeMrzo 0:398cc0dda9e7 148
GuillaumeMrzo 0:398cc0dda9e7 149 // Push Rev button 1st
GuillaumeMrzo 0:398cc0dda9e7 150 while(flagRev == 1){
GuillaumeMrzo 0:398cc0dda9e7 151 led1 = led2 = led3 = led4 = 1;
GuillaumeMrzo 0:398cc0dda9e7 152 /*
GuillaumeMrzo 0:398cc0dda9e7 153 iR = (int)(100*IR_right);
GuillaumeMrzo 0:398cc0dda9e7 154 iL = (int)(100*IR_left);
GuillaumeMrzo 0:398cc0dda9e7 155
GuillaumeMrzo 0:398cc0dda9e7 156
GuillaumeMrzo 0:398cc0dda9e7 157 xbee.putc('R');
GuillaumeMrzo 0:398cc0dda9e7 158 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 159 xbee.putc(iR);
GuillaumeMrzo 0:398cc0dda9e7 160 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 161
GuillaumeMrzo 0:398cc0dda9e7 162 xbee.putc('L');
GuillaumeMrzo 0:398cc0dda9e7 163 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 164 xbee.putc(iL);
GuillaumeMrzo 0:398cc0dda9e7 165 Thread::wait(100);*/
GuillaumeMrzo 0:398cc0dda9e7 166
GuillaumeMrzo 0:398cc0dda9e7 167
GuillaumeMrzo 0:398cc0dda9e7 168 if(flagLeft == 1){
GuillaumeMrzo 0:398cc0dda9e7 169 led1 = 0;
GuillaumeMrzo 0:398cc0dda9e7 170 m_r.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 171 m_l.speed(-s_min);
GuillaumeMrzo 0:398cc0dda9e7 172 }
GuillaumeMrzo 0:398cc0dda9e7 173 if(flagRight == 1){
GuillaumeMrzo 0:398cc0dda9e7 174 led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 175 m_r.speed(-s_min);
GuillaumeMrzo 0:398cc0dda9e7 176 m_l.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 177 }
GuillaumeMrzo 0:398cc0dda9e7 178 if(flagFW == 1){
GuillaumeMrzo 0:398cc0dda9e7 179 led1 = led2 = led3 = led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 180 m_r.speed(-s_min);
GuillaumeMrzo 0:398cc0dda9e7 181 m_l.speed(-s_min);
GuillaumeMrzo 0:398cc0dda9e7 182 }
GuillaumeMrzo 0:398cc0dda9e7 183 else{
GuillaumeMrzo 0:398cc0dda9e7 184 m_r.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 185 m_l.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 186 }
GuillaumeMrzo 0:398cc0dda9e7 187 }
GuillaumeMrzo 0:398cc0dda9e7 188
GuillaumeMrzo 0:398cc0dda9e7 189 // Push Left button 1st
GuillaumeMrzo 0:398cc0dda9e7 190 while(flagLeft == 1){
GuillaumeMrzo 0:398cc0dda9e7 191 led1 = 1;
GuillaumeMrzo 0:398cc0dda9e7 192 led2 = led3 = led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 193
GuillaumeMrzo 0:398cc0dda9e7 194 /*
GuillaumeMrzo 0:398cc0dda9e7 195 iR = (int)(100*IR_right);
GuillaumeMrzo 0:398cc0dda9e7 196 iL = (int)(100*IR_left);
GuillaumeMrzo 0:398cc0dda9e7 197
GuillaumeMrzo 0:398cc0dda9e7 198 xbee.putc('R');
GuillaumeMrzo 0:398cc0dda9e7 199 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 200 xbee.putc(iR);
GuillaumeMrzo 0:398cc0dda9e7 201 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 202
GuillaumeMrzo 0:398cc0dda9e7 203 xbee.putc('L');
GuillaumeMrzo 0:398cc0dda9e7 204 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 205 xbee.putc(iL);
GuillaumeMrzo 0:398cc0dda9e7 206 Thread::wait(100);*/
GuillaumeMrzo 0:398cc0dda9e7 207
GuillaumeMrzo 0:398cc0dda9e7 208 if(flagFW == 1){
GuillaumeMrzo 0:398cc0dda9e7 209 led2 = led3 = 1;
GuillaumeMrzo 0:398cc0dda9e7 210 m_l.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 211 // No obstacle
GuillaumeMrzo 0:398cc0dda9e7 212 if(IR_left < dist){
GuillaumeMrzo 0:398cc0dda9e7 213 m_r.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 214 }
GuillaumeMrzo 0:398cc0dda9e7 215 // Obstacle on the left
GuillaumeMrzo 0:398cc0dda9e7 216 else{
GuillaumeMrzo 0:398cc0dda9e7 217 m_r.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 218 }
GuillaumeMrzo 0:398cc0dda9e7 219 }
GuillaumeMrzo 0:398cc0dda9e7 220 if(flagRight == 1){
GuillaumeMrzo 0:398cc0dda9e7 221 led1 = led2 = led3 = led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 222 m_r.speed(s_min);
GuillaumeMrzo 0:398cc0dda9e7 223 m_l.speed(s_min);
GuillaumeMrzo 0:398cc0dda9e7 224 }
GuillaumeMrzo 0:398cc0dda9e7 225 if(flagRev == 1){
GuillaumeMrzo 0:398cc0dda9e7 226 led2 = led3 = led4 = 1;
GuillaumeMrzo 0:398cc0dda9e7 227 led1 = 0;
GuillaumeMrzo 0:398cc0dda9e7 228 m_r.speed(-s_min);
GuillaumeMrzo 0:398cc0dda9e7 229 m_l.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 230 }
GuillaumeMrzo 0:398cc0dda9e7 231 else{
GuillaumeMrzo 0:398cc0dda9e7 232 m_r.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 233 m_l.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 234 }
GuillaumeMrzo 0:398cc0dda9e7 235 }
GuillaumeMrzo 0:398cc0dda9e7 236
GuillaumeMrzo 0:398cc0dda9e7 237 // Push Right button 1st
GuillaumeMrzo 0:398cc0dda9e7 238 while(flagRight == 1){
GuillaumeMrzo 0:398cc0dda9e7 239 led4 = 1;
GuillaumeMrzo 0:398cc0dda9e7 240 led1 = led2 = led3 = 0;
GuillaumeMrzo 0:398cc0dda9e7 241
GuillaumeMrzo 0:398cc0dda9e7 242 /*
GuillaumeMrzo 0:398cc0dda9e7 243 iR = (int)(100*IR_right);
GuillaumeMrzo 0:398cc0dda9e7 244 iL = (int)(100*IR_left);
GuillaumeMrzo 0:398cc0dda9e7 245
GuillaumeMrzo 0:398cc0dda9e7 246
GuillaumeMrzo 0:398cc0dda9e7 247 xbee.putc('R');
GuillaumeMrzo 0:398cc0dda9e7 248 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 249 xbee.putc(iR);
GuillaumeMrzo 0:398cc0dda9e7 250 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 251
GuillaumeMrzo 0:398cc0dda9e7 252 xbee.putc('L');
GuillaumeMrzo 0:398cc0dda9e7 253 Thread::wait(100);
GuillaumeMrzo 0:398cc0dda9e7 254 xbee.putc(iL);
GuillaumeMrzo 0:398cc0dda9e7 255 Thread::wait(100);*/
GuillaumeMrzo 0:398cc0dda9e7 256
GuillaumeMrzo 0:398cc0dda9e7 257 if(flagFW == 1){
GuillaumeMrzo 0:398cc0dda9e7 258 led2 = led3 = 1;
GuillaumeMrzo 0:398cc0dda9e7 259 m_r.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 260 // No obstacle
GuillaumeMrzo 0:398cc0dda9e7 261 if(IR_right < dist){
GuillaumeMrzo 0:398cc0dda9e7 262 m_l.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 263 }
GuillaumeMrzo 0:398cc0dda9e7 264 // Obstacle on the right
GuillaumeMrzo 0:398cc0dda9e7 265 else{
GuillaumeMrzo 0:398cc0dda9e7 266 m_l.speed(0);
GuillaumeMrzo 0:398cc0dda9e7 267 }
GuillaumeMrzo 0:398cc0dda9e7 268 }
GuillaumeMrzo 0:398cc0dda9e7 269 if(flagLeft == 1){
GuillaumeMrzo 0:398cc0dda9e7 270 led1 = led2 = led3 = led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 271 m_r.speed(s_min);
GuillaumeMrzo 0:398cc0dda9e7 272 m_l.speed(s_min);
GuillaumeMrzo 0:398cc0dda9e7 273 }
GuillaumeMrzo 0:398cc0dda9e7 274 if(flagRev == 1){
GuillaumeMrzo 0:398cc0dda9e7 275 led2 = led3 = led1 = 1;
GuillaumeMrzo 0:398cc0dda9e7 276 led4 = 0;
GuillaumeMrzo 0:398cc0dda9e7 277 m_r.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 278 m_l.speed(-s_min);
GuillaumeMrzo 0:398cc0dda9e7 279 }
GuillaumeMrzo 0:398cc0dda9e7 280 else{
GuillaumeMrzo 0:398cc0dda9e7 281 m_r.speed(-s_max);
GuillaumeMrzo 0:398cc0dda9e7 282 m_l.speed(s_max);
GuillaumeMrzo 0:398cc0dda9e7 283 }
GuillaumeMrzo 0:398cc0dda9e7 284 }
GuillaumeMrzo 0:398cc0dda9e7 285 }
GuillaumeMrzo 0:398cc0dda9e7 286
GuillaumeMrzo 0:398cc0dda9e7 287
GuillaumeMrzo 0:398cc0dda9e7 288 Thread::wait(10);
GuillaumeMrzo 0:398cc0dda9e7 289 }
GuillaumeMrzo 0:398cc0dda9e7 290 }
GuillaumeMrzo 0:398cc0dda9e7 291
GuillaumeMrzo 0:398cc0dda9e7 292
GuillaumeMrzo 0:398cc0dda9e7 293 // -------------------- Callback Xbee receive ---------------------- //
GuillaumeMrzo 0:398cc0dda9e7 294 void xbee_recv(){
GuillaumeMrzo 0:398cc0dda9e7 295 led1 = !led1;
GuillaumeMrzo 0:398cc0dda9e7 296 while(xbee.readable()) {
GuillaumeMrzo 0:398cc0dda9e7 297 xbee_mutex.lock();
GuillaumeMrzo 0:398cc0dda9e7 298 X = xbee.getc();
GuillaumeMrzo 0:398cc0dda9e7 299 xbee_mutex.unlock();
GuillaumeMrzo 0:398cc0dda9e7 300 switch (X) {
GuillaumeMrzo 0:398cc0dda9e7 301 case '1': //number button 1
GuillaumeMrzo 0:398cc0dda9e7 302 s_max = 0.3;
GuillaumeMrzo 0:398cc0dda9e7 303 dist = 0.2;
GuillaumeMrzo 0:398cc0dda9e7 304 pc.printf("Speed max 1: %.2f\n", s_max);
GuillaumeMrzo 0:398cc0dda9e7 305 break;
GuillaumeMrzo 0:398cc0dda9e7 306 case '2': //number button 2
GuillaumeMrzo 0:398cc0dda9e7 307 s_max = 0.5;
GuillaumeMrzo 0:398cc0dda9e7 308 dist = 0.2;
GuillaumeMrzo 0:398cc0dda9e7 309 pc.printf("Speed max 1: %.2f\n", s_max);
GuillaumeMrzo 0:398cc0dda9e7 310 break;
GuillaumeMrzo 0:398cc0dda9e7 311 case '3': //number button 3
GuillaumeMrzo 0:398cc0dda9e7 312 s_max = 0.8;
GuillaumeMrzo 0:398cc0dda9e7 313 dist = 0.25;
GuillaumeMrzo 0:398cc0dda9e7 314 pc.printf("Speed max 1: %.2f\n", s_max);
GuillaumeMrzo 0:398cc0dda9e7 315 break;
GuillaumeMrzo 0:398cc0dda9e7 316 case '4': //number button 4
GuillaumeMrzo 0:398cc0dda9e7 317 s_max = 1.0;
GuillaumeMrzo 0:398cc0dda9e7 318 dist = 0.3;
GuillaumeMrzo 0:398cc0dda9e7 319 pc.printf("Speed max 1: %.2f\n", s_max);
GuillaumeMrzo 0:398cc0dda9e7 320 break;
GuillaumeMrzo 0:398cc0dda9e7 321 case 'U': // key up arrow
GuillaumeMrzo 0:398cc0dda9e7 322 flagFW = 1;
GuillaumeMrzo 0:398cc0dda9e7 323 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 324 break;
GuillaumeMrzo 0:398cc0dda9e7 325 case 'D': // key 6 down arrow
GuillaumeMrzo 0:398cc0dda9e7 326 flagRev = 1;
GuillaumeMrzo 0:398cc0dda9e7 327 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 328 break;
GuillaumeMrzo 0:398cc0dda9e7 329 case 'L': // key 7 left arrow
GuillaumeMrzo 0:398cc0dda9e7 330 flagLeft = 1;
GuillaumeMrzo 0:398cc0dda9e7 331 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 332 break;
GuillaumeMrzo 0:398cc0dda9e7 333 case 'R': //button 8 right arrow
GuillaumeMrzo 0:398cc0dda9e7 334 flagRight = 1;
GuillaumeMrzo 0:398cc0dda9e7 335 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 336 break;
GuillaumeMrzo 0:398cc0dda9e7 337 case 'A': //button 8 right arrow
GuillaumeMrzo 0:398cc0dda9e7 338 flagFW = 0;
GuillaumeMrzo 0:398cc0dda9e7 339 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 340 break;
GuillaumeMrzo 0:398cc0dda9e7 341 case 'B': //button 8 right arrow
GuillaumeMrzo 0:398cc0dda9e7 342 flagRev = 0;
GuillaumeMrzo 0:398cc0dda9e7 343 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 344 break;
GuillaumeMrzo 0:398cc0dda9e7 345 case 'C': //button 8 right arrow
GuillaumeMrzo 0:398cc0dda9e7 346 flagLeft = 0;
GuillaumeMrzo 0:398cc0dda9e7 347 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 348 break;
GuillaumeMrzo 0:398cc0dda9e7 349 case 'E': //button 8 right arrow
GuillaumeMrzo 0:398cc0dda9e7 350 flagRight = 0;
GuillaumeMrzo 0:398cc0dda9e7 351 pc.putc(X);
GuillaumeMrzo 0:398cc0dda9e7 352 break;
GuillaumeMrzo 0:398cc0dda9e7 353 default:
GuillaumeMrzo 0:398cc0dda9e7 354 break;
GuillaumeMrzo 0:398cc0dda9e7 355 }
GuillaumeMrzo 0:398cc0dda9e7 356 }
GuillaumeMrzo 0:398cc0dda9e7 357 }
GuillaumeMrzo 0:398cc0dda9e7 358
GuillaumeMrzo 0:398cc0dda9e7 359
GuillaumeMrzo 0:398cc0dda9e7 360 // -------------------- Main ---------------------- //
GuillaumeMrzo 0:398cc0dda9e7 361 int main(){
GuillaumeMrzo 0:398cc0dda9e7 362 pc.baud(9600);
GuillaumeMrzo 0:398cc0dda9e7 363 rst1 = 0; //Set reset pin to 0
GuillaumeMrzo 0:398cc0dda9e7 364 wait_ms(1);
GuillaumeMrzo 0:398cc0dda9e7 365 rst1 = 1; //Set reset pin to 1
GuillaumeMrzo 0:398cc0dda9e7 366 wait_ms(1);
GuillaumeMrzo 0:398cc0dda9e7 367
GuillaumeMrzo 0:398cc0dda9e7 368 xbee.attach(&xbee_recv, Serial::RxIrq);
GuillaumeMrzo 0:398cc0dda9e7 369
GuillaumeMrzo 0:398cc0dda9e7 370 Thread t2(thread_Motor);
GuillaumeMrzo 0:398cc0dda9e7 371
GuillaumeMrzo 0:398cc0dda9e7 372 while(1) {
GuillaumeMrzo 0:398cc0dda9e7 373 Thread::wait(500); // wait 0.5s
GuillaumeMrzo 0:398cc0dda9e7 374 }
GuillaumeMrzo 0:398cc0dda9e7 375 }