CreaLab / Mbed 2 deprecated OvnitBot

First release

Dependencies:   CreaBotLib LED_WS2812 MotorLib X_NUCLEO_6180XA1 mbed

main.cpp@1:3589e8f6e99c, 2018-09-24 (annotated)

Committer:
alcocerg
Date:
Mon Sep 24 12:53:58 2018 +0000
Revision:
1:3589e8f6e99c
Parent:
0:1f59690eebe2 
update

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 mybot.move(FORWARD,200);
alcocerg 1:3589e8f6e99c 75 ledBand.SetColor(BLUE,0) ;
alcocerg 1:3589e8f6e99c 76 ledBand.SetColor(BLACK,1) ;
alcocerg 1:3589e8f6e99c 77 ledBand.SetColor(BLACK,2) ;
alcocerg 1:3589e8f6e99c 78 ledBand.SetColor(BLACK,3) ;
alcocerg 1:3589e8f6e99c 79 ledBand.StartRotation(0.5) ;
alcocerg 1:3589e8f6e99c 80 wait(2);
alcocerg 1:3589e8f6e99c 81 ledBand.SetColor(WHITE,0) ;
alcocerg 1:3589e8f6e99c 82 ledBand.SetColor(BLACK,1) ;
alcocerg 1:3589e8f6e99c 83 ledBand.SetColor(BLACK,2) ;
alcocerg 1:3589e8f6e99c 84 ledBand.SetColor(BLACK,3) ;
alcocerg 1:3589e8f6e99c 85 ledBand.StartRotation(0.5) ;
alcocerg 0:1f59690eebe2 86 state=0;
alcocerg 0:1f59690eebe2 87 /* mybot.move(FORWARD,9);
alcocerg 0:1f59690eebe2 88 dist=200;
alcocerg 0:1f59690eebe2 89 while (dist>120)
alcocerg 0:1f59690eebe2 90 {wait (0.3);
alcocerg 0:1f59690eebe2 91 board->sensor_bottom->get_distance(&dist) ; }
alcocerg 0:1f59690eebe2 92 DEBUG ("Distance: %d \n\r", dist);
alcocerg 1:3589e8f6e99c 93 mybot.stopMove();
alcocerg 1:3589e8f6e99c 94 ledBand.SetColor(WHITE,0) ;
alcocerg 1:3589e8f6e99c 95 ledBand.SetColor(RED,1) ;
alcocerg 1:3589e8f6e99c 96 ledBand.SetColor(BLUE,2) ;
alcocerg 1:3589e8f6e99c 97 ledBand.SetColor(GREEN,3) ; */
alcocerg 0:1f59690eebe2 98 }
alcocerg 0:1f59690eebe2 99
alcocerg 0:1f59690eebe2 100 void routine2()
alcocerg 0:1f59690eebe2 101 {
alcocerg 0:1f59690eebe2 102 DEBUG("routine2\n\r");
alcocerg 0:1f59690eebe2 103 wait(1);
alcocerg 0:1f59690eebe2 104 ledBand.SetColor(WHITE,0) ;
alcocerg 0:1f59690eebe2 105 ledBand.SetColor(RED,1) ;
alcocerg 0:1f59690eebe2 106 ledBand.SetColor(BLUE,2) ;
alcocerg 0:1f59690eebe2 107 ledBand.SetColor(GREEN,3) ;
alcocerg 0:1f59690eebe2 108 ledBand.StartRotation(0.6) ;
alcocerg 0:1f59690eebe2 109 mybot.move(BACKWARD,200);
alcocerg 0:1f59690eebe2 110 state=0;
alcocerg 0:1f59690eebe2 111 /* mybot.move(FORWARD,9);
alcocerg 0:1f59690eebe2 112 dist=200;
alcocerg 0:1f59690eebe2 113 while (dist>120)
alcocerg 0:1f59690eebe2 114 {wait (0.3);
alcocerg 0:1f59690eebe2 115 board->sensor_bottom->get_distance(&dist) ; }
alcocerg 0:1f59690eebe2 116 DEBUG ("Distance: %d \n\r", dist);
alcocerg 0:1f59690eebe2 117 mybot.stopMove(); */
alcocerg 0:1f59690eebe2 118 }
alcocerg 0:1f59690eebe2 119
alcocerg 0:1f59690eebe2 120 void routines()
alcocerg 0:1f59690eebe2 121 {
alcocerg 0:1f59690eebe2 122 DEBUG("routines\n\r");
alcocerg 0:1f59690eebe2 123 wait(1);
alcocerg 0:1f59690eebe2 124 ledBand.StopRotation();
alcocerg 0:1f59690eebe2 125 ledBand.SetColor(BLACK,0) ;
alcocerg 0:1f59690eebe2 126 ledBand.SetColor(BLACK,1) ;
alcocerg 0:1f59690eebe2 127 ledBand.SetColor(BLACK,2) ;
alcocerg 0:1f59690eebe2 128 ledBand.SetColor(BLACK,3) ;
alcocerg 0:1f59690eebe2 129 mybot.stopMove();
alcocerg 0:1f59690eebe2 130 state=0;
alcocerg 0:1f59690eebe2 131 }
alcocerg 0:1f59690eebe2 132
alcocerg 0:1f59690eebe2 133
alcocerg 0:1f59690eebe2 134 void executeCommand(char c) {
alcocerg 0:1f59690eebe2 135 bool flaghelp = false;
alcocerg 0:1f59690eebe2 136 switch (c) {
alcocerg 0:1f59690eebe2 137 case 'h':
alcocerg 0:1f59690eebe2 138 help();
alcocerg 0:1f59690eebe2 139 state=0;
alcocerg 0:1f59690eebe2 140 flaghelp=true;
alcocerg 0:1f59690eebe2 141 CASE('l', "lance la routine 1", routine1(); )
alcocerg 0:1f59690eebe2 142 CASE('m', "lance la routine 2", routine2(); )
alcocerg 0:1f59690eebe2 143 CASE('s', "stoppe les routines", routines(); )
alcocerg 1:3589e8f6e99c 144 CASE('t', "accelere", routines(); )
alcocerg 0:1f59690eebe2 145 default :
alcocerg 0:1f59690eebe2 146 DEBUG("invalid command; use: 'h' for help()\n\r");
alcocerg 0:1f59690eebe2 147 state=0;
alcocerg 0:1f59690eebe2 148 }}
alcocerg 0:1f59690eebe2 149
alcocerg 0:1f59690eebe2 150 void analyseCommand(char *command) {
alcocerg 0:1f59690eebe2 151 switch(command[0]) {
alcocerg 0:1f59690eebe2 152 case 'l':
alcocerg 0:1f59690eebe2 153 commandRECEIVED = 'l';
alcocerg 0:1f59690eebe2 154 break;
alcocerg 0:1f59690eebe2 155 case 's':
alcocerg 0:1f59690eebe2 156 commandRECEIVED = 's';
alcocerg 0:1f59690eebe2 157 break;
alcocerg 0:1f59690eebe2 158 case 'm':
alcocerg 0:1f59690eebe2 159 commandRECEIVED = 'm';
alcocerg 0:1f59690eebe2 160 break;
alcocerg 1:3589e8f6e99c 161 case 't':
alcocerg 1:3589e8f6e99c 162 commandRECEIVED = 't';
alcocerg 1:3589e8f6e99c 163 break;
alcocerg 0:1f59690eebe2 164 default:
alcocerg 0:1f59690eebe2 165 commandRECEIVED = 'h';
alcocerg 0:1f59690eebe2 166 } }
alcocerg 0:1f59690eebe2 167
alcocerg 0:1f59690eebe2 168 void checkCommand(int result, char *command) {
alcocerg 0:1f59690eebe2 169 if(result==1) {
alcocerg 0:1f59690eebe2 170 analyseCommand(command);
alcocerg 0:1f59690eebe2 171 // DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED,state);
alcocerg 0:1f59690eebe2 172 receivedCOMMAND = true;
alcocerg 0:1f59690eebe2 173 } }
alcocerg 0:1f59690eebe2 174
alcocerg 0:1f59690eebe2 175 void split(char *line, int length) {
alcocerg 0:1f59690eebe2 176 char command[256];
alcocerg 0:1f59690eebe2 177 int parameter=0;
alcocerg 0:1f59690eebe2 178 int result = 1;
alcocerg 0:1f59690eebe2 179 int i=0;
alcocerg 0:1f59690eebe2 180 int j=0;
alcocerg 0:1f59690eebe2 181 while(i<length && line[i]==' ') {
alcocerg 0:1f59690eebe2 182 i++;}
alcocerg 0:1f59690eebe2 183 while(i<length && line[i]!=' ') {
alcocerg 0:1f59690eebe2 184 command[j]=line[i];
alcocerg 0:1f59690eebe2 185 i++;
alcocerg 0:1f59690eebe2 186 j++;}
alcocerg 0:1f59690eebe2 187 command[j]=0;
alcocerg 0:1f59690eebe2 188 i++;
alcocerg 0:1f59690eebe2 189 j=0;
alcocerg 0:1f59690eebe2 190 while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
alcocerg 0:1f59690eebe2 191 i++;
alcocerg 0:1f59690eebe2 192 j++;}
alcocerg 0:1f59690eebe2 193 if(j>0) {
alcocerg 0:1f59690eebe2 194 result++;
alcocerg 0:1f59690eebe2 195 i--;
alcocerg 0:1f59690eebe2 196 int k=1;
alcocerg 0:1f59690eebe2 197 while(j>0) {
alcocerg 0:1f59690eebe2 198 parameter += (line[i]-'0')*k;
alcocerg 0:1f59690eebe2 199 j--;
alcocerg 0:1f59690eebe2 200 i--;
alcocerg 0:1f59690eebe2 201 k=k*10;}
alcocerg 0:1f59690eebe2 202 }
alcocerg 0:1f59690eebe2 203 checkCommand(result, command);
alcocerg 0:1f59690eebe2 204 }
alcocerg 0:1f59690eebe2 205
alcocerg 0:1f59690eebe2 206 void storeC(char c) {
alcocerg 0:1f59690eebe2 207 if(c==10 || c==13|| commandPosition >= 255) {
alcocerg 0:1f59690eebe2 208 split(commandLine, commandPosition);
alcocerg 0:1f59690eebe2 209 commandPosition=0;}
alcocerg 0:1f59690eebe2 210 else {
alcocerg 0:1f59690eebe2 211 commandLine[commandPosition++]=c;
alcocerg 0:1f59690eebe2 212 commandLine[commandPosition]=0;}
alcocerg 0:1f59690eebe2 213 }
alcocerg 0:1f59690eebe2 214
alcocerg 0:1f59690eebe2 215 /*void getBT() {
alcocerg 0:1f59690eebe2 216 char c = bt_uart.getc();
alcocerg 0:1f59690eebe2 217 storeC(c);
alcocerg 0:1f59690eebe2 218 }*/
alcocerg 0:1f59690eebe2 219
alcocerg 0:1f59690eebe2 220 void getPC() {
alcocerg 0:1f59690eebe2 221 char c = pc_uart.getc();
alcocerg 0:1f59690eebe2 222 storeC(c);
alcocerg 0:1f59690eebe2 223 }
alcocerg 0:1f59690eebe2 224
alcocerg 0:1f59690eebe2 225 /* Stop all processes */
alcocerg 0:1f59690eebe2 226 void stop_all()
alcocerg 0:1f59690eebe2 227 {
alcocerg 0:1f59690eebe2 228 mybot.stopMove();
alcocerg 0:1f59690eebe2 229 }
alcocerg 0:1f59690eebe2 230
alcocerg 0:1f59690eebe2 231 /* Interrupt routine, switch of end of course */
alcocerg 0:1f59690eebe2 232 void clicked()
alcocerg 0:1f59690eebe2 233 {
alcocerg 0:1f59690eebe2 234 // DEBUG("Labyrinthe\n\r");
alcocerg 0:1f59690eebe2 235 commandRECEIVED = 'l';
alcocerg 0:1f59690eebe2 236 receivedCOMMAND = true;
alcocerg 0:1f59690eebe2 237 }
alcocerg 0:1f59690eebe2 238
alcocerg 0:1f59690eebe2 239 void endOfMove(int status) {
alcocerg 0:1f59690eebe2 240 DEBUG("ENDOFMOVE\n\r");
alcocerg 0:1f59690eebe2 241 state=0;
alcocerg 0:1f59690eebe2 242 }
alcocerg 0:1f59690eebe2 243
alcocerg 0:1f59690eebe2 244 /* Main Routine */
alcocerg 0:1f59690eebe2 245 int main()
alcocerg 0:1f59690eebe2 246 {
alcocerg 0:1f59690eebe2 247 wait(1);
alcocerg 0:1f59690eebe2 248
alcocerg 0:1f59690eebe2 249 /* Connect EoC button */
alcocerg 0:1f59690eebe2 250 CATCH_BUTTON(buttonBox,clicked);
alcocerg 0:1f59690eebe2 251
alcocerg 0:1f59690eebe2 252 /* Init 6180 X-Nucleo */
alcocerg 0:1f59690eebe2 253 int status;
alcocerg 0:1f59690eebe2 254 DevI2C *device_i2c = new DevI2C(VL6180X_I2C_SDA, VL6180X_I2C_SCL);
alcocerg 0:1f59690eebe2 255 board = XNucleo6180XA1::instance(device_i2c, A5, A4, D13, D4);
alcocerg 0:1f59690eebe2 256 status = board->init_board();
alcocerg 0:1f59690eebe2 257 if (status) {DEBUG("Failed to init board!\n\r"); }
alcocerg 0:1f59690eebe2 258
alcocerg 0:1f59690eebe2 259 commandPosition=0;
alcocerg 0:1f59690eebe2 260 // bt_uart.attach(getBT);
alcocerg 0:1f59690eebe2 261 pc_uart.attach(getPC);
alcocerg 0:1f59690eebe2 262 mybot.setCallBack(endOfMove);
alcocerg 0:1f59690eebe2 263 mybot.setSpeed(5); // max 8 cm.s, average 5 cm.s
alcocerg 0:1f59690eebe2 264 ledBand.SetColor(BLACK);
alcocerg 0:1f59690eebe2 265 state=0;
alcocerg 0:1f59690eebe2 266 receivedCOMMAND = false;
alcocerg 0:1f59690eebe2 267 DEBUG("OvniBot\n\r");
alcocerg 0:1f59690eebe2 268 while(1) {
alcocerg 0:1f59690eebe2 269 if(state==0 && receivedCOMMAND) {
alcocerg 0:1f59690eebe2 270 receivedCOMMAND = false;
alcocerg 0:1f59690eebe2 271 state=1;
alcocerg 0:1f59690eebe2 272 executeCommand(commandRECEIVED);
alcocerg 0:1f59690eebe2 273 }
alcocerg 0:1f59690eebe2 274 wait (0.3);
alcocerg 0:1f59690eebe2 275 board->sensor_bottom->get_distance(&dist) ;
alcocerg 0:1f59690eebe2 276 DEBUG ("Distance bottom: %d \n\r", dist);
alcocerg 0:1f59690eebe2 277 if (dist<100) {
alcocerg 0:1f59690eebe2 278 commandRECEIVED = 'l';
alcocerg 0:1f59690eebe2 279 executeCommand(commandRECEIVED);}
alcocerg 0:1f59690eebe2 280 wait (0.3);
alcocerg 0:1f59690eebe2 281 board->sensor_top->get_distance(&dist) ;
alcocerg 0:1f59690eebe2 282 DEBUG ("Distance top: %d \n\r", dist);
alcocerg 0:1f59690eebe2 283 if (dist<100) {
alcocerg 0:1f59690eebe2 284 commandRECEIVED = 's';
alcocerg 0:1f59690eebe2 285 executeCommand(commandRECEIVED);}
alcocerg 0:1f59690eebe2 286 wait(0.1);
alcocerg 0:1f59690eebe2 287 } }