First release
Dependencies: CreaBotLib LED_WS2812 MotorLib X_NUCLEO_6180XA1 mbed
main.cpp
- Committer:
- alcocerg
- Date:
- 2018-09-24
- Revision:
- 1:3589e8f6e99c
- Parent:
- 0:1f59690eebe2
File content as of revision 1:3589e8f6e99c:
/* * OvniBot * */ #include "XNucleo6180XA1.h" #undef printf #include "Crealab.h" Serial bt_uart(PA_9, PA_10); // PA9 = Tx, PA10 = Rx Serial pc_uart(USBTX, USBRX); // USBTX = PA2, USBRX = PA3 // ---------------- PIN DEFINITIONS --------------------- InterruptIn buttonBox(PC_13); Motor motorRG(PB_13, PB_14, PB_15, PB_1); Motor motorRD(PA_0, PA_1, PA_4, PB_0); // insert the motors and indicate wheel diameter and distance between wheels Creabot mybot(&motorRG, &motorRD, 9.0f,13.2f); // proto CréaLab // PIN & number of LEDS. Available color ==> BLUE, ORANGE, RED, GREEN, BLACK, WHITE, PURPLE, PINK, YELLOW LED_WS2812 ledBand(PB_3,4); bool receivedCOMMAND; char commandRECEIVED; int parameterRECEIVED; int state; char commandLine[256]; int commandPosition; uint32_t dist; // ----------- PIN Definitions 6180 ------------- #define VL6180X_I2C_SDA D14 #define VL6180X_I2C_SCL D15 static XNucleo6180XA1 *board = NULL; // penser à modifier dans vl6180x_platform.h -> commenter la ligne // #define MY_LOG 1 void help() // Display list of Commands { DEBUG("List of commands:\n\r"); DEBUG(" h --> Help, display list of commands\n\r"); } void callback() { switch(state){ case 0: break; case 1: break; case 2: break; case 3: break; default: } state=state+1; } void detect(int parameter) { mybot.move(FORWARD,300); dist=250; while (dist>parameter) {wait (0.2); board->sensor_bottom->get_distance(&dist) ; } // DEBUG ("Distance: %d \n\r", dist); mybot.stopMove(); } void routine1() { DEBUG("routine1\n\r"); wait(1); mybot.move(FORWARD,200); ledBand.SetColor(BLUE,0) ; ledBand.SetColor(BLACK,1) ; ledBand.SetColor(BLACK,2) ; ledBand.SetColor(BLACK,3) ; ledBand.StartRotation(0.5) ; wait(2); ledBand.SetColor(WHITE,0) ; ledBand.SetColor(BLACK,1) ; ledBand.SetColor(BLACK,2) ; ledBand.SetColor(BLACK,3) ; ledBand.StartRotation(0.5) ; state=0; /* mybot.move(FORWARD,9); dist=200; while (dist>120) {wait (0.3); board->sensor_bottom->get_distance(&dist) ; } DEBUG ("Distance: %d \n\r", dist); mybot.stopMove(); ledBand.SetColor(WHITE,0) ; ledBand.SetColor(RED,1) ; ledBand.SetColor(BLUE,2) ; ledBand.SetColor(GREEN,3) ; */ } void routine2() { DEBUG("routine2\n\r"); wait(1); ledBand.SetColor(WHITE,0) ; ledBand.SetColor(RED,1) ; ledBand.SetColor(BLUE,2) ; ledBand.SetColor(GREEN,3) ; ledBand.StartRotation(0.6) ; mybot.move(BACKWARD,200); state=0; /* mybot.move(FORWARD,9); dist=200; while (dist>120) {wait (0.3); board->sensor_bottom->get_distance(&dist) ; } DEBUG ("Distance: %d \n\r", dist); mybot.stopMove(); */ } void routines() { DEBUG("routines\n\r"); wait(1); ledBand.StopRotation(); ledBand.SetColor(BLACK,0) ; ledBand.SetColor(BLACK,1) ; ledBand.SetColor(BLACK,2) ; ledBand.SetColor(BLACK,3) ; mybot.stopMove(); state=0; } void executeCommand(char c) { bool flaghelp = false; switch (c) { case 'h': help(); state=0; flaghelp=true; CASE('l', "lance la routine 1", routine1(); ) CASE('m', "lance la routine 2", routine2(); ) CASE('s', "stoppe les routines", routines(); ) CASE('t', "accelere", routines(); ) default : DEBUG("invalid command; use: 'h' for help()\n\r"); state=0; }} void analyseCommand(char *command) { switch(command[0]) { case 'l': commandRECEIVED = 'l'; break; case 's': commandRECEIVED = 's'; break; case 'm': commandRECEIVED = 'm'; break; case 't': commandRECEIVED = 't'; break; default: commandRECEIVED = 'h'; } } void checkCommand(int result, char *command) { if(result==1) { analyseCommand(command); // DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED,state); receivedCOMMAND = true; } } void split(char *line, int length) { char command[256]; int parameter=0; int result = 1; int i=0; int j=0; while(i<length && line[i]==' ') { i++;} while(i<length && line[i]!=' ') { command[j]=line[i]; i++; j++;} command[j]=0; i++; j=0; while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') { i++; j++;} if(j>0) { result++; i--; int k=1; while(j>0) { parameter += (line[i]-'0')*k; j--; i--; k=k*10;} } checkCommand(result, command); } void storeC(char c) { if(c==10 || c==13|| commandPosition >= 255) { split(commandLine, commandPosition); commandPosition=0;} else { commandLine[commandPosition++]=c; commandLine[commandPosition]=0;} } /*void getBT() { char c = bt_uart.getc(); storeC(c); }*/ void getPC() { char c = pc_uart.getc(); storeC(c); } /* Stop all processes */ void stop_all() { mybot.stopMove(); } /* Interrupt routine, switch of end of course */ void clicked() { // DEBUG("Labyrinthe\n\r"); commandRECEIVED = 'l'; receivedCOMMAND = true; } void endOfMove(int status) { DEBUG("ENDOFMOVE\n\r"); state=0; } /* Main Routine */ int main() { wait(1); /* Connect EoC button */ CATCH_BUTTON(buttonBox,clicked); /* Init 6180 X-Nucleo */ int status; DevI2C *device_i2c = new DevI2C(VL6180X_I2C_SDA, VL6180X_I2C_SCL); board = XNucleo6180XA1::instance(device_i2c, A5, A4, D13, D4); status = board->init_board(); if (status) {DEBUG("Failed to init board!\n\r"); } commandPosition=0; // bt_uart.attach(getBT); pc_uart.attach(getPC); mybot.setCallBack(endOfMove); mybot.setSpeed(5); // max 8 cm.s, average 5 cm.s ledBand.SetColor(BLACK); state=0; receivedCOMMAND = false; DEBUG("OvniBot\n\r"); while(1) { if(state==0 && receivedCOMMAND) { receivedCOMMAND = false; state=1; executeCommand(commandRECEIVED); } wait (0.3); board->sensor_bottom->get_distance(&dist) ; DEBUG ("Distance bottom: %d \n\r", dist); if (dist<100) { commandRECEIVED = 'l'; executeCommand(commandRECEIVED);} wait (0.3); board->sensor_top->get_distance(&dist) ; DEBUG ("Distance top: %d \n\r", dist); if (dist<100) { commandRECEIVED = 's'; executeCommand(commandRECEIVED);} wait(0.1); } }