eLab Team / Mbed 2 deprecated myRobot

Working version without LEDs

Dependencies:   mbed WS2812

Voici le dernier schéma de cablage (version du 08/02/2020)

https://os.mbed.com/media/uploads/max_ence/schemarobot_fev2020.pdf

mainCopy.txt@1:69b5d8f0ba9c, 2020-05-30 (annotated)

Committer:
elab
Date:
Sat May 30 09:31:57 2020 +0000
Revision:
1:69b5d8f0ba9c
Parent:
0:0e577ce96b2f 
pour eLab;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elab 0:0e577ce96b2f 1 /*
elab 0:0e577ce96b2f 2 * Crealab : version Scratch
elab 0:0e577ce96b2f 3 */
elab 0:0e577ce96b2f 4
elab 0:0e577ce96b2f 5 #undef printf
elab 0:0e577ce96b2f 6 #include "Crealab.h"
elab 0:0e577ce96b2f 7 #include "LED_WS2812.h"
elab 0:0e577ce96b2f 8 #include "VL53L0X.h"
elab 0:0e577ce96b2f 9
elab 0:0e577ce96b2f 10 //#include "AsyncToF.h"
elab 0:0e577ce96b2f 11
elab 0:0e577ce96b2f 12 Serial pc_uart(USBTX, USBRX); // USBTX = PA2
elab 0:0e577ce96b2f 13 Serial bt_uart(D5, D4); // PA9 = Tx, PA10 = Rx
elab 0:0e577ce96b2f 14 uint32_t go;
elab 0:0e577ce96b2f 15 bool ShowDataLog=false;
elab 0:0e577ce96b2f 16 uint16_t TOF_UseRange_mm = 450;
elab 0:0e577ce96b2f 17 // ---------------- PIN DEFINITIONS ---------------------
elab 0:0e577ce96b2f 18
elab 0:0e577ce96b2f 19 InterruptIn buttonBox(PA_12);
elab 0:0e577ce96b2f 20
elab 0:0e577ce96b2f 21 // --- Define the Four PINs & Time of movement used for Motor drive
elab 0:0e577ce96b2f 22 //Motor motorRD(PA_4, PA_3, PA_1, PA_0);
elab 0:0e577ce96b2f 23 //Motor motorRG(PA_8, PA_11, PB_5, PB_4);
elab 0:0e577ce96b2f 24 Motor motorRG(D12,D11,D10,D9);
elab 0:0e577ce96b2f 25 Motor motorRD(A3,A2,A1,A0);
elab 0:0e577ce96b2f 26 Creabot mybot(&motorRG, &motorRD, 8.10f,8.3f); // insert the motors and indicate wheel diameter and distance between wheels
elab 0:0e577ce96b2f 27 LED_WS2812 ledBand(A4,2);
elab 0:0e577ce96b2f 28 /*AsyncToF myToF(D4,D5,D13,A2,0x52);*/
elab 0:0e577ce96b2f 29 //AsyncToF myToF(D4,D5,D13,NC,0x52);
elab 0:0e577ce96b2f 30 DigitalIn isCmd(A5); // relier le pin A5 a la masse (GND) pour faire fonctionner le robot en mode demo autonome
elab 0:0e577ce96b2f 31
elab 0:0e577ce96b2f 32 static I2C busI2C(D0,D1);
elab 0:0e577ce96b2f 33 static DigitalOut shutDownPin(D6);
elab 0:0e577ce96b2f 34 static VL53L0X TOF1 (&busI2C, &shutDownPin, D7);
elab 0:0e577ce96b2f 35
elab 0:0e577ce96b2f 36 bool receivedCOMMAND;
elab 0:0e577ce96b2f 37 char commandRECEIVED;
elab 0:0e577ce96b2f 38 int parameterRECEIVED;
elab 0:0e577ce96b2f 39 int state;
elab 0:0e577ce96b2f 40 char commandLine[256];
elab 0:0e577ce96b2f 41 int commandPosition;
elab 0:0e577ce96b2f 42 VL53L0X_Error TOFError;
elab 0:0e577ce96b2f 43
elab 0:0e577ce96b2f 44 void init_TOF( void )
elab 0:0e577ce96b2f 45 {
elab 0:0e577ce96b2f 46 pc_uart.printf("\n\r--- Initializing the TOF sensor ---\n\r");
elab 0:0e577ce96b2f 47
elab 0:0e577ce96b2f 48 /* Init the TOF sensor with default params */
elab 0:0e577ce96b2f 49 TOFError = TOF1.init_sensor();
elab 0:0e577ce96b2f 50 /* This configuration "Range_Config_HIGH_SPEED" sets following parameters:
elab 0:0e577ce96b2f 51 signalLimit = (FixPoint1616_t)(0.15*65536); ==> VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE
elab 0:0e577ce96b2f 52 sigmaLimit = (FixPoint1616_t)(32*65536); ==> VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE
elab 0:0e577ce96b2f 53 timingBudget = 10000; Must be > c_min_timing_budget_us = 20000;
elab 0:0e577ce96b2f 54 preRangeVcselPeriod = 14;
elab 0:0e577ce96b2f 55 finalRangeVcselPeriod = 10; */
elab 0:0e577ce96b2f 56
elab 0:0e577ce96b2f 57 if (TOFError == VL53L0X_ERROR_NONE) { TOFError = TOF1.start_measurement(range_continuous_polling, NULL, Range_Config_DEFAULT); }
elab 0:0e577ce96b2f 58 pc_uart.printf("TOFError = %d. ( 0 = OK ) \n\r", TOFError);
elab 0:0e577ce96b2f 59 pc_uart.printf(" TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = %4.2f \n\r",
elab 0:0e577ce96b2f 60 TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] /65536.1);
elab 0:0e577ce96b2f 61 pc_uart.printf(" TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = %4.2f \n\r",
elab 0:0e577ce96b2f 62 TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] /65536.1);
elab 0:0e577ce96b2f 63
elab 0:0e577ce96b2f 64 if (TOFError == VL53L0X_ERROR_NONE) { TOFError = TOF1.VL53L0X_set_measurement_timing_budget_us(100000); }
elab 0:0e577ce96b2f 65 TOF1.CurrentParameters.RangeOffset_um = 14; // seems it is wrongly configured by default to 54mm offset!
elab 0:0e577ce96b2f 66 pc_uart.printf("TOFError = %d. ( 0 = OK ) \n\r", TOFError);
elab 0:0e577ce96b2f 67
elab 0:0e577ce96b2f 68 // Report_Deep_Infos(TOF1);
elab 0:0e577ce96b2f 69
elab 0:0e577ce96b2f 70 if (TOFError != VL53L0X_ERROR_NONE)
elab 0:0e577ce96b2f 71 { pc_uart.printf("\n\r--- Error Initializing the TOF sensor ---\n\r");
elab 0:0e577ce96b2f 72 //while(1) { }; // consider to hang up the device here
elab 0:0e577ce96b2f 73 // alternatively just clear the Error State:
elab 0:0e577ce96b2f 74 // TOF1.ErrState = VL53L0X_ERROR_NONE;
elab 0:0e577ce96b2f 75 }
elab 0:0e577ce96b2f 76 else
elab 0:0e577ce96b2f 77 { pc_uart.printf("\n\r--- Success Initializing the TOF sensor ---\n\r"); }
elab 0:0e577ce96b2f 78 }
elab 0:0e577ce96b2f 79
elab 0:0e577ce96b2f 80 bool TOF_OK;
elab 0:0e577ce96b2f 81 VL53L0X_RangingMeasurementData_t RangeResults;
elab 0:0e577ce96b2f 82 uint16_t TOF_Dist_mm;
elab 0:0e577ce96b2f 83 double TOF_RelDist; // measured distance relative to the Used Range. Should be clamped to 0 ... 1, but can actually also be negative or >1
elab 0:0e577ce96b2f 84 const uint32_t out_of_range = 0xffff;
elab 0:0e577ce96b2f 85 uint16_t TOF_MinDist_mm=5;
elab 0:0e577ce96b2f 86
elab 0:0e577ce96b2f 87 bool GetNewTOF()
elab 0:0e577ce96b2f 88 { uint32_t NewDist_mm;
elab 0:0e577ce96b2f 89 TOFError = TOF1.get_measurement(range_continuous_polling, &RangeResults);
elab 0:0e577ce96b2f 90 if ( (TOFError == VL53L0X_ERROR_NONE) && (RangeResults.RangeStatus == 0) )
elab 0:0e577ce96b2f 91 { // we have a valid range.
elab 0:0e577ce96b2f 92 // Report_Range_Infos( RangeResults );
elab 0:0e577ce96b2f 93 if (ShowDataLog) pc_uart.printf("Dist=%3d, SigRate=%4.2fM/s, AmbRate=%4.2fM/s, SpadRtnCnt=%3.1f, SigmEst=%3.3f\n\r",
elab 0:0e577ce96b2f 94 RangeResults.Range_mm, RangeResults.SignalRateRtn_MHz/65536.1, RangeResults.AmbientRateRtn_MHz/65536.1,
elab 0:0e577ce96b2f 95 RangeResults.EffectiveSpadRtnCount/256.1, RangeResults.SigmaEstimate/65536.1);
elab 0:0e577ce96b2f 96 //Report_Deep_Infos(TOF1);
elab 0:0e577ce96b2f 97 NewDist_mm = RangeResults.Range_mm;
elab 0:0e577ce96b2f 98 // pc_uart.printf("%6ld\r", NewDist_mm); // Distance is ~1700 when looking at the ceiling, so approximately 1 per mm. A really usable distance range is only up to 1270.
elab 0:0e577ce96b2f 99
elab 0:0e577ce96b2f 100 if (TOF_Dist_mm == out_of_range) // previous value was useless
elab 0:0e577ce96b2f 101 { TOF_Dist_mm = NewDist_mm; } // only take the new value
elab 0:0e577ce96b2f 102 else // average old with new value!!
elab 0:0e577ce96b2f 103 { TOF_Dist_mm = ( NewDist_mm * 2 + TOF_Dist_mm * 3 ) / 5 ;
elab 0:0e577ce96b2f 104 if (TOF_Dist_mm < TOF_MinDist_mm) { TOF_MinDist_mm = TOF_Dist_mm; pc_uart.printf("DMin = %3d. \r\n", TOF_MinDist_mm);} // adapt the min dist parameter
elab 0:0e577ce96b2f 105
elab 0:0e577ce96b2f 106 // The device theoretical working distance range is from 0 up to 1270mm, distance is always expressed in mm!
elab 0:0e577ce96b2f 107 // However here we transform to a relative number, of which only the range 0 ... 1 should be used.
elab 0:0e577ce96b2f 108 TOF_RelDist = ( (double) TOF_Dist_mm - TOF_MinDist_mm) / TOF_UseRange_mm;
elab 0:0e577ce96b2f 109 if (TOF_RelDist <0) {TOF_RelDist = 0;} // clamping values <0 (that should actually never occur with above adaption of Min dist!, values >1 can still happen!
elab 0:0e577ce96b2f 110 }
elab 0:0e577ce96b2f 111 return true; // new measurement is available
elab 0:0e577ce96b2f 112 }
elab 0:0e577ce96b2f 113 else
elab 0:0e577ce96b2f 114 { // pc_uart.printf(" --\r");
elab 0:0e577ce96b2f 115 TOF_Dist_mm = out_of_range;
elab 0:0e577ce96b2f 116 TOF_RelDist = out_of_range;
elab 0:0e577ce96b2f 117 return false; // no new measurement is available
elab 0:0e577ce96b2f 118 }
elab 0:0e577ce96b2f 119 }
elab 0:0e577ce96b2f 120
elab 0:0e577ce96b2f 121
elab 0:0e577ce96b2f 122
elab 0:0e577ce96b2f 123 void help() // Display list of Commands
elab 0:0e577ce96b2f 124 {
elab 0:0e577ce96b2f 125 DEBUG("List of commands:\n\r");
elab 0:0e577ce96b2f 126 DEBUG(" h --> Help, display list of commands\n\r");
elab 0:0e577ce96b2f 127 }
elab 0:0e577ce96b2f 128
elab 0:0e577ce96b2f 129 void callback()
elab 0:0e577ce96b2f 130 {
elab 0:0e577ce96b2f 131 switch(state) {
elab 0:0e577ce96b2f 132 case 0:
elab 0:0e577ce96b2f 133 break;
elab 0:0e577ce96b2f 134 case 1:
elab 0:0e577ce96b2f 135 break;
elab 0:0e577ce96b2f 136 case 2:
elab 0:0e577ce96b2f 137 break;
elab 0:0e577ce96b2f 138 case 3:
elab 0:0e577ce96b2f 139 break;
elab 0:0e577ce96b2f 140 default:
elab 0:0e577ce96b2f 141 }
elab 0:0e577ce96b2f 142 state=state+1;
elab 0:0e577ce96b2f 143 }
elab 0:0e577ce96b2f 144
elab 0:0e577ce96b2f 145 /* Stop all processes */
elab 0:0e577ce96b2f 146 void stop_all()
elab 0:0e577ce96b2f 147 {
elab 0:0e577ce96b2f 148 mybot.stopMove();
elab 0:0e577ce96b2f 149 }
elab 0:0e577ce96b2f 150
elab 0:0e577ce96b2f 151 void clicked()
elab 0:0e577ce96b2f 152 {
elab 0:0e577ce96b2f 153 DEBUG("Labyrinthe\n\r");
elab 0:0e577ce96b2f 154 commandRECEIVED = 'l';
elab 0:0e577ce96b2f 155 receivedCOMMAND = true;
elab 0:0e577ce96b2f 156 }
elab 0:0e577ce96b2f 157
elab 0:0e577ce96b2f 158 void labyrinthe()
elab 0:0e577ce96b2f 159 {
elab 0:0e577ce96b2f 160 wait(1);
elab 0:0e577ce96b2f 161 mybot.move(FORWARD,15);
elab 0:0e577ce96b2f 162 mybot.waitEndMove();
elab 0:0e577ce96b2f 163 mybot.move(ROTATE, 90);
elab 0:0e577ce96b2f 164 mybot.waitEndMove();
elab 0:0e577ce96b2f 165 mybot.move(FORWARD,15);
elab 0:0e577ce96b2f 166 mybot.waitEndMove();
elab 0:0e577ce96b2f 167 mybot.move(ROTATE, 90);
elab 0:0e577ce96b2f 168 mybot.waitEndMove();
elab 0:0e577ce96b2f 169 }
elab 0:0e577ce96b2f 170
elab 0:0e577ce96b2f 171 void executeCommand(char c, int parameter)
elab 0:0e577ce96b2f 172 {
elab 0:0e577ce96b2f 173 bool flaghelp = false;
elab 0:0e577ce96b2f 174 pc_uart.printf("hffehihgiehighegieighieughei\n");
elab 0:0e577ce96b2f 175 switch (c) {
elab 0:0e577ce96b2f 176 case 'h':
elab 0:0e577ce96b2f 177 help();
elab 0:0e577ce96b2f 178 state=0;
elab 0:0e577ce96b2f 179 flaghelp=true;
elab 0:0e577ce96b2f 180 CASE('a', "Avance <cm> (a <cm>) => Avance du nombre de cm indiques", mybot.move(FORWARD,parameter); )
elab 0:0e577ce96b2f 181 CASE('r', "Recule <cm> (r <cm>) => Recule du nombre de cm indiques", mybot.move(BACKWARD,parameter); )
elab 0:0e577ce96b2f 182 CASE('d', "Droite <deg> (d <deg>) => Tourne a droite du nombre de degres indiques", mybot.move(RIGHT,parameter,0); )
elab 0:0e577ce96b2f 183 CASE('g', "Gauche <deg> (g <deg>) => Tourne a gauche du nombre de degres indiques", mybot.move(LEFT,parameter,0); )
elab 0:0e577ce96b2f 184 CASE('p', "Pivote_d <deh> (p <deg>) => pivote a droite du nombre de degres indiques", mybot.move(ROTATE,parameter); )
elab 0:0e577ce96b2f 185 CASE('q', "Pivote_g <deg> (q <deg>) => pivote a gauche du nombre de degres indiques", mybot.move(ROTATE,-parameter); )
elab 0:0e577ce96b2f 186 CASE('s', "Stop => Arrete les moteurs", mybot.stopMove(); state=0; )
elab 0:0e577ce96b2f 187 CASE('l', "Labyrinthe => Lance le parcours Labyrinthe", labyrinthe(); )
elab 0:0e577ce96b2f 188 default :
elab 0:0e577ce96b2f 189 DEBUG("invalid command; use: 'h' for help()\n\r");
elab 0:0e577ce96b2f 190 state=0;
elab 0:0e577ce96b2f 191 }
elab 0:0e577ce96b2f 192 }
elab 0:0e577ce96b2f 193
elab 0:0e577ce96b2f 194 void analyseCommand(char *command, int parameter)
elab 0:0e577ce96b2f 195 {
elab 0:0e577ce96b2f 196 parameterRECEIVED = parameter;
elab 0:0e577ce96b2f 197 switch(command[0]) {
elab 0:0e577ce96b2f 198 case 'A':
elab 0:0e577ce96b2f 199 case 'a':
elab 0:0e577ce96b2f 200 commandRECEIVED = 'a';
elab 0:0e577ce96b2f 201 break;
elab 0:0e577ce96b2f 202 case 'R':
elab 0:0e577ce96b2f 203 case 'r':
elab 0:0e577ce96b2f 204 commandRECEIVED = 'r';
elab 0:0e577ce96b2f 205 break;
elab 0:0e577ce96b2f 206 case 'D':
elab 0:0e577ce96b2f 207 case 'd':
elab 0:0e577ce96b2f 208 commandRECEIVED = 'd';
elab 0:0e577ce96b2f 209 break;
elab 0:0e577ce96b2f 210 case 'G':
elab 0:0e577ce96b2f 211 case 'g':
elab 0:0e577ce96b2f 212 commandRECEIVED = 'g';
elab 0:0e577ce96b2f 213 break;
elab 0:0e577ce96b2f 214 case 'L':
elab 0:0e577ce96b2f 215 case 'l':
elab 0:0e577ce96b2f 216 commandRECEIVED = 'l';
elab 0:0e577ce96b2f 217 break;
elab 0:0e577ce96b2f 218 case 'P':
elab 0:0e577ce96b2f 219 if(command[7]=='d') {
elab 0:0e577ce96b2f 220 commandRECEIVED = 'p';
elab 0:0e577ce96b2f 221 } else if (command[7]=='g') {
elab 0:0e577ce96b2f 222 commandRECEIVED = 'q';
elab 0:0e577ce96b2f 223 } else {
elab 0:0e577ce96b2f 224 commandRECEIVED = 'h';
elab 0:0e577ce96b2f 225 }
elab 0:0e577ce96b2f 226 break;
elab 0:0e577ce96b2f 227 case 'p':
elab 0:0e577ce96b2f 228 commandRECEIVED = 'p';
elab 0:0e577ce96b2f 229 break;
elab 0:0e577ce96b2f 230 case 'q':
elab 0:0e577ce96b2f 231 commandRECEIVED = 'q';
elab 0:0e577ce96b2f 232 break;
elab 0:0e577ce96b2f 233 case 'S':
elab 0:0e577ce96b2f 234 case 's':
elab 0:0e577ce96b2f 235 commandRECEIVED = 's';
elab 0:0e577ce96b2f 236 mybot.stopMove();
elab 0:0e577ce96b2f 237 state=0;
elab 0:0e577ce96b2f 238 break;
elab 0:0e577ce96b2f 239 default:
elab 0:0e577ce96b2f 240 commandRECEIVED = 'h';
elab 0:0e577ce96b2f 241 }
elab 0:0e577ce96b2f 242 }
elab 0:0e577ce96b2f 243
elab 0:0e577ce96b2f 244 void checkCommand(int result, char *command, int parameter)
elab 0:0e577ce96b2f 245 {
elab 0:0e577ce96b2f 246 if(result==2 || command[0]=='h' || command[0]=='s') {
elab 0:0e577ce96b2f 247 if(command[0]=='c') {
elab 0:0e577ce96b2f 248 DEBUG("CREABOT PRESENT\n\r");
elab 0:0e577ce96b2f 249 } else {
elab 0:0e577ce96b2f 250 analyseCommand(command, parameter);
elab 0:0e577ce96b2f 251 receivedCOMMAND = true;
elab 0:0e577ce96b2f 252 }
elab 0:0e577ce96b2f 253 }
elab 0:0e577ce96b2f 254 }
elab 0:0e577ce96b2f 255
elab 0:0e577ce96b2f 256 void split(char *line, int length)
elab 0:0e577ce96b2f 257 {
elab 0:0e577ce96b2f 258 char command[256];
elab 0:0e577ce96b2f 259 int parameter=0;
elab 0:0e577ce96b2f 260 int result = 1;
elab 0:0e577ce96b2f 261 int i=0;
elab 0:0e577ce96b2f 262 int j=0;
elab 0:0e577ce96b2f 263 while(i<length && line[i]==' ') {
elab 0:0e577ce96b2f 264 i++;
elab 0:0e577ce96b2f 265 }
elab 0:0e577ce96b2f 266 while(i<length && line[i]!=' ') {
elab 0:0e577ce96b2f 267 command[j]=line[i];
elab 0:0e577ce96b2f 268 i++;
elab 0:0e577ce96b2f 269 j++;
elab 0:0e577ce96b2f 270 }
elab 0:0e577ce96b2f 271 command[j]=0;
elab 0:0e577ce96b2f 272 i++;
elab 0:0e577ce96b2f 273 j=0;
elab 0:0e577ce96b2f 274 while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
elab 0:0e577ce96b2f 275 i++;
elab 0:0e577ce96b2f 276 j++;
elab 0:0e577ce96b2f 277 }
elab 0:0e577ce96b2f 278 if(j>0) {
elab 0:0e577ce96b2f 279 result++;
elab 0:0e577ce96b2f 280 i--;
elab 0:0e577ce96b2f 281 int k=1;
elab 0:0e577ce96b2f 282 while(j>0) {
elab 0:0e577ce96b2f 283 parameter += (line[i]-'0')*k;
elab 0:0e577ce96b2f 284 j--;
elab 0:0e577ce96b2f 285 i--;
elab 0:0e577ce96b2f 286 k=k*10;
elab 0:0e577ce96b2f 287 }
elab 0:0e577ce96b2f 288 }
elab 0:0e577ce96b2f 289 checkCommand(result, command, parameter);
elab 0:0e577ce96b2f 290 }
elab 0:0e577ce96b2f 291
elab 0:0e577ce96b2f 292 void storeC(char c)
elab 0:0e577ce96b2f 293 {
elab 0:0e577ce96b2f 294 if(c==10 || c==13|| commandPosition >= 254) {
elab 0:0e577ce96b2f 295 split(commandLine, commandPosition);
elab 0:0e577ce96b2f 296 commandPosition=0;
elab 0:0e577ce96b2f 297 } else {
elab 0:0e577ce96b2f 298 commandLine[commandPosition++]=c;
elab 0:0e577ce96b2f 299 commandLine[commandPosition]=0;
elab 0:0e577ce96b2f 300 }
elab 0:0e577ce96b2f 301 }
elab 0:0e577ce96b2f 302
elab 0:0e577ce96b2f 303 void getBT()
elab 0:0e577ce96b2f 304 {
elab 0:0e577ce96b2f 305 char c = bt_uart.getc();
elab 0:0e577ce96b2f 306 storeC(c);
elab 0:0e577ce96b2f 307 }
elab 0:0e577ce96b2f 308
elab 0:0e577ce96b2f 309 void getPC()
elab 0:0e577ce96b2f 310 {
elab 0:0e577ce96b2f 311 char c = pc_uart.getc();
elab 0:0e577ce96b2f 312 storeC(c);
elab 0:0e577ce96b2f 313 }
elab 0:0e577ce96b2f 314
elab 0:0e577ce96b2f 315 void endOfMove(int status)
elab 0:0e577ce96b2f 316 {
elab 0:0e577ce96b2f 317 DEBUG("ENDOFMOVE\n\r");
elab 0:0e577ce96b2f 318 state=0;
elab 0:0e577ce96b2f 319 }
elab 0:0e577ce96b2f 320 void demoMode(void)
elab 0:0e577ce96b2f 321 {
elab 0:0e577ce96b2f 322 init_TOF();
elab 0:0e577ce96b2f 323 TOF_Dist_mm=200;
elab 0:0e577ce96b2f 324 while(1){
elab 0:0e577ce96b2f 325 if(GetNewTOF()){
elab 0:0e577ce96b2f 326 // pc_uart.printf("\n\rDistance : %d mm\n\r",TOF_Dist_mm);
elab 0:0e577ce96b2f 327 // DEBUG("\n\rDistance : %d mm\n\r",TOF_Dist_mm)
elab 0:0e577ce96b2f 328 }
elab 0:0e577ce96b2f 329
elab 0:0e577ce96b2f 330 if (TOF_Dist_mm < 60 & TOF_Dist_mm > 0){
elab 0:0e577ce96b2f 331 mybot.move(BACKWARD,5);
elab 0:0e577ce96b2f 332 mybot.waitEndMove(5000000); // 5 seconds
elab 0:0e577ce96b2f 333 mybot.move(ROTATE,-90);
elab 0:0e577ce96b2f 334 mybot.waitEndMove(5000000); // 5 seconds
elab 0:0e577ce96b2f 335 }
elab 0:0e577ce96b2f 336 else{
elab 0:0e577ce96b2f 337 mybot.move(FORWARD,5);
elab 0:0e577ce96b2f 338 // mybot.waitEndMove(5000000); // 5 seconds
elab 0:0e577ce96b2f 339 }
elab 0:0e577ce96b2f 340 }
elab 0:0e577ce96b2f 341 }
elab 0:0e577ce96b2f 342 /* Routine */
elab 0:0e577ce96b2f 343 int main()
elab 0:0e577ce96b2f 344 {
elab 0:0e577ce96b2f 345 /* Rename HC-C6 BT*/
elab 0:0e577ce96b2f 346
elab 0:0e577ce96b2f 347 commandPosition=0;
elab 0:0e577ce96b2f 348 pc_uart.attach(getPC);
elab 0:0e577ce96b2f 349 bt_uart.attach(getBT);
elab 0:0e577ce96b2f 350
elab 0:0e577ce96b2f 351 mybot.setCallBack(endOfMove);
elab 0:0e577ce96b2f 352 mybot.setSpeed(6.0); // max 6.9 cm.s, average 5 cm.s
elab 0:0e577ce96b2f 353 state=0;
elab 0:0e577ce96b2f 354 receivedCOMMAND = false;
elab 0:0e577ce96b2f 355 DEBUG("CREABOT alpha version\n\r");
elab 0:0e577ce96b2f 356 __disable_irq();
elab 0:0e577ce96b2f 357 pc_uart.printf("Init Done ...\n\r");
elab 0:0e577ce96b2f 358 __enable_irq();
elab 0:0e577ce96b2f 359 pc_uart.printf("start passed ok\n");
elab 0:0e577ce96b2f 360
elab 0:0e577ce96b2f 361
elab 0:0e577ce96b2f 362 ledBand.SetColor(BLUE,0);
elab 0:0e577ce96b2f 363 ledBand.SetColor(BLUE,1);
elab 0:0e577ce96b2f 364
elab 0:0e577ce96b2f 365 isCmd.mode(PullUp);
elab 0:0e577ce96b2f 366 pc_uart.printf("switchState=%d\n\r", isCmd.read());
elab 0:0e577ce96b2f 367 if (isCmd.read()==0){
elab 0:0e577ce96b2f 368 demoMode();
elab 0:0e577ce96b2f 369 }
elab 0:0e577ce96b2f 370 while(1) {
elab 0:0e577ce96b2f 371 if(state==0 && receivedCOMMAND) {
elab 0:0e577ce96b2f 372 receivedCOMMAND = false;
elab 0:0e577ce96b2f 373 state=1;
elab 0:0e577ce96b2f 374 pc_uart.printf("tototo\n");
elab 0:0e577ce96b2f 375 executeCommand(commandRECEIVED, parameterRECEIVED);
elab 0:0e577ce96b2f 376 }
elab 0:0e577ce96b2f 377 wait(0.1);
elab 0:0e577ce96b2f 378 }
elab 0:0e577ce96b2f 379 }