CreaLab / Mbed 2 deprecated OvnitBot

First release

Dependencies:   CreaBotLib LED_WS2812 MotorLib X_NUCLEO_6180XA1 mbed

main.cpp@0:1f59690eebe2, 2018-09-19 (annotated)

Committer:
alcocerg
Date:
Wed Sep 19 14:20:26 2018 +0000
Revision:
0:1f59690eebe2
Child:
1:3589e8f6e99c
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alcocerg 0:1f59690eebe2 1 /*
alcocerg 0:1f59690eebe2 2 * OvniBot
alcocerg 0:1f59690eebe2 3 *
alcocerg 0:1f59690eebe2 4 */
alcocerg 0:1f59690eebe2 5
alcocerg 0:1f59690eebe2 6 #include "XNucleo6180XA1.h"
alcocerg 0:1f59690eebe2 7 #undef printf
alcocerg 0:1f59690eebe2 8 #include "Crealab.h"
alcocerg 0:1f59690eebe2 9
alcocerg 0:1f59690eebe2 10 Serial bt_uart(PA_9, PA_10); // PA9 = Tx, PA10 = Rx
alcocerg 0:1f59690eebe2 11 Serial pc_uart(USBTX, USBRX); // USBTX = PA2, USBRX = PA3
alcocerg 0:1f59690eebe2 12
alcocerg 0:1f59690eebe2 13 // ---------------- PIN DEFINITIONS ---------------------
alcocerg 0:1f59690eebe2 14 InterruptIn buttonBox(PC_13);
alcocerg 0:1f59690eebe2 15 Motor motorRG(PB_13, PB_14, PB_15, PB_1);
alcocerg 0:1f59690eebe2 16 Motor motorRD(PA_0, PA_1, PA_4, PB_0);
alcocerg 0:1f59690eebe2 17
alcocerg 0:1f59690eebe2 18 // insert the motors and indicate wheel diameter and distance between wheels
alcocerg 0:1f59690eebe2 19 Creabot mybot(&motorRG, &motorRD, 9.0f,13.2f); // proto CréaLab
alcocerg 0:1f59690eebe2 20
alcocerg 0:1f59690eebe2 21 // PIN & number of LEDS. Available color ==> BLUE, ORANGE, RED, GREEN, BLACK, WHITE, PURPLE, PINK, YELLOW
alcocerg 0:1f59690eebe2 22 LED_WS2812 ledBand(PB_3,4);
alcocerg 0:1f59690eebe2 23
alcocerg 0:1f59690eebe2 24 bool receivedCOMMAND;
alcocerg 0:1f59690eebe2 25 char commandRECEIVED;
alcocerg 0:1f59690eebe2 26 int parameterRECEIVED;
alcocerg 0:1f59690eebe2 27 int state;
alcocerg 0:1f59690eebe2 28 char commandLine[256];
alcocerg 0:1f59690eebe2 29 int commandPosition;
alcocerg 0:1f59690eebe2 30 uint32_t dist;
alcocerg 0:1f59690eebe2 31
alcocerg 0:1f59690eebe2 32 // ----------- PIN Definitions 6180 -------------
alcocerg 0:1f59690eebe2 33 #define VL6180X_I2C_SDA D14
alcocerg 0:1f59690eebe2 34 #define VL6180X_I2C_SCL D15
alcocerg 0:1f59690eebe2 35 static XNucleo6180XA1 *board = NULL;
alcocerg 0:1f59690eebe2 36 // penser à modifier dans vl6180x_platform.h -> commenter la ligne // #define MY_LOG 1
alcocerg 0:1f59690eebe2 37
alcocerg 0:1f59690eebe2 38 void help() // Display list of Commands
alcocerg 0:1f59690eebe2 39 {
alcocerg 0:1f59690eebe2 40 DEBUG("List of commands:\n\r");
alcocerg 0:1f59690eebe2 41 DEBUG(" h --> Help, display list of commands\n\r");
alcocerg 0:1f59690eebe2 42 }
alcocerg 0:1f59690eebe2 43
alcocerg 0:1f59690eebe2 44 void callback() {
alcocerg 0:1f59690eebe2 45 switch(state){
alcocerg 0:1f59690eebe2 46 case 0:
alcocerg 0:1f59690eebe2 47 break;
alcocerg 0:1f59690eebe2 48 case 1:
alcocerg 0:1f59690eebe2 49 break;
alcocerg 0:1f59690eebe2 50 case 2:
alcocerg 0:1f59690eebe2 51 break;
alcocerg 0:1f59690eebe2 52 case 3:
alcocerg 0:1f59690eebe2 53 break;
alcocerg 0:1f59690eebe2 54 default:
alcocerg 0:1f59690eebe2 55 }
alcocerg 0:1f59690eebe2 56 state=state+1;
alcocerg 0:1f59690eebe2 57 }
alcocerg 0:1f59690eebe2 58
alcocerg 0:1f59690eebe2 59 void detect(int parameter)
alcocerg 0:1f59690eebe2 60 {
alcocerg 0:1f59690eebe2 61 mybot.move(FORWARD,300);
alcocerg 0:1f59690eebe2 62 dist=250;
alcocerg 0:1f59690eebe2 63 while (dist>parameter)
alcocerg 0:1f59690eebe2 64 {wait (0.2);
alcocerg 0:1f59690eebe2 65 board->sensor_bottom->get_distance(&dist) ; }
alcocerg 0:1f59690eebe2 66 // DEBUG ("Distance: %d \n\r", dist);
alcocerg 0:1f59690eebe2 67 mybot.stopMove();
alcocerg 0:1f59690eebe2 68 }
alcocerg 0:1f59690eebe2 69
alcocerg 0:1f59690eebe2 70 void routine1()
alcocerg 0:1f59690eebe2 71 {
alcocerg 0:1f59690eebe2 72 DEBUG("routine1\n\r");
alcocerg 0:1f59690eebe2 73 wait(1);
alcocerg 0:1f59690eebe2 74 ledBand.SetColor(WHITE,0) ;
alcocerg 0:1f59690eebe2 75 ledBand.SetColor(RED,1) ;
alcocerg 0:1f59690eebe2 76 ledBand.SetColor(BLUE,2) ;
alcocerg 0:1f59690eebe2 77 ledBand.SetColor(GREEN,3) ;
alcocerg 0:1f59690eebe2 78 ledBand.StartRotation(1.0) ;
alcocerg 0:1f59690eebe2 79 mybot.move(FORWARD,200);
alcocerg 0:1f59690eebe2 80 state=0;
alcocerg 0:1f59690eebe2 81 /* mybot.move(FORWARD,9);
alcocerg 0:1f59690eebe2 82 dist=200;
alcocerg 0:1f59690eebe2 83 while (dist>120)
alcocerg 0:1f59690eebe2 84 {wait (0.3);
alcocerg 0:1f59690eebe2 85 board->sensor_bottom->get_distance(&dist) ; }
alcocerg 0:1f59690eebe2 86 DEBUG ("Distance: %d \n\r", dist);
alcocerg 0:1f59690eebe2 87 mybot.stopMove(); */
alcocerg 0:1f59690eebe2 88 }
alcocerg 0:1f59690eebe2 89
alcocerg 0:1f59690eebe2 90 void routine2()
alcocerg 0:1f59690eebe2 91 {
alcocerg 0:1f59690eebe2 92 DEBUG("routine2\n\r");
alcocerg 0:1f59690eebe2 93 wait(1);
alcocerg 0:1f59690eebe2 94 ledBand.SetColor(WHITE,0) ;
alcocerg 0:1f59690eebe2 95 ledBand.SetColor(RED,1) ;
alcocerg 0:1f59690eebe2 96 ledBand.SetColor(BLUE,2) ;
alcocerg 0:1f59690eebe2 97 ledBand.SetColor(GREEN,3) ;
alcocerg 0:1f59690eebe2 98 ledBand.StartRotation(0.6) ;
alcocerg 0:1f59690eebe2 99 mybot.move(BACKWARD,200);
alcocerg 0:1f59690eebe2 100 state=0;
alcocerg 0:1f59690eebe2 101 /* mybot.move(FORWARD,9);
alcocerg 0:1f59690eebe2 102 dist=200;
alcocerg 0:1f59690eebe2 103 while (dist>120)
alcocerg 0:1f59690eebe2 104 {wait (0.3);
alcocerg 0:1f59690eebe2 105 board->sensor_bottom->get_distance(&dist) ; }
alcocerg 0:1f59690eebe2 106 DEBUG ("Distance: %d \n\r", dist);
alcocerg 0:1f59690eebe2 107 mybot.stopMove(); */
alcocerg 0:1f59690eebe2 108 }
alcocerg 0:1f59690eebe2 109
alcocerg 0:1f59690eebe2 110 void routines()
alcocerg 0:1f59690eebe2 111 {
alcocerg 0:1f59690eebe2 112 DEBUG("routines\n\r");
alcocerg 0:1f59690eebe2 113 wait(1);
alcocerg 0:1f59690eebe2 114 ledBand.StopRotation();
alcocerg 0:1f59690eebe2 115 ledBand.StopBlink() ;
alcocerg 0:1f59690eebe2 116 ledBand.SetColor(BLACK,0) ;
alcocerg 0:1f59690eebe2 117 ledBand.SetColor(BLACK,1) ;
alcocerg 0:1f59690eebe2 118 ledBand.SetColor(BLACK,2) ;
alcocerg 0:1f59690eebe2 119 ledBand.SetColor(BLACK,3) ;
alcocerg 0:1f59690eebe2 120 mybot.stopMove();
alcocerg 0:1f59690eebe2 121 state=0;
alcocerg 0:1f59690eebe2 122 }
alcocerg 0:1f59690eebe2 123
alcocerg 0:1f59690eebe2 124
alcocerg 0:1f59690eebe2 125 void executeCommand(char c) {
alcocerg 0:1f59690eebe2 126 bool flaghelp = false;
alcocerg 0:1f59690eebe2 127 switch (c) {
alcocerg 0:1f59690eebe2 128 case 'h':
alcocerg 0:1f59690eebe2 129 help();
alcocerg 0:1f59690eebe2 130 state=0;
alcocerg 0:1f59690eebe2 131 flaghelp=true;
alcocerg 0:1f59690eebe2 132 CASE('l', "lance la routine 1", routine1(); )
alcocerg 0:1f59690eebe2 133 CASE('m', "lance la routine 2", routine2(); )
alcocerg 0:1f59690eebe2 134 CASE('s', "stoppe les routines", routines(); )
alcocerg 0:1f59690eebe2 135
alcocerg 0:1f59690eebe2 136 default :
alcocerg 0:1f59690eebe2 137 DEBUG("invalid command; use: 'h' for help()\n\r");
alcocerg 0:1f59690eebe2 138 state=0;
alcocerg 0:1f59690eebe2 139 }}
alcocerg 0:1f59690eebe2 140
alcocerg 0:1f59690eebe2 141 void analyseCommand(char *command) {
alcocerg 0:1f59690eebe2 142 switch(command[0]) {
alcocerg 0:1f59690eebe2 143 case 'l':
alcocerg 0:1f59690eebe2 144 commandRECEIVED = 'l';
alcocerg 0:1f59690eebe2 145 break;
alcocerg 0:1f59690eebe2 146 case 's':
alcocerg 0:1f59690eebe2 147 commandRECEIVED = 's';
alcocerg 0:1f59690eebe2 148 break;
alcocerg 0:1f59690eebe2 149 case 'm':
alcocerg 0:1f59690eebe2 150 commandRECEIVED = 'm';
alcocerg 0:1f59690eebe2 151 break;
alcocerg 0:1f59690eebe2 152
alcocerg 0:1f59690eebe2 153 default:
alcocerg 0:1f59690eebe2 154 commandRECEIVED = 'h';
alcocerg 0:1f59690eebe2 155 } }
alcocerg 0:1f59690eebe2 156
alcocerg 0:1f59690eebe2 157 void checkCommand(int result, char *command) {
alcocerg 0:1f59690eebe2 158 if(result==1) {
alcocerg 0:1f59690eebe2 159 analyseCommand(command);
alcocerg 0:1f59690eebe2 160 // DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED,state);
alcocerg 0:1f59690eebe2 161 receivedCOMMAND = true;
alcocerg 0:1f59690eebe2 162 } }
alcocerg 0:1f59690eebe2 163
alcocerg 0:1f59690eebe2 164 void split(char *line, int length) {
alcocerg 0:1f59690eebe2 165 char command[256];
alcocerg 0:1f59690eebe2 166 int parameter=0;
alcocerg 0:1f59690eebe2 167 int result = 1;
alcocerg 0:1f59690eebe2 168 int i=0;
alcocerg 0:1f59690eebe2 169 int j=0;
alcocerg 0:1f59690eebe2 170 while(i<length && line[i]==' ') {
alcocerg 0:1f59690eebe2 171 i++;}
alcocerg 0:1f59690eebe2 172 while(i<length && line[i]!=' ') {
alcocerg 0:1f59690eebe2 173 command[j]=line[i];
alcocerg 0:1f59690eebe2 174 i++;
alcocerg 0:1f59690eebe2 175 j++;}
alcocerg 0:1f59690eebe2 176 command[j]=0;
alcocerg 0:1f59690eebe2 177 i++;
alcocerg 0:1f59690eebe2 178 j=0;
alcocerg 0:1f59690eebe2 179 while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
alcocerg 0:1f59690eebe2 180 i++;
alcocerg 0:1f59690eebe2 181 j++;}
alcocerg 0:1f59690eebe2 182 if(j>0) {
alcocerg 0:1f59690eebe2 183 result++;
alcocerg 0:1f59690eebe2 184 i--;
alcocerg 0:1f59690eebe2 185 int k=1;
alcocerg 0:1f59690eebe2 186 while(j>0) {
alcocerg 0:1f59690eebe2 187 parameter += (line[i]-'0')*k;
alcocerg 0:1f59690eebe2 188 j--;
alcocerg 0:1f59690eebe2 189 i--;
alcocerg 0:1f59690eebe2 190 k=k*10;}
alcocerg 0:1f59690eebe2 191 }
alcocerg 0:1f59690eebe2 192 checkCommand(result, command);
alcocerg 0:1f59690eebe2 193 }
alcocerg 0:1f59690eebe2 194
alcocerg 0:1f59690eebe2 195 void storeC(char c) {
alcocerg 0:1f59690eebe2 196 if(c==10 || c==13|| commandPosition >= 255) {
alcocerg 0:1f59690eebe2 197 split(commandLine, commandPosition);
alcocerg 0:1f59690eebe2 198 commandPosition=0;}
alcocerg 0:1f59690eebe2 199 else {
alcocerg 0:1f59690eebe2 200 commandLine[commandPosition++]=c;
alcocerg 0:1f59690eebe2 201 commandLine[commandPosition]=0;}
alcocerg 0:1f59690eebe2 202 }
alcocerg 0:1f59690eebe2 203
alcocerg 0:1f59690eebe2 204 /*void getBT() {
alcocerg 0:1f59690eebe2 205 char c = bt_uart.getc();
alcocerg 0:1f59690eebe2 206 storeC(c);
alcocerg 0:1f59690eebe2 207 }*/
alcocerg 0:1f59690eebe2 208
alcocerg 0:1f59690eebe2 209 void getPC() {
alcocerg 0:1f59690eebe2 210 char c = pc_uart.getc();
alcocerg 0:1f59690eebe2 211 storeC(c);
alcocerg 0:1f59690eebe2 212 }
alcocerg 0:1f59690eebe2 213
alcocerg 0:1f59690eebe2 214 /* Stop all processes */
alcocerg 0:1f59690eebe2 215 void stop_all()
alcocerg 0:1f59690eebe2 216 {
alcocerg 0:1f59690eebe2 217 mybot.stopMove();
alcocerg 0:1f59690eebe2 218 }
alcocerg 0:1f59690eebe2 219
alcocerg 0:1f59690eebe2 220 /* Interrupt routine, switch of end of course */
alcocerg 0:1f59690eebe2 221 void clicked()
alcocerg 0:1f59690eebe2 222 {
alcocerg 0:1f59690eebe2 223 // DEBUG("Labyrinthe\n\r");
alcocerg 0:1f59690eebe2 224 commandRECEIVED = 'l';
alcocerg 0:1f59690eebe2 225 receivedCOMMAND = true;
alcocerg 0:1f59690eebe2 226 }
alcocerg 0:1f59690eebe2 227
alcocerg 0:1f59690eebe2 228 void endOfMove(int status) {
alcocerg 0:1f59690eebe2 229 DEBUG("ENDOFMOVE\n\r");
alcocerg 0:1f59690eebe2 230 state=0;
alcocerg 0:1f59690eebe2 231 }
alcocerg 0:1f59690eebe2 232
alcocerg 0:1f59690eebe2 233 /* Main Routine */
alcocerg 0:1f59690eebe2 234 int main()
alcocerg 0:1f59690eebe2 235 {
alcocerg 0:1f59690eebe2 236 wait(1);
alcocerg 0:1f59690eebe2 237
alcocerg 0:1f59690eebe2 238 /* Connect EoC button */
alcocerg 0:1f59690eebe2 239 CATCH_BUTTON(buttonBox,clicked);
alcocerg 0:1f59690eebe2 240
alcocerg 0:1f59690eebe2 241 /* Init 6180 X-Nucleo */
alcocerg 0:1f59690eebe2 242 int status;
alcocerg 0:1f59690eebe2 243 DevI2C *device_i2c = new DevI2C(VL6180X_I2C_SDA, VL6180X_I2C_SCL);
alcocerg 0:1f59690eebe2 244 board = XNucleo6180XA1::instance(device_i2c, A5, A4, D13, D4);
alcocerg 0:1f59690eebe2 245 status = board->init_board();
alcocerg 0:1f59690eebe2 246 if (status) {DEBUG("Failed to init board!\n\r"); }
alcocerg 0:1f59690eebe2 247
alcocerg 0:1f59690eebe2 248 commandPosition=0;
alcocerg 0:1f59690eebe2 249 // bt_uart.attach(getBT);
alcocerg 0:1f59690eebe2 250 pc_uart.attach(getPC);
alcocerg 0:1f59690eebe2 251 mybot.setCallBack(endOfMove);
alcocerg 0:1f59690eebe2 252 mybot.setSpeed(5); // max 8 cm.s, average 5 cm.s
alcocerg 0:1f59690eebe2 253 ledBand.SetColor(BLACK);
alcocerg 0:1f59690eebe2 254 state=0;
alcocerg 0:1f59690eebe2 255 receivedCOMMAND = false;
alcocerg 0:1f59690eebe2 256 DEBUG("OvniBot\n\r");
alcocerg 0:1f59690eebe2 257 while(1) {
alcocerg 0:1f59690eebe2 258 if(state==0 && receivedCOMMAND) {
alcocerg 0:1f59690eebe2 259 receivedCOMMAND = false;
alcocerg 0:1f59690eebe2 260 state=1;
alcocerg 0:1f59690eebe2 261 executeCommand(commandRECEIVED);
alcocerg 0:1f59690eebe2 262 }
alcocerg 0:1f59690eebe2 263 wait (0.3);
alcocerg 0:1f59690eebe2 264 board->sensor_bottom->get_distance(&dist) ;
alcocerg 0:1f59690eebe2 265 DEBUG ("Distance bottom: %d \n\r", dist);
alcocerg 0:1f59690eebe2 266 if (dist<100) {
alcocerg 0:1f59690eebe2 267 commandRECEIVED = 'l';
alcocerg 0:1f59690eebe2 268 executeCommand(commandRECEIVED);}
alcocerg 0:1f59690eebe2 269 wait (0.3);
alcocerg 0:1f59690eebe2 270 board->sensor_top->get_distance(&dist) ;
alcocerg 0:1f59690eebe2 271 DEBUG ("Distance top: %d \n\r", dist);
alcocerg 0:1f59690eebe2 272 if (dist<100) {
alcocerg 0:1f59690eebe2 273 commandRECEIVED = 's';
alcocerg 0:1f59690eebe2 274 executeCommand(commandRECEIVED);}
alcocerg 0:1f59690eebe2 275 wait(0.1);
alcocerg 0:1f59690eebe2 276 } }