CreaLab / Mbed 2 deprecated MJBot2

Main robot of the 2019 MJCup

Dependencies:   LED_WS2812 mbed X_NUCLEO_IHM02A1

main.cpp@22:82611fe41c5c, 2019-09-22 (annotated)

Committer:
alcocerg
Date:
Sun Sep 22 06:21:26 2019 +0000
Revision:
22:82611fe41c5c
Parent:
18:591a007effc2 
First version of the main robot of the 2019 MJCup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alcocerg 22:82611fe41c5c 1 /*
alcocerg 22:82611fe41c5c 2 * MJBot : version 2019
alcocerg 22:82611fe41c5c 3 *
Davidroid 0:5148e9486cf2 4 */
alcocerg 22:82611fe41c5c 5
Davidroid 0:5148e9486cf2 6 /* Includes ------------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 7
Davidroid 0:5148e9486cf2 8 #include "mbed.h"
Davidroid 0:5148e9486cf2 9 #include "DevSPI.h"
alcocerg 22:82611fe41c5c 10 #include "x_nucleo_ihm02a1_class.h"
alcocerg 22:82611fe41c5c 11 #undef printf
alcocerg 22:82611fe41c5c 12 #include "Robot.h"
Davidroid 0:5148e9486cf2 13
alcocerg 22:82611fe41c5c 14 Serial bt_uart(PA_9, PA_10); // PA9 = Tx, PA10 = Rx
alcocerg 22:82611fe41c5c 15 Serial pc_uart(USBTX, USBRX); // USBTX = PA2, USBRX = PA3
Davidroid 0:5148e9486cf2 16
Davidroid 0:5148e9486cf2 17 /* Definitions ---------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 18
Davidroid 0:5148e9486cf2 19 /* Number of movements per revolution. */
Davidroid 0:5148e9486cf2 20 #define MPR_1 4
Davidroid 1:9f1974b0960d 21
Davidroid 1:9f1974b0960d 22 /* Number of steps. */
alcocerg 22:82611fe41c5c 23 #define STEPS_1 (40000 * 128) /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
Davidroid 18:591a007effc2 24 #define STEPS_2 (STEPS_1 * 2)
Davidroid 0:5148e9486cf2 25
Davidroid 0:5148e9486cf2 26 /* Delay in milliseconds. */
Davidroid 1:9f1974b0960d 27 #define DELAY_1 1000
Davidroid 0:5148e9486cf2 28 #define DELAY_2 2000
Davidroid 0:5148e9486cf2 29 #define DELAY_3 5000
Davidroid 0:5148e9486cf2 30
alcocerg 22:82611fe41c5c 31 #define MID 1500
alcocerg 22:82611fe41c5c 32 #define MIN 800 // 750
alcocerg 22:82611fe41c5c 33 #define MAX 2200 // 2250
alcocerg 22:82611fe41c5c 34
alcocerg 22:82611fe41c5c 35 bool receivedCOMMAND;
alcocerg 22:82611fe41c5c 36 char commandRECEIVED;
alcocerg 22:82611fe41c5c 37 int parameterRECEIVED;
alcocerg 22:82611fe41c5c 38 int state;
alcocerg 22:82611fe41c5c 39 char commandLine[256];
alcocerg 22:82611fe41c5c 40 int commandPosition;
alcocerg 22:82611fe41c5c 41 int action;
alcocerg 22:82611fe41c5c 42 int pince;
alcocerg 22:82611fe41c5c 43 int repos;
alcocerg 22:82611fe41c5c 44 int inter;
alcocerg 22:82611fe41c5c 45 int attente;
alcocerg 22:82611fe41c5c 46 int trigger;
alcocerg 22:82611fe41c5c 47 int Bot; // MJBot1 = 0, MJBot2 = 1 and STBot = 2
alcocerg 22:82611fe41c5c 48
alcocerg 22:82611fe41c5c 49 LED_WS2812 ledBand(PB_10,4);
alcocerg 22:82611fe41c5c 50 PwmOut myservo(PB_4);
alcocerg 22:82611fe41c5c 51 DigitalOut ventilo(PB_0);
alcocerg 22:82611fe41c5c 52
alcocerg 22:82611fe41c5c 53 // Buzzer buzzer(PB_0);
Davidroid 0:5148e9486cf2 54
Davidroid 0:5148e9486cf2 55 /* Variables -----------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 56
Davidroid 0:5148e9486cf2 57 /* Motor Control Expansion Board. */
Davidroid 0:5148e9486cf2 58 X_NUCLEO_IHM02A1 *x_nucleo_ihm02a1;
Davidroid 0:5148e9486cf2 59
Davidroid 9:f35fbeedb8f4 60 /* Initialization parameters of the motors connected to the expansion board. */
Davidroid 15:698fe51556c0 61 L6470_Init_t init[L6470DAISYCHAINSIZE] =
Davidroid 9:f35fbeedb8f4 62 {
alcocerg 22:82611fe41c5c 63 /* First Motor. */
Davidroid 9:f35fbeedb8f4 64 {
alcocerg 22:82611fe41c5c 65 12.0, /* Motor supply voltage in V. */
alcocerg 22:82611fe41c5c 66 200, /* Min number of steps per revolution for the motor. */
alcocerg 22:82611fe41c5c 67 0.9, /* Max motor phase voltage in A. */
alcocerg 22:82611fe41c5c 68 3.0, /* Max motor phase voltage in V. */
alcocerg 22:82611fe41c5c 69 50.0, /* Motor initial speed [step/s]. */
alcocerg 22:82611fe41c5c 70 100.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
alcocerg 22:82611fe41c5c 71 100.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
alcocerg 22:82611fe41c5c 72 200.0, /* Motor maximum speed [step/s]. */
Davidroid 18:591a007effc2 73 0.0, /* Motor minimum speed [step/s]. */
alcocerg 22:82611fe41c5c 74 490.0, /* Motor full-step speed threshold [step/s]. */
Davidroid 18:591a007effc2 75 3.06, /* Holding kval [V]. */
Davidroid 18:591a007effc2 76 3.06, /* Constant speed kval [V]. */
Davidroid 18:591a007effc2 77 3.06, /* Acceleration starting kval [V]. */
Davidroid 18:591a007effc2 78 3.06, /* Deceleration starting kval [V]. */
Davidroid 18:591a007effc2 79 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Davidroid 18:591a007effc2 80 392.1569e-6, /* Start slope [s/step]. */
Davidroid 18:591a007effc2 81 643.1372e-6, /* Acceleration final slope [s/step]. */
Davidroid 18:591a007effc2 82 643.1372e-6, /* Deceleration final slope [s/step]. */
Davidroid 18:591a007effc2 83 0, /* Thermal compensation factor (range [0, 15]). */
Davidroid 18:591a007effc2 84 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Davidroid 18:591a007effc2 85 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Davidroid 18:591a007effc2 86 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
alcocerg 22:82611fe41c5c 87 0x00, /* Alarm conditions enable. */
Davidroid 18:591a007effc2 88 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 89 },
Davidroid 9:f35fbeedb8f4 90
Davidroid 9:f35fbeedb8f4 91 /* Second Motor. */
Davidroid 9:f35fbeedb8f4 92 {
alcocerg 22:82611fe41c5c 93 12, /* Motor supply voltage in V. */
alcocerg 22:82611fe41c5c 94 200, /* Min number of steps per revolution for the motor. */
alcocerg 22:82611fe41c5c 95 0.9, /* Max motor phase voltage in A. */
alcocerg 22:82611fe41c5c 96 3.0, /* Max motor phase voltage in V. */
alcocerg 22:82611fe41c5c 97 50.0, /* Motor initial speed [step/s]. */
alcocerg 22:82611fe41c5c 98 100.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
alcocerg 22:82611fe41c5c 99 100.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
alcocerg 22:82611fe41c5c 100 200.0, /* Motor maximum speed [step/s]. */
Davidroid 18:591a007effc2 101 0.0, /* Motor minimum speed [step/s]. */
alcocerg 22:82611fe41c5c 102 490.0, /* Motor full-step speed threshold [step/s]. */
Davidroid 18:591a007effc2 103 3.06, /* Holding kval [V]. */
Davidroid 18:591a007effc2 104 3.06, /* Constant speed kval [V]. */
Davidroid 18:591a007effc2 105 3.06, /* Acceleration starting kval [V]. */
Davidroid 18:591a007effc2 106 3.06, /* Deceleration starting kval [V]. */
Davidroid 18:591a007effc2 107 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Davidroid 18:591a007effc2 108 392.1569e-6, /* Start slope [s/step]. */
Davidroid 18:591a007effc2 109 643.1372e-6, /* Acceleration final slope [s/step]. */
Davidroid 18:591a007effc2 110 643.1372e-6, /* Deceleration final slope [s/step]. */
Davidroid 18:591a007effc2 111 0, /* Thermal compensation factor (range [0, 15]). */
Davidroid 18:591a007effc2 112 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Davidroid 18:591a007effc2 113 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Davidroid 18:591a007effc2 114 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
alcocerg 22:82611fe41c5c 115 0x00, /* Alarm conditions enable. */
Davidroid 18:591a007effc2 116 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 117 }
Davidroid 9:f35fbeedb8f4 118 };
Davidroid 9:f35fbeedb8f4 119
alcocerg 22:82611fe41c5c 120 void help() // Display list of Commands
alcocerg 22:82611fe41c5c 121 {
alcocerg 22:82611fe41c5c 122 DEBUG("List of commands:\n\r");
alcocerg 22:82611fe41c5c 123 DEBUG(" h --> Help, display list of commands\n\r");
alcocerg 22:82611fe41c5c 124 }
alcocerg 22:82611fe41c5c 125
alcocerg 22:82611fe41c5c 126 void executeCommand(char c) {
alcocerg 22:82611fe41c5c 127 bool flaghelp = false;
alcocerg 22:82611fe41c5c 128 switch (c) {
alcocerg 22:82611fe41c5c 129 case 'h':
alcocerg 22:82611fe41c5c 130 help();
alcocerg 22:82611fe41c5c 131 action=0;
alcocerg 22:82611fe41c5c 132 flaghelp=true;
alcocerg 22:82611fe41c5c 133 CASE('a', "Avance", action=2; )
alcocerg 22:82611fe41c5c 134 CASE('r', "Recule", action=3; )
alcocerg 22:82611fe41c5c 135 CASE('d', "Droite AV", action=4; )
alcocerg 22:82611fe41c5c 136 CASE('g', "Gauche AV", action=5; )
alcocerg 22:82611fe41c5c 137 CASE('p', "Pivote D", action=6; )
alcocerg 22:82611fe41c5c 138 CASE('q', "Pivote G", action=7; )
alcocerg 22:82611fe41c5c 139 CASE('s', "Stop", action=1; )
alcocerg 22:82611fe41c5c 140 CASE('n', "Debrayer", action=9; )
alcocerg 22:82611fe41c5c 141 CASE('k', "Pince haute", action=10; )
alcocerg 22:82611fe41c5c 142 CASE('j', "Pince basse", action=11; )
alcocerg 22:82611fe41c5c 143 CASE('b', "Ventilo On", action=12; )
alcocerg 22:82611fe41c5c 144 CASE('l', "Ventilo Off", action=13; )
alcocerg 22:82611fe41c5c 145 CASE('c', "LEDs ON", action=14; )
alcocerg 22:82611fe41c5c 146 CASE('x', "LEDs OFF", action=15; )
alcocerg 22:82611fe41c5c 147 CASE('y', "Pince haute/basse", action=16; )
alcocerg 22:82611fe41c5c 148 CASE('D', "Droite AR", action=17; )
alcocerg 22:82611fe41c5c 149 CASE('G', "Gauche AR", action=18; )
alcocerg 22:82611fe41c5c 150 CASE('t', "Ventilo ON/OFF", action=19; )
alcocerg 22:82611fe41c5c 151
alcocerg 22:82611fe41c5c 152 default :
alcocerg 22:82611fe41c5c 153 DEBUG("invalid command; use: 'h' for help()\n\r");
alcocerg 22:82611fe41c5c 154 action=0;
alcocerg 22:82611fe41c5c 155 }}
alcocerg 22:82611fe41c5c 156
alcocerg 22:82611fe41c5c 157 void analyseCommand(char *command) {
alcocerg 22:82611fe41c5c 158 switch(command[0]) {
alcocerg 22:82611fe41c5c 159 case 'a':
alcocerg 22:82611fe41c5c 160 commandRECEIVED = 'a';
alcocerg 22:82611fe41c5c 161 break;
alcocerg 22:82611fe41c5c 162 case 'r':
alcocerg 22:82611fe41c5c 163 commandRECEIVED = 'r';
alcocerg 22:82611fe41c5c 164 break;
alcocerg 22:82611fe41c5c 165 case 'd':
alcocerg 22:82611fe41c5c 166 commandRECEIVED = 'd';
alcocerg 22:82611fe41c5c 167 break;
alcocerg 22:82611fe41c5c 168 case 'g':
alcocerg 22:82611fe41c5c 169 commandRECEIVED = 'g';
alcocerg 22:82611fe41c5c 170 break;
alcocerg 22:82611fe41c5c 171 case 'D':
alcocerg 22:82611fe41c5c 172 commandRECEIVED = 'D';
alcocerg 22:82611fe41c5c 173 break;
alcocerg 22:82611fe41c5c 174 case 'G':
alcocerg 22:82611fe41c5c 175 commandRECEIVED = 'G';
alcocerg 22:82611fe41c5c 176 break;
alcocerg 22:82611fe41c5c 177 case 's':
alcocerg 22:82611fe41c5c 178 commandRECEIVED = 's';
alcocerg 22:82611fe41c5c 179 break;
alcocerg 22:82611fe41c5c 180 case 'p':
alcocerg 22:82611fe41c5c 181 commandRECEIVED = 'p';
alcocerg 22:82611fe41c5c 182 break;
alcocerg 22:82611fe41c5c 183 case 'q':
alcocerg 22:82611fe41c5c 184 commandRECEIVED = 'q';
alcocerg 22:82611fe41c5c 185 break;
alcocerg 22:82611fe41c5c 186 case 'n':
alcocerg 22:82611fe41c5c 187 commandRECEIVED = 'n';
alcocerg 22:82611fe41c5c 188 break;
alcocerg 22:82611fe41c5c 189 case 'k':
alcocerg 22:82611fe41c5c 190 commandRECEIVED = 'k';
alcocerg 22:82611fe41c5c 191 break;
alcocerg 22:82611fe41c5c 192 case 'j':
alcocerg 22:82611fe41c5c 193 commandRECEIVED = 'j';
alcocerg 22:82611fe41c5c 194 break;
alcocerg 22:82611fe41c5c 195 case 'b':
alcocerg 22:82611fe41c5c 196 commandRECEIVED = 'b';
alcocerg 22:82611fe41c5c 197 break;
alcocerg 22:82611fe41c5c 198 case 'l':
alcocerg 22:82611fe41c5c 199 commandRECEIVED = 'l';
alcocerg 22:82611fe41c5c 200 break;
alcocerg 22:82611fe41c5c 201 case 'c':
alcocerg 22:82611fe41c5c 202 commandRECEIVED = 'c';
alcocerg 22:82611fe41c5c 203 break;
alcocerg 22:82611fe41c5c 204 case 'x':
alcocerg 22:82611fe41c5c 205 commandRECEIVED = 'x';
alcocerg 22:82611fe41c5c 206 break;
alcocerg 22:82611fe41c5c 207 case 'y':
alcocerg 22:82611fe41c5c 208 commandRECEIVED = 'y';
alcocerg 22:82611fe41c5c 209 break;
alcocerg 22:82611fe41c5c 210 case 't':
alcocerg 22:82611fe41c5c 211 commandRECEIVED = 't';
alcocerg 22:82611fe41c5c 212 break;
alcocerg 22:82611fe41c5c 213
alcocerg 22:82611fe41c5c 214 default:
alcocerg 22:82611fe41c5c 215 commandRECEIVED = 'h';
alcocerg 22:82611fe41c5c 216 } }
alcocerg 22:82611fe41c5c 217
alcocerg 22:82611fe41c5c 218 void checkCommand(int result, char *command) {
alcocerg 22:82611fe41c5c 219 if(result==1) {
alcocerg 22:82611fe41c5c 220 analyseCommand(command);
alcocerg 22:82611fe41c5c 221 // DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED, parameterRECEIVED,state);
alcocerg 22:82611fe41c5c 222 receivedCOMMAND = true;
alcocerg 22:82611fe41c5c 223 } }
alcocerg 22:82611fe41c5c 224
alcocerg 22:82611fe41c5c 225 void split(char *line, int length) {
alcocerg 22:82611fe41c5c 226 char command[256];
alcocerg 22:82611fe41c5c 227 int parameter=0;
alcocerg 22:82611fe41c5c 228 int result = 1;
alcocerg 22:82611fe41c5c 229 int i=0;
alcocerg 22:82611fe41c5c 230 int j=0;
alcocerg 22:82611fe41c5c 231 while(i<length && line[i]==' ') {
alcocerg 22:82611fe41c5c 232 i++;}
alcocerg 22:82611fe41c5c 233 while(i<length && line[i]!=' ') {
alcocerg 22:82611fe41c5c 234 command[j]=line[i];
alcocerg 22:82611fe41c5c 235 i++;
alcocerg 22:82611fe41c5c 236 j++;}
alcocerg 22:82611fe41c5c 237 command[j]=0;
alcocerg 22:82611fe41c5c 238 i++;
alcocerg 22:82611fe41c5c 239 j=0;
alcocerg 22:82611fe41c5c 240 while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
alcocerg 22:82611fe41c5c 241 i++;
alcocerg 22:82611fe41c5c 242 j++;}
alcocerg 22:82611fe41c5c 243 if(j>0) {
alcocerg 22:82611fe41c5c 244 result++;
alcocerg 22:82611fe41c5c 245 i--;
alcocerg 22:82611fe41c5c 246 int k=1;
alcocerg 22:82611fe41c5c 247 while(j>0) {
alcocerg 22:82611fe41c5c 248 parameter += (line[i]-'0')*k;
alcocerg 22:82611fe41c5c 249 j--;
alcocerg 22:82611fe41c5c 250 i--;
alcocerg 22:82611fe41c5c 251 k=k*10;}
alcocerg 22:82611fe41c5c 252 }
alcocerg 22:82611fe41c5c 253 checkCommand(result, command);
alcocerg 22:82611fe41c5c 254 }
alcocerg 22:82611fe41c5c 255
alcocerg 22:82611fe41c5c 256 void storeC(char c) {
alcocerg 22:82611fe41c5c 257 if(c==10 || c==13|| commandPosition >= 255) {
alcocerg 22:82611fe41c5c 258 split(commandLine, commandPosition);
alcocerg 22:82611fe41c5c 259 commandPosition=0;}
alcocerg 22:82611fe41c5c 260 else {
alcocerg 22:82611fe41c5c 261 commandLine[commandPosition++]=c;
alcocerg 22:82611fe41c5c 262 commandLine[commandPosition]=0;}
alcocerg 22:82611fe41c5c 263 }
alcocerg 22:82611fe41c5c 264
alcocerg 22:82611fe41c5c 265 void getBT() {
alcocerg 22:82611fe41c5c 266 char c = bt_uart.getc();
alcocerg 22:82611fe41c5c 267 storeC(c);
alcocerg 22:82611fe41c5c 268 }
alcocerg 22:82611fe41c5c 269
alcocerg 22:82611fe41c5c 270 void getPC() {
alcocerg 22:82611fe41c5c 271 char c = pc_uart.getc();
alcocerg 22:82611fe41c5c 272 storeC(c);
alcocerg 22:82611fe41c5c 273 }
alcocerg 22:82611fe41c5c 274
alcocerg 22:82611fe41c5c 275 void leda(int parameter) {
alcocerg 22:82611fe41c5c 276 ledBand.SetColor(WHITE,0) ;
alcocerg 22:82611fe41c5c 277 ledBand.SetColor(RED,1) ;
alcocerg 22:82611fe41c5c 278 ledBand.SetColor(BLUE,2) ;
alcocerg 22:82611fe41c5c 279 ledBand.SetColor(GREEN,3) ;
alcocerg 22:82611fe41c5c 280 if (parameter == 0) {
alcocerg 22:82611fe41c5c 281 ledBand.StopRotation();
alcocerg 22:82611fe41c5c 282 // ledBand.StopBlink() ;
alcocerg 22:82611fe41c5c 283 ledBand.SetColor(BLACK,0) ;
alcocerg 22:82611fe41c5c 284 ledBand.SetColor(BLACK,1) ;
alcocerg 22:82611fe41c5c 285 ledBand.SetColor(BLACK,2) ;
alcocerg 22:82611fe41c5c 286 ledBand.SetColor(BLACK,3) ;}
alcocerg 22:82611fe41c5c 287 if (parameter == 1) {
alcocerg 22:82611fe41c5c 288 ledBand.SetColor(RED,0) ;
alcocerg 22:82611fe41c5c 289 ledBand.SetColor(RED,1) ;
alcocerg 22:82611fe41c5c 290 ledBand.SetColor(RED,2) ;
alcocerg 22:82611fe41c5c 291 ledBand.SetColor(RED,3);}
alcocerg 22:82611fe41c5c 292 if (parameter == 2) {ledBand.StartRotation(0.6);}
alcocerg 22:82611fe41c5c 293 if (parameter == 3) {ledBand.StartRotation(1);}
alcocerg 22:82611fe41c5c 294 state=0;
alcocerg 22:82611fe41c5c 295 }
Davidroid 0:5148e9486cf2 296
Davidroid 0:5148e9486cf2 297 /* Main ----------------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 298
Davidroid 0:5148e9486cf2 299 int main()
Davidroid 0:5148e9486cf2 300 {
alcocerg 22:82611fe41c5c 301 /*----- Initialization. -----*/
alcocerg 22:82611fe41c5c 302 wait(1);
alcocerg 22:82611fe41c5c 303 // bt_uart.printf("AT+NAMEPETITBULLES"); // Changement nom module BT
alcocerg 22:82611fe41c5c 304 // wait(2);
Davidroid 1:9f1974b0960d 305
Davidroid 2:41eeee48951b 306 /* Initializing SPI bus. */
Davidroid 3:fd280b953f77 307 DevSPI dev_spi(D11, D12, D3);
Davidroid 5:3b8e19bbf386 308 /* Initializing Motor Control Expansion Board. */
Davidroid 9:f35fbeedb8f4 309 x_nucleo_ihm02a1 = new X_NUCLEO_IHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
Davidroid 1:9f1974b0960d 310 /* Building a list of motor control components. */
Davidroid 1:9f1974b0960d 311 L6470 **motors = x_nucleo_ihm02a1->GetComponents();
Davidroid 1:9f1974b0960d 312
alcocerg 22:82611fe41c5c 313 pc_uart.printf("MJBot2 \r\n\n"); // Printing to the console
alcocerg 22:82611fe41c5c 314 commandPosition=0;
alcocerg 22:82611fe41c5c 315 bt_uart.attach(getBT);
alcocerg 22:82611fe41c5c 316 pc_uart.attach(getPC);
alcocerg 22:82611fe41c5c 317 state=0;
alcocerg 22:82611fe41c5c 318 action=0;
alcocerg 22:82611fe41c5c 319 pince=0;
alcocerg 22:82611fe41c5c 320 inter=0;
alcocerg 22:82611fe41c5c 321 repos=0;
alcocerg 22:82611fe41c5c 322 attente=0; // temps d'inactivité avant débrayage
alcocerg 22:82611fe41c5c 323 trigger=0; // est à 1 si Stop envoyé une seule fois
alcocerg 22:82611fe41c5c 324 Bot=0; // MJBot Tullins = 0, MJBot R2D2 = 1 and STBot = 2
alcocerg 22:82611fe41c5c 325 ventilo=0;
alcocerg 22:82611fe41c5c 326 myservo.period_ms(20);
alcocerg 22:82611fe41c5c 327 myservo.pulsewidth_us(MID);
alcocerg 22:82611fe41c5c 328 receivedCOMMAND = false;
alcocerg 22:82611fe41c5c 329 while(1) {
alcocerg 22:82611fe41c5c 330 if(receivedCOMMAND) {
alcocerg 22:82611fe41c5c 331 receivedCOMMAND = false;
alcocerg 22:82611fe41c5c 332 executeCommand(commandRECEIVED);
alcocerg 22:82611fe41c5c 333 if (action>=1) {
alcocerg 22:82611fe41c5c 334 if (action==1) {
alcocerg 22:82611fe41c5c 335 if (repos==1) {
alcocerg 22:82611fe41c5c 336 pc_uart.printf("stop hard \r\n\n");
alcocerg 22:82611fe41c5c 337 motors[0]->PrepareHardStop();
alcocerg 22:82611fe41c5c 338 motors[1]->PrepareHardStop();
alcocerg 22:82611fe41c5c 339 repos=0;
alcocerg 22:82611fe41c5c 340 attente=0;
alcocerg 22:82611fe41c5c 341 trigger=1;
alcocerg 22:82611fe41c5c 342 inter=0; }
alcocerg 22:82611fe41c5c 343 else {
alcocerg 22:82611fe41c5c 344 pc_uart.printf("stop debraye \r\n\n");
alcocerg 22:82611fe41c5c 345 motors[0]->PrepareSoftHiZ();
alcocerg 22:82611fe41c5c 346 motors[1]->PrepareSoftHiZ();
alcocerg 22:82611fe41c5c 347 trigger=0;}
alcocerg 22:82611fe41c5c 348 x_nucleo_ihm02a1->PerformPreparedActions();}
alcocerg 22:82611fe41c5c 349 if (action==2) {
alcocerg 22:82611fe41c5c 350 pc_uart.printf("Avance \r\n\n");
alcocerg 22:82611fe41c5c 351 motors[0]->PrepareRun(StepperMotor::BWD, 150);
alcocerg 22:82611fe41c5c 352 motors[1]->PrepareRun(StepperMotor::FWD, 150);
alcocerg 22:82611fe41c5c 353 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 354 repos=1; }
alcocerg 22:82611fe41c5c 355 if (action==3) {
alcocerg 22:82611fe41c5c 356 pc_uart.printf("Recule \r\n\n");
alcocerg 22:82611fe41c5c 357 // motors[0]->Run(StepperMotor::BWD, 200);
alcocerg 22:82611fe41c5c 358 // motors[1]->Run(StepperMotor::FWD, 200);}
alcocerg 22:82611fe41c5c 359 motors[0]->PrepareRun(StepperMotor::FWD, 150);
alcocerg 22:82611fe41c5c 360 motors[1]->PrepareRun(StepperMotor::BWD, 150);
alcocerg 22:82611fe41c5c 361 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 362 repos=1; }
alcocerg 22:82611fe41c5c 363 if (action==4) {
alcocerg 22:82611fe41c5c 364 pc_uart.printf("droite AV \r\n\n");
alcocerg 22:82611fe41c5c 365 // motors[0]->Run(StepperMotor::FWD, 100);
alcocerg 22:82611fe41c5c 366 motors[0]->PrepareHardStop();
alcocerg 22:82611fe41c5c 367 motors[1]->PrepareRun(StepperMotor::FWD, 150);
alcocerg 22:82611fe41c5c 368 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 369 repos=1; }
alcocerg 22:82611fe41c5c 370 if (action==5) {
alcocerg 22:82611fe41c5c 371 pc_uart.printf("gauche AV \r\n\n");
alcocerg 22:82611fe41c5c 372 motors[0]->PrepareRun(StepperMotor::BWD, 150);
alcocerg 22:82611fe41c5c 373 motors[1]->PrepareHardStop();
alcocerg 22:82611fe41c5c 374 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 375 repos=1; }
alcocerg 22:82611fe41c5c 376 if (action==17) {
alcocerg 22:82611fe41c5c 377 pc_uart.printf("droite AR \r\n\n");
alcocerg 22:82611fe41c5c 378 motors[0]->PrepareHardStop();
alcocerg 22:82611fe41c5c 379 motors[1]->PrepareRun(StepperMotor::BWD, 150);
alcocerg 22:82611fe41c5c 380 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 381 repos=1; }
alcocerg 22:82611fe41c5c 382 if (action==18) {
alcocerg 22:82611fe41c5c 383 pc_uart.printf("gauche AR \r\n\n");
alcocerg 22:82611fe41c5c 384 motors[0]->PrepareRun(StepperMotor::FWD, 150);
alcocerg 22:82611fe41c5c 385 motors[1]->PrepareHardStop();
alcocerg 22:82611fe41c5c 386 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 387 repos=1; }
alcocerg 22:82611fe41c5c 388 if (action==6) {
alcocerg 22:82611fe41c5c 389 pc_uart.printf("pivote d \r\n\n");
alcocerg 22:82611fe41c5c 390 motors[0]->PrepareRun(StepperMotor::FWD, 75);
alcocerg 22:82611fe41c5c 391 motors[1]->PrepareRun(StepperMotor::FWD, 75);
alcocerg 22:82611fe41c5c 392 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 393 repos=1; }
alcocerg 22:82611fe41c5c 394 if (action==7) {
alcocerg 22:82611fe41c5c 395 pc_uart.printf("pivote g\r\n\n");
alcocerg 22:82611fe41c5c 396 motors[0]->PrepareRun(StepperMotor::BWD, 75);
alcocerg 22:82611fe41c5c 397 motors[1]->PrepareRun(StepperMotor::BWD, 75);
alcocerg 22:82611fe41c5c 398 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 399 repos=1; }
alcocerg 22:82611fe41c5c 400 // motors[0]->Run(StepperMotor::FWD, 200);
alcocerg 22:82611fe41c5c 401 // motors[1]->Run(StepperMotor::FWD, 200); }
alcocerg 22:82611fe41c5c 402 if (action==9) {
alcocerg 22:82611fe41c5c 403 pc_uart.printf("debrayer \r\n\n");
alcocerg 22:82611fe41c5c 404 motors[0]->PrepareSoftHiZ();
alcocerg 22:82611fe41c5c 405 motors[1]->PrepareSoftHiZ();
alcocerg 22:82611fe41c5c 406 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 407 repos=0;
alcocerg 22:82611fe41c5c 408 trigger=0;
alcocerg 22:82611fe41c5c 409 attente=0;}
alcocerg 22:82611fe41c5c 410 if (action==10) {
alcocerg 22:82611fe41c5c 411 pc_uart.printf("pince haute \r\n\n");
alcocerg 22:82611fe41c5c 412 myservo.pulsewidth_us(MIN);}
alcocerg 22:82611fe41c5c 413 if (action==11) {
alcocerg 22:82611fe41c5c 414 pc_uart.printf("pince basse \r\n\n");
alcocerg 22:82611fe41c5c 415 myservo.pulsewidth_us(MAX);}
alcocerg 22:82611fe41c5c 416 if (action==12) {
alcocerg 22:82611fe41c5c 417 pc_uart.printf("Ventilo On \r\n\n");
alcocerg 22:82611fe41c5c 418 ventilo=1;
alcocerg 22:82611fe41c5c 419 }
alcocerg 22:82611fe41c5c 420 if (action==13) {
alcocerg 22:82611fe41c5c 421 pc_uart.printf("Ventilo Off \r\n\n");
alcocerg 22:82611fe41c5c 422 ventilo=0;
alcocerg 22:82611fe41c5c 423 }
alcocerg 22:82611fe41c5c 424 if (action==19) {
alcocerg 22:82611fe41c5c 425 pc_uart.printf("Ventilo ON/OFF \r\n\n");
alcocerg 22:82611fe41c5c 426 if (ventilo==0) {
alcocerg 22:82611fe41c5c 427 ventilo=1;}
alcocerg 22:82611fe41c5c 428 else {ventilo=0;}}
alcocerg 22:82611fe41c5c 429 if (action==14) {
alcocerg 22:82611fe41c5c 430 pc_uart.printf("LEDs ON \r\n\n");
alcocerg 22:82611fe41c5c 431 leda(3);}
alcocerg 22:82611fe41c5c 432 if (action==15) {
alcocerg 22:82611fe41c5c 433 pc_uart.printf("LEDs OFF \r\n\n");
alcocerg 22:82611fe41c5c 434 leda(0);}
alcocerg 22:82611fe41c5c 435 if (action==16) {
alcocerg 22:82611fe41c5c 436 pc_uart.printf("pince haute/basse \r\n\n");
alcocerg 22:82611fe41c5c 437 if (pince==0) {
alcocerg 22:82611fe41c5c 438 leda(1);
alcocerg 22:82611fe41c5c 439 inter=0;
alcocerg 22:82611fe41c5c 440 myservo.pulsewidth_us(MIN);
alcocerg 22:82611fe41c5c 441 pince=1;}
alcocerg 22:82611fe41c5c 442 else {
alcocerg 22:82611fe41c5c 443 myservo.pulsewidth_us(MAX);
alcocerg 22:82611fe41c5c 444 leda(2);
alcocerg 22:82611fe41c5c 445 pince=0;}
alcocerg 22:82611fe41c5c 446
alcocerg 22:82611fe41c5c 447 action=0;
alcocerg 22:82611fe41c5c 448 } }
alcocerg 22:82611fe41c5c 449 wait(0.1);
alcocerg 22:82611fe41c5c 450 attente++;
alcocerg 22:82611fe41c5c 451 if (attente>50 && repos==0 && trigger==1) {
alcocerg 22:82611fe41c5c 452 pc_uart.printf("attente OK\r\n\n");
alcocerg 22:82611fe41c5c 453 motors[0]->PrepareSoftHiZ();
alcocerg 22:82611fe41c5c 454 motors[1]->PrepareSoftHiZ();
alcocerg 22:82611fe41c5c 455 x_nucleo_ihm02a1->PerformPreparedActions();
alcocerg 22:82611fe41c5c 456 attente=0;
alcocerg 22:82611fe41c5c 457 trigger=0; }
alcocerg 22:82611fe41c5c 458 } } }