Diff: main_scratch.h

Revision:

0:63321f7d8d0c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_scratch.h	Mon Apr 01 16:37:48 2019 +0000
@@ -0,0 +1,251 @@
+/*
+ *   Crealab : version Scratch  
+ */
+ 
+#undef printf
+#include "Crealab.h"
+
+Serial pc_uart(USBTX, USBRX); // USBTX = PA2
+Serial bt_uart(PA_9, PA_10);  // PA9 = Tx, PA10 = Rx
+uint32_t go;
+
+// ---------------- PIN DEFINITIONS ---------------------
+
+InterruptIn  buttonBox(PA_12);
+
+// --- Define the Four PINs & Time of movement used for Motor drive
+Motor motorRD(PA_4, PA_3, PA_1, PA_0);
+Motor motorRG(PA_8, PA_11, PB_5, PB_4);
+Creabot mybot(&motorRG, &motorRD, 8.14f,9.6f); // insert the motors and indicate wheel diameter and distance between wheels
+
+bool receivedCOMMAND;
+char commandRECEIVED;
+int parameterRECEIVED;
+int state;
+char commandLine[256];
+int commandPosition;
+
+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;
+}
+
+/* Stop all processes */
+void stop_all()
+{
+    mybot.stopMove();
+}
+
+void clicked()
+{
+    DEBUG("Labyrinthe\n\r");
+    commandRECEIVED = 'l';
+    receivedCOMMAND = true;
+}
+
+void labyrinthe() 
+{
+     wait(1);
+     mybot.move(FORWARD,35);
+     mybot.waitEndMove();
+     mybot.move(LEFT, 45);
+     mybot.waitEndMove();
+     mybot.move(FORWARD,26);
+     mybot.waitEndMove();
+     mybot.move(RIGHT, 45);
+     mybot.waitEndMove();
+     mybot.move(FORWARD,8);
+     mybot.waitEndMove();
+     mybot.move(BACKWARD, 14); 
+     mybot.waitEndMove();
+     mybot.move(ROTATE, 178);
+     mybot.waitEndMove();
+     mybot.move(FORWARD, 15);
+     mybot.waitEndMove();
+     mybot.move(RIGHT, 90);
+     mybot.waitEndMove();
+     mybot.move(FORWARD,14);
+     mybot.waitEndMove();
+     mybot.move(RIGHT, 85);
+     mybot.waitEndMove();
+     mybot.move(FORWARD,55);
+     mybot.waitEndMove();                        
+}
+
+void executeCommand(char c, int parameter) {
+        bool flaghelp = false;
+        switch (c) {
+            case 'h':
+                help();
+                state=0;
+                flaghelp=true;
+                CASE('a', "Avance   <cm>  (a <cm>)  => Avance du nombre de cm indiques", mybot.move(FORWARD,parameter);    )
+                CASE('r', "Recule   <cm>  (r <cm>)  => Recule du nombre de cm indiques", mybot.move(BACKWARD,parameter);    )
+                CASE('d', "Droite   <deg> (d <deg>) => Tourne a droite du nombre de degres indiques", mybot.move(RIGHT,parameter,0);    )
+                CASE('g', "Gauche   <deg> (g <deg>) => Tourne a gauche du nombre de degres indiques", mybot.move(LEFT,parameter,0);    )
+                CASE('p', "Pivote_d <deh> (p <deg>) => pivote a droite du nombre de degres indiques", mybot.move(ROTATE,parameter);    )
+                CASE('q', "Pivote_g <deg> (q <deg>) => pivote a gauche du nombre de degres indiques", mybot.move(ROTATE,-parameter);    )
+                CASE('s', "Stop                     => Arrete les moteurs", mybot.stopMove(); state=0;  )
+                CASE('l', "Labyrinthe               => Lance le parcours Labyrinthe", labyrinthe();  )
+            default :
+                DEBUG("invalid command; use: 'h' for help()\n\r");
+                state=0;
+        }}
+        
+void analyseCommand(char *command, int parameter) {
+    parameterRECEIVED = parameter;
+    switch(command[0]) {
+        case 'A':
+        case 'a':
+            commandRECEIVED = 'a';
+            break;
+        case 'R':
+        case 'r':
+            commandRECEIVED = 'r';
+             break;
+        case 'D':
+        case 'd':
+            commandRECEIVED = 'd';
+             break;
+        case 'G':
+        case 'g':
+            commandRECEIVED = 'g';
+             break;
+        case 'L':
+        case 'l':
+            commandRECEIVED = 'l';
+             break;
+        case 'P':
+            if(command[7]=='d') {
+               commandRECEIVED = 'p';
+            } else if (command[7]=='g') {
+               commandRECEIVED = 'q';
+            } else {
+               commandRECEIVED = 'h';
+            }
+             break;
+        case 'p':
+            commandRECEIVED = 'p';
+             break;
+        case 'q':
+            commandRECEIVED = 'q';
+             break;
+        case 'S':
+        case 's':
+            commandRECEIVED = 's';
+            mybot.stopMove(); state=0;
+             break;
+       default:
+            commandRECEIVED = 'h';
+    } }
+
+void checkCommand(int result, char *command, int parameter) {
+    if(result==2 || command[0]=='h' || command[0]=='s') {
+        if(command[0]=='c') {
+            DEBUG("CREABOT PRESENT\n\r"); 
+        } else {
+            analyseCommand(command, parameter);        
+            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, parameter);
+}
+
+void storeC(char c) {
+    if(c==10 || c==13|| commandPosition >= 254) {
+       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();
+    // DEBUG("ENTERING INSIDE GETPC '%c' - %d\n\r", c, c);
+    storeC(c);
+}
+
+void endOfMove(int status) {
+    DEBUG("ENDOFMOVE\n\r");
+    state=0;
+}
+
+/* Main Routine */
+int main()
+{
+    /* Connect EoC button */
+    CATCH_BUTTON(buttonBox,clicked);
+
+    commandPosition=0;
+    bt_uart.attach(getBT);
+    pc_uart.attach(getPC);
+    mybot.setCallBack(endOfMove);
+    mybot.setSpeed(6.5); // max 8 cm.s, average 5 cm.s
+    state=0;
+    receivedCOMMAND = false;
+    DEBUG("CREABOT alpha version\n\r");
+    while(1) {
+       if(state==0 && receivedCOMMAND) {
+            receivedCOMMAND = false;
+            state=1;
+            executeCommand(commandRECEIVED, parameterRECEIVED);
+        } 
+        wait(0.1);
+    }
+}
\ No newline at end of file