CreaLab
Main robot of the 2019 MJCup
Dependencies: LED_WS2812 mbed X_NUCLEO_IHM02A1
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 00002 * MJBot : version 2019 00003 * 00004 */ 00005 00006 /* Includes ------------------------------------------------------------------*/ 00007 00008 #include "mbed.h" 00009 #include "DevSPI.h" 00010 #include "x_nucleo_ihm02a1_class.h" 00011 #undef printf 00012 #include "Robot.h" 00013 00014 Serial bt_uart(PA_9, PA_10); // PA9 = Tx, PA10 = Rx 00015 Serial pc_uart(USBTX, USBRX); // USBTX = PA2, USBRX = PA3 00016 00017 /* Definitions ---------------------------------------------------------------*/ 00018 00019 /* Number of movements per revolution. */ 00020 #define MPR_1 4 00021 00022 /* Number of steps. */ 00023 #define STEPS_1 (40000 * 128) /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */ 00024 #define STEPS_2 (STEPS_1 * 2) 00025 00026 /* Delay in milliseconds. */ 00027 #define DELAY_1 1000 00028 #define DELAY_2 2000 00029 #define DELAY_3 5000 00030 00031 #define MID 1500 00032 #define MIN 800 // 750 00033 #define MAX 2200 // 2250 00034 00035 bool receivedCOMMAND; 00036 char commandRECEIVED; 00037 int parameterRECEIVED; 00038 int state; 00039 char commandLine[256]; 00040 int commandPosition; 00041 int action; 00042 int pince; 00043 int repos; 00044 int inter; 00045 int attente; 00046 int trigger; 00047 int Bot; // MJBot1 = 0, MJBot2 = 1 and STBot = 2 00048 00049 LED_WS2812 ledBand(PB_10,4); 00050 PwmOut myservo(PB_4); 00051 DigitalOut ventilo(PB_0); 00052 00053 // Buzzer buzzer(PB_0); 00054 00055 /* Variables -----------------------------------------------------------------*/ 00056 00057 /* Motor Control Expansion Board. */ 00058 X_NUCLEO_IHM02A1 *x_nucleo_ihm02a1; 00059 00060 /* Initialization parameters of the motors connected to the expansion board. */ 00061 L6470_Init_t init[L6470DAISYCHAINSIZE] = 00062 { 00063 /* First Motor. */ 00064 { 00065 12.0, /* Motor supply voltage in V. */ 00066 200, /* Min number of steps per revolution for the motor. */ 00067 0.9, /* Max motor phase voltage in A. */ 00068 3.0, /* Max motor phase voltage in V. */ 00069 50.0, /* Motor initial speed [step/s]. */ 00070 100.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */ 00071 100.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */ 00072 200.0, /* Motor maximum speed [step/s]. */ 00073 0.0, /* Motor minimum speed [step/s]. */ 00074 490.0, /* Motor full-step speed threshold [step/s]. */ 00075 3.06, /* Holding kval [V]. */ 00076 3.06, /* Constant speed kval [V]. */ 00077 3.06, /* Acceleration starting kval [V]. */ 00078 3.06, /* Deceleration starting kval [V]. */ 00079 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */ 00080 392.1569e-6, /* Start slope [s/step]. */ 00081 643.1372e-6, /* Acceleration final slope [s/step]. */ 00082 643.1372e-6, /* Deceleration final slope [s/step]. */ 00083 0, /* Thermal compensation factor (range [0, 15]). */ 00084 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */ 00085 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */ 00086 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */ 00087 0x00, /* Alarm conditions enable. */ 00088 0x2E88 /* Ic configuration. */ 00089 }, 00090 00091 /* Second Motor. */ 00092 { 00093 12, /* Motor supply voltage in V. */ 00094 200, /* Min number of steps per revolution for the motor. */ 00095 0.9, /* Max motor phase voltage in A. */ 00096 3.0, /* Max motor phase voltage in V. */ 00097 50.0, /* Motor initial speed [step/s]. */ 00098 100.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */ 00099 100.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */ 00100 200.0, /* Motor maximum speed [step/s]. */ 00101 0.0, /* Motor minimum speed [step/s]. */ 00102 490.0, /* Motor full-step speed threshold [step/s]. */ 00103 3.06, /* Holding kval [V]. */ 00104 3.06, /* Constant speed kval [V]. */ 00105 3.06, /* Acceleration starting kval [V]. */ 00106 3.06, /* Deceleration starting kval [V]. */ 00107 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */ 00108 392.1569e-6, /* Start slope [s/step]. */ 00109 643.1372e-6, /* Acceleration final slope [s/step]. */ 00110 643.1372e-6, /* Deceleration final slope [s/step]. */ 00111 0, /* Thermal compensation factor (range [0, 15]). */ 00112 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */ 00113 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */ 00114 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */ 00115 0x00, /* Alarm conditions enable. */ 00116 0x2E88 /* Ic configuration. */ 00117 } 00118 }; 00119 00120 void help() // Display list of Commands 00121 { 00122 DEBUG("List of commands:\n\r"); 00123 DEBUG(" h --> Help, display list of commands\n\r"); 00124 } 00125 00126 void executeCommand(char c) { 00127 bool flaghelp = false; 00128 switch (c) { 00129 case 'h': 00130 help(); 00131 action=0; 00132 flaghelp=true; 00133 CASE('a', "Avance", action=2; ) 00134 CASE('r', "Recule", action=3; ) 00135 CASE('d', "Droite AV", action=4; ) 00136 CASE('g', "Gauche AV", action=5; ) 00137 CASE('p', "Pivote D", action=6; ) 00138 CASE('q', "Pivote G", action=7; ) 00139 CASE('s', "Stop", action=1; ) 00140 CASE('n', "Debrayer", action=9; ) 00141 CASE('k', "Pince haute", action=10; ) 00142 CASE('j', "Pince basse", action=11; ) 00143 CASE('b', "Ventilo On", action=12; ) 00144 CASE('l', "Ventilo Off", action=13; ) 00145 CASE('c', "LEDs ON", action=14; ) 00146 CASE('x', "LEDs OFF", action=15; ) 00147 CASE('y', "Pince haute/basse", action=16; ) 00148 CASE('D', "Droite AR", action=17; ) 00149 CASE('G', "Gauche AR", action=18; ) 00150 CASE('t', "Ventilo ON/OFF", action=19; ) 00151 00152 default : 00153 DEBUG("invalid command; use: 'h' for help()\n\r"); 00154 action=0; 00155 }} 00156 00157 void analyseCommand(char *command) { 00158 switch(command[0]) { 00159 case 'a': 00160 commandRECEIVED = 'a'; 00161 break; 00162 case 'r': 00163 commandRECEIVED = 'r'; 00164 break; 00165 case 'd': 00166 commandRECEIVED = 'd'; 00167 break; 00168 case 'g': 00169 commandRECEIVED = 'g'; 00170 break; 00171 case 'D': 00172 commandRECEIVED = 'D'; 00173 break; 00174 case 'G': 00175 commandRECEIVED = 'G'; 00176 break; 00177 case 's': 00178 commandRECEIVED = 's'; 00179 break; 00180 case 'p': 00181 commandRECEIVED = 'p'; 00182 break; 00183 case 'q': 00184 commandRECEIVED = 'q'; 00185 break; 00186 case 'n': 00187 commandRECEIVED = 'n'; 00188 break; 00189 case 'k': 00190 commandRECEIVED = 'k'; 00191 break; 00192 case 'j': 00193 commandRECEIVED = 'j'; 00194 break; 00195 case 'b': 00196 commandRECEIVED = 'b'; 00197 break; 00198 case 'l': 00199 commandRECEIVED = 'l'; 00200 break; 00201 case 'c': 00202 commandRECEIVED = 'c'; 00203 break; 00204 case 'x': 00205 commandRECEIVED = 'x'; 00206 break; 00207 case 'y': 00208 commandRECEIVED = 'y'; 00209 break; 00210 case 't': 00211 commandRECEIVED = 't'; 00212 break; 00213 00214 default: 00215 commandRECEIVED = 'h'; 00216 } } 00217 00218 void checkCommand(int result, char *command) { 00219 if(result==1) { 00220 analyseCommand(command); 00221 // DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED, parameterRECEIVED,state); 00222 receivedCOMMAND = true; 00223 } } 00224 00225 void split(char *line, int length) { 00226 char command[256]; 00227 int parameter=0; 00228 int result = 1; 00229 int i=0; 00230 int j=0; 00231 while(i<length && line[i]==' ') { 00232 i++;} 00233 while(i<length && line[i]!=' ') { 00234 command[j]=line[i]; 00235 i++; 00236 j++;} 00237 command[j]=0; 00238 i++; 00239 j=0; 00240 while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') { 00241 i++; 00242 j++;} 00243 if(j>0) { 00244 result++; 00245 i--; 00246 int k=1; 00247 while(j>0) { 00248 parameter += (line[i]-'0')*k; 00249 j--; 00250 i--; 00251 k=k*10;} 00252 } 00253 checkCommand(result, command); 00254 } 00255 00256 void storeC(char c) { 00257 if(c==10 || c==13|| commandPosition >= 255) { 00258 split(commandLine, commandPosition); 00259 commandPosition=0;} 00260 else { 00261 commandLine[commandPosition++]=c; 00262 commandLine[commandPosition]=0;} 00263 } 00264 00265 void getBT() { 00266 char c = bt_uart.getc(); 00267 storeC(c); 00268 } 00269 00270 void getPC() { 00271 char c = pc_uart.getc(); 00272 storeC(c); 00273 } 00274 00275 void leda(int parameter) { 00276 ledBand.SetColor(WHITE,0) ; 00277 ledBand.SetColor(RED,1) ; 00278 ledBand.SetColor(BLUE,2) ; 00279 ledBand.SetColor(GREEN,3) ; 00280 if (parameter == 0) { 00281 ledBand.StopRotation(); 00282 // ledBand.StopBlink() ; 00283 ledBand.SetColor(BLACK,0) ; 00284 ledBand.SetColor(BLACK,1) ; 00285 ledBand.SetColor(BLACK,2) ; 00286 ledBand.SetColor(BLACK,3) ;} 00287 if (parameter == 1) { 00288 ledBand.SetColor(RED,0) ; 00289 ledBand.SetColor(RED,1) ; 00290 ledBand.SetColor(RED,2) ; 00291 ledBand.SetColor(RED,3);} 00292 if (parameter == 2) {ledBand.StartRotation(0.6);} 00293 if (parameter == 3) {ledBand.StartRotation(1);} 00294 state=0; 00295 } 00296 00297 /* Main ----------------------------------------------------------------------*/ 00298 00299 int main() 00300 { 00301 /*----- Initialization. -----*/ 00302 wait(1); 00303 // bt_uart.printf("AT+NAMEPETITBULLES"); // Changement nom module BT 00304 // wait(2); 00305 00306 /* Initializing SPI bus. */ 00307 DevSPI dev_spi(D11, D12, D3); 00308 /* Initializing Motor Control Expansion Board. */ 00309 x_nucleo_ihm02a1 = new X_NUCLEO_IHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi); 00310 /* Building a list of motor control components. */ 00311 L6470 **motors = x_nucleo_ihm02a1->GetComponents(); 00312 00313 pc_uart.printf("MJBot2 \r\n\n"); // Printing to the console 00314 commandPosition=0; 00315 bt_uart.attach(getBT); 00316 pc_uart.attach(getPC); 00317 state=0; 00318 action=0; 00319 pince=0; 00320 inter=0; 00321 repos=0; 00322 attente=0; // temps d'inactivité avant débrayage 00323 trigger=0; // est à 1 si Stop envoyé une seule fois 00324 Bot=0; // MJBot Tullins = 0, MJBot R2D2 = 1 and STBot = 2 00325 ventilo=0; 00326 myservo.period_ms(20); 00327 myservo.pulsewidth_us(MID); 00328 receivedCOMMAND = false; 00329 while(1) { 00330 if(receivedCOMMAND) { 00331 receivedCOMMAND = false; 00332 executeCommand(commandRECEIVED); 00333 if (action>=1) { 00334 if (action==1) { 00335 if (repos==1) { 00336 pc_uart.printf("stop hard \r\n\n"); 00337 motors[0]->PrepareHardStop(); 00338 motors[1]->PrepareHardStop(); 00339 repos=0; 00340 attente=0; 00341 trigger=1; 00342 inter=0; } 00343 else { 00344 pc_uart.printf("stop debraye \r\n\n"); 00345 motors[0]->PrepareSoftHiZ(); 00346 motors[1]->PrepareSoftHiZ(); 00347 trigger=0;} 00348 x_nucleo_ihm02a1->PerformPreparedActions();} 00349 if (action==2) { 00350 pc_uart.printf("Avance \r\n\n"); 00351 motors[0]->PrepareRun(StepperMotor::BWD, 150); 00352 motors[1]->PrepareRun(StepperMotor::FWD, 150); 00353 x_nucleo_ihm02a1->PerformPreparedActions(); 00354 repos=1; } 00355 if (action==3) { 00356 pc_uart.printf("Recule \r\n\n"); 00357 // motors[0]->Run(StepperMotor::BWD, 200); 00358 // motors[1]->Run(StepperMotor::FWD, 200);} 00359 motors[0]->PrepareRun(StepperMotor::FWD, 150); 00360 motors[1]->PrepareRun(StepperMotor::BWD, 150); 00361 x_nucleo_ihm02a1->PerformPreparedActions(); 00362 repos=1; } 00363 if (action==4) { 00364 pc_uart.printf("droite AV \r\n\n"); 00365 // motors[0]->Run(StepperMotor::FWD, 100); 00366 motors[0]->PrepareHardStop(); 00367 motors[1]->PrepareRun(StepperMotor::FWD, 150); 00368 x_nucleo_ihm02a1->PerformPreparedActions(); 00369 repos=1; } 00370 if (action==5) { 00371 pc_uart.printf("gauche AV \r\n\n"); 00372 motors[0]->PrepareRun(StepperMotor::BWD, 150); 00373 motors[1]->PrepareHardStop(); 00374 x_nucleo_ihm02a1->PerformPreparedActions(); 00375 repos=1; } 00376 if (action==17) { 00377 pc_uart.printf("droite AR \r\n\n"); 00378 motors[0]->PrepareHardStop(); 00379 motors[1]->PrepareRun(StepperMotor::BWD, 150); 00380 x_nucleo_ihm02a1->PerformPreparedActions(); 00381 repos=1; } 00382 if (action==18) { 00383 pc_uart.printf("gauche AR \r\n\n"); 00384 motors[0]->PrepareRun(StepperMotor::FWD, 150); 00385 motors[1]->PrepareHardStop(); 00386 x_nucleo_ihm02a1->PerformPreparedActions(); 00387 repos=1; } 00388 if (action==6) { 00389 pc_uart.printf("pivote d \r\n\n"); 00390 motors[0]->PrepareRun(StepperMotor::FWD, 75); 00391 motors[1]->PrepareRun(StepperMotor::FWD, 75); 00392 x_nucleo_ihm02a1->PerformPreparedActions(); 00393 repos=1; } 00394 if (action==7) { 00395 pc_uart.printf("pivote g\r\n\n"); 00396 motors[0]->PrepareRun(StepperMotor::BWD, 75); 00397 motors[1]->PrepareRun(StepperMotor::BWD, 75); 00398 x_nucleo_ihm02a1->PerformPreparedActions(); 00399 repos=1; } 00400 // motors[0]->Run(StepperMotor::FWD, 200); 00401 // motors[1]->Run(StepperMotor::FWD, 200); } 00402 if (action==9) { 00403 pc_uart.printf("debrayer \r\n\n"); 00404 motors[0]->PrepareSoftHiZ(); 00405 motors[1]->PrepareSoftHiZ(); 00406 x_nucleo_ihm02a1->PerformPreparedActions(); 00407 repos=0; 00408 trigger=0; 00409 attente=0;} 00410 if (action==10) { 00411 pc_uart.printf("pince haute \r\n\n"); 00412 myservo.pulsewidth_us(MIN);} 00413 if (action==11) { 00414 pc_uart.printf("pince basse \r\n\n"); 00415 myservo.pulsewidth_us(MAX);} 00416 if (action==12) { 00417 pc_uart.printf("Ventilo On \r\n\n"); 00418 ventilo=1; 00419 } 00420 if (action==13) { 00421 pc_uart.printf("Ventilo Off \r\n\n"); 00422 ventilo=0; 00423 } 00424 if (action==19) { 00425 pc_uart.printf("Ventilo ON/OFF \r\n\n"); 00426 if (ventilo==0) { 00427 ventilo=1;} 00428 else {ventilo=0;}} 00429 if (action==14) { 00430 pc_uart.printf("LEDs ON \r\n\n"); 00431 leda(3);} 00432 if (action==15) { 00433 pc_uart.printf("LEDs OFF \r\n\n"); 00434 leda(0);} 00435 if (action==16) { 00436 pc_uart.printf("pince haute/basse \r\n\n"); 00437 if (pince==0) { 00438 leda(1); 00439 inter=0; 00440 myservo.pulsewidth_us(MIN); 00441 pince=1;} 00442 else { 00443 myservo.pulsewidth_us(MAX); 00444 leda(2); 00445 pince=0;} 00446 00447 action=0; 00448 } } 00449 wait(0.1); 00450 attente++; 00451 if (attente>50 && repos==0 && trigger==1) { 00452 pc_uart.printf("attente OK\r\n\n"); 00453 motors[0]->PrepareSoftHiZ(); 00454 motors[1]->PrepareSoftHiZ(); 00455 x_nucleo_ihm02a1->PerformPreparedActions(); 00456 attente=0; 00457 trigger=0; } 00458 } } }
Generated on Mon Aug 1 2022 15:36:04 by
1.7.2