eLab Team / Mbed 2 deprecated myRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed WS2812

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