Diff: main.cpp

Revision:

0:1f59690eebe2

Child:

1:3589e8f6e99c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 19 14:20:26 2018 +0000
@@ -0,0 +1,276 @@
+/*
+ *   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);
+     ledBand.SetColor(WHITE,0) ;
+     ledBand.SetColor(RED,1) ;
+     ledBand.SetColor(BLUE,2) ;
+     ledBand.SetColor(GREEN,3) ;
+     ledBand.StartRotation(1.0) ;
+     mybot.move(FORWARD,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 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.StopBlink() ;
+     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();  )
+                            
+            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;
+
+       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);
+    } }