CreaLab / Mbed 2 deprecated StockBot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed CreaBotLib MotorLib

main.cpp@0:63321f7d8d0c, 2019-04-01 (annotated)

Committer:
alcocerg
Date:
Mon Apr 01 16:37:48 2019 +0000
Revision:
0:63321f7d8d0c
Child:
1:97b1890508bb
New

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alcocerg 0:63321f7d8d0c 1 /*
alcocerg 0:63321f7d8d0c 2 * Crealab : version Scratch
alcocerg 0:63321f7d8d0c 3 */
alcocerg 0:63321f7d8d0c 4
alcocerg 0:63321f7d8d0c 5 #undef printf
alcocerg 0:63321f7d8d0c 6 #include "Crealab.h"
alcocerg 0:63321f7d8d0c 7
alcocerg 0:63321f7d8d0c 8 Serial pc_uart(USBTX, USBRX); // USBTX = PA2
alcocerg 0:63321f7d8d0c 9 Serial bt_uart(PA_9, PA_10); // PA9 = Tx, PA10 = Rx
alcocerg 0:63321f7d8d0c 10 uint32_t go;
alcocerg 0:63321f7d8d0c 11
alcocerg 0:63321f7d8d0c 12 // ---------------- PIN DEFINITIONS ---------------------
alcocerg 0:63321f7d8d0c 13
alcocerg 0:63321f7d8d0c 14 InterruptIn buttonBox(PA_12);
alcocerg 0:63321f7d8d0c 15
alcocerg 0:63321f7d8d0c 16 // --- Define the Four PINs & Time of movement used for Motor drive
alcocerg 0:63321f7d8d0c 17 Motor motorRD(PA_4, PA_3, PA_1, PA_0);
alcocerg 0:63321f7d8d0c 18 Motor motorRG(PA_8, PA_11, PB_5, PB_4);
alcocerg 0:63321f7d8d0c 19 Creabot mybot(&motorRG, &motorRD, 8.14f,9.6f); // insert the motors and indicate wheel diameter and distance between wheels
alcocerg 0:63321f7d8d0c 20
alcocerg 0:63321f7d8d0c 21 bool receivedCOMMAND;
alcocerg 0:63321f7d8d0c 22 char commandRECEIVED;
alcocerg 0:63321f7d8d0c 23 int parameterRECEIVED;
alcocerg 0:63321f7d8d0c 24 int state;
alcocerg 0:63321f7d8d0c 25 char commandLine[256];
alcocerg 0:63321f7d8d0c 26 int commandPosition;
alcocerg 0:63321f7d8d0c 27
alcocerg 0:63321f7d8d0c 28 void help() // Display list of Commands
alcocerg 0:63321f7d8d0c 29 {
alcocerg 0:63321f7d8d0c 30 DEBUG("List of commands:\n\r");
alcocerg 0:63321f7d8d0c 31 DEBUG(" h --> Help, display list of commands\n\r");
alcocerg 0:63321f7d8d0c 32 }
alcocerg 0:63321f7d8d0c 33
alcocerg 0:63321f7d8d0c 34 void callback() {
alcocerg 0:63321f7d8d0c 35 switch(state){
alcocerg 0:63321f7d8d0c 36 case 0:
alcocerg 0:63321f7d8d0c 37 break;
alcocerg 0:63321f7d8d0c 38 case 1:
alcocerg 0:63321f7d8d0c 39 break;
alcocerg 0:63321f7d8d0c 40 case 2:
alcocerg 0:63321f7d8d0c 41 break;
alcocerg 0:63321f7d8d0c 42 case 3:
alcocerg 0:63321f7d8d0c 43 break;
alcocerg 0:63321f7d8d0c 44 default:
alcocerg 0:63321f7d8d0c 45 }
alcocerg 0:63321f7d8d0c 46 state=state+1;
alcocerg 0:63321f7d8d0c 47 }
alcocerg 0:63321f7d8d0c 48
alcocerg 0:63321f7d8d0c 49 /* Stop all processes */
alcocerg 0:63321f7d8d0c 50 void stop_all()
alcocerg 0:63321f7d8d0c 51 {
alcocerg 0:63321f7d8d0c 52 mybot.stopMove();
alcocerg 0:63321f7d8d0c 53 }
alcocerg 0:63321f7d8d0c 54
alcocerg 0:63321f7d8d0c 55 void clicked()
alcocerg 0:63321f7d8d0c 56 {
alcocerg 0:63321f7d8d0c 57 DEBUG("Labyrinthe\n\r");
alcocerg 0:63321f7d8d0c 58 commandRECEIVED = 'l';
alcocerg 0:63321f7d8d0c 59 receivedCOMMAND = true;
alcocerg 0:63321f7d8d0c 60 }
alcocerg 0:63321f7d8d0c 61
alcocerg 0:63321f7d8d0c 62 void labyrinthe()
alcocerg 0:63321f7d8d0c 63 {
alcocerg 0:63321f7d8d0c 64 wait(1);
alcocerg 0:63321f7d8d0c 65 mybot.move(FORWARD,15);
alcocerg 0:63321f7d8d0c 66 mybot.waitEndMove();
alcocerg 0:63321f7d8d0c 67 mybot.move(ROTATE, 90);
alcocerg 0:63321f7d8d0c 68 mybot.waitEndMove();
alcocerg 0:63321f7d8d0c 69 mybot.move(FORWARD,15);
alcocerg 0:63321f7d8d0c 70 mybot.waitEndMove();
alcocerg 0:63321f7d8d0c 71 mybot.move(ROTATE, 90);
alcocerg 0:63321f7d8d0c 72 mybot.waitEndMove();
alcocerg 0:63321f7d8d0c 73 }
alcocerg 0:63321f7d8d0c 74
alcocerg 0:63321f7d8d0c 75 void executeCommand(char c, int parameter) {
alcocerg 0:63321f7d8d0c 76 bool flaghelp = false;
alcocerg 0:63321f7d8d0c 77 switch (c) {
alcocerg 0:63321f7d8d0c 78 case 'h':
alcocerg 0:63321f7d8d0c 79 help();
alcocerg 0:63321f7d8d0c 80 state=0;
alcocerg 0:63321f7d8d0c 81 flaghelp=true;
alcocerg 0:63321f7d8d0c 82 CASE('a', "Avance <cm> (a <cm>) => Avance du nombre de cm indiques", mybot.move(FORWARD,parameter); )
alcocerg 0:63321f7d8d0c 83 CASE('r', "Recule <cm> (r <cm>) => Recule du nombre de cm indiques", mybot.move(BACKWARD,parameter); )
alcocerg 0:63321f7d8d0c 84 CASE('d', "Droite <deg> (d <deg>) => Tourne a droite du nombre de degres indiques", mybot.move(RIGHT,parameter,0); )
alcocerg 0:63321f7d8d0c 85 CASE('g', "Gauche <deg> (g <deg>) => Tourne a gauche du nombre de degres indiques", mybot.move(LEFT,parameter,0); )
alcocerg 0:63321f7d8d0c 86 CASE('p', "Pivote_d <deh> (p <deg>) => pivote a droite du nombre de degres indiques", mybot.move(ROTATE,parameter); )
alcocerg 0:63321f7d8d0c 87 CASE('q', "Pivote_g <deg> (q <deg>) => pivote a gauche du nombre de degres indiques", mybot.move(ROTATE,-parameter); )
alcocerg 0:63321f7d8d0c 88 CASE('s', "Stop => Arrete les moteurs", mybot.stopMove(); state=0; )
alcocerg 0:63321f7d8d0c 89 CASE('l', "Labyrinthe => Lance le parcours Labyrinthe", labyrinthe(); )
alcocerg 0:63321f7d8d0c 90 default :
alcocerg 0:63321f7d8d0c 91 DEBUG("invalid command; use: 'h' for help()\n\r");
alcocerg 0:63321f7d8d0c 92 state=0;
alcocerg 0:63321f7d8d0c 93 }}
alcocerg 0:63321f7d8d0c 94
alcocerg 0:63321f7d8d0c 95 void analyseCommand(char *command, int parameter) {
alcocerg 0:63321f7d8d0c 96 parameterRECEIVED = parameter;
alcocerg 0:63321f7d8d0c 97 switch(command[0]) {
alcocerg 0:63321f7d8d0c 98 case 'A':
alcocerg 0:63321f7d8d0c 99 case 'a':
alcocerg 0:63321f7d8d0c 100 commandRECEIVED = 'a';
alcocerg 0:63321f7d8d0c 101 break;
alcocerg 0:63321f7d8d0c 102 case 'R':
alcocerg 0:63321f7d8d0c 103 case 'r':
alcocerg 0:63321f7d8d0c 104 commandRECEIVED = 'r';
alcocerg 0:63321f7d8d0c 105 break;
alcocerg 0:63321f7d8d0c 106 case 'D':
alcocerg 0:63321f7d8d0c 107 case 'd':
alcocerg 0:63321f7d8d0c 108 commandRECEIVED = 'd';
alcocerg 0:63321f7d8d0c 109 break;
alcocerg 0:63321f7d8d0c 110 case 'G':
alcocerg 0:63321f7d8d0c 111 case 'g':
alcocerg 0:63321f7d8d0c 112 commandRECEIVED = 'g';
alcocerg 0:63321f7d8d0c 113 break;
alcocerg 0:63321f7d8d0c 114 case 'L':
alcocerg 0:63321f7d8d0c 115 case 'l':
alcocerg 0:63321f7d8d0c 116 commandRECEIVED = 'l';
alcocerg 0:63321f7d8d0c 117 break;
alcocerg 0:63321f7d8d0c 118 case 'P':
alcocerg 0:63321f7d8d0c 119 if(command[7]=='d') {
alcocerg 0:63321f7d8d0c 120 commandRECEIVED = 'p';
alcocerg 0:63321f7d8d0c 121 } else if (command[7]=='g') {
alcocerg 0:63321f7d8d0c 122 commandRECEIVED = 'q';
alcocerg 0:63321f7d8d0c 123 } else {
alcocerg 0:63321f7d8d0c 124 commandRECEIVED = 'h';
alcocerg 0:63321f7d8d0c 125 }
alcocerg 0:63321f7d8d0c 126 break;
alcocerg 0:63321f7d8d0c 127 case 'p':
alcocerg 0:63321f7d8d0c 128 commandRECEIVED = 'p';
alcocerg 0:63321f7d8d0c 129 break;
alcocerg 0:63321f7d8d0c 130 case 'q':
alcocerg 0:63321f7d8d0c 131 commandRECEIVED = 'q';
alcocerg 0:63321f7d8d0c 132 break;
alcocerg 0:63321f7d8d0c 133 case 'S':
alcocerg 0:63321f7d8d0c 134 case 's':
alcocerg 0:63321f7d8d0c 135 commandRECEIVED = 's';
alcocerg 0:63321f7d8d0c 136 mybot.stopMove(); state=0;
alcocerg 0:63321f7d8d0c 137 break;
alcocerg 0:63321f7d8d0c 138 default:
alcocerg 0:63321f7d8d0c 139 commandRECEIVED = 'h';
alcocerg 0:63321f7d8d0c 140 } }
alcocerg 0:63321f7d8d0c 141
alcocerg 0:63321f7d8d0c 142 void checkCommand(int result, char *command, int parameter) {
alcocerg 0:63321f7d8d0c 143 if(result==2 || command[0]=='h' || command[0]=='s') {
alcocerg 0:63321f7d8d0c 144 if(command[0]=='c') {
alcocerg 0:63321f7d8d0c 145 DEBUG("CREABOT PRESENT\n\r");
alcocerg 0:63321f7d8d0c 146 } else {
alcocerg 0:63321f7d8d0c 147 analyseCommand(command, parameter);
alcocerg 0:63321f7d8d0c 148 receivedCOMMAND = true;
alcocerg 0:63321f7d8d0c 149 }
alcocerg 0:63321f7d8d0c 150 } }
alcocerg 0:63321f7d8d0c 151
alcocerg 0:63321f7d8d0c 152 void split(char *line, int length) {
alcocerg 0:63321f7d8d0c 153 char command[256];
alcocerg 0:63321f7d8d0c 154 int parameter=0;
alcocerg 0:63321f7d8d0c 155 int result = 1;
alcocerg 0:63321f7d8d0c 156 int i=0;
alcocerg 0:63321f7d8d0c 157 int j=0;
alcocerg 0:63321f7d8d0c 158 while(i<length && line[i]==' ') {
alcocerg 0:63321f7d8d0c 159 i++;
alcocerg 0:63321f7d8d0c 160 }
alcocerg 0:63321f7d8d0c 161 while(i<length && line[i]!=' ') {
alcocerg 0:63321f7d8d0c 162 command[j]=line[i];
alcocerg 0:63321f7d8d0c 163 i++;
alcocerg 0:63321f7d8d0c 164 j++;
alcocerg 0:63321f7d8d0c 165 }
alcocerg 0:63321f7d8d0c 166 command[j]=0;
alcocerg 0:63321f7d8d0c 167 i++;
alcocerg 0:63321f7d8d0c 168 j=0;
alcocerg 0:63321f7d8d0c 169 while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
alcocerg 0:63321f7d8d0c 170 i++;
alcocerg 0:63321f7d8d0c 171 j++;
alcocerg 0:63321f7d8d0c 172 }
alcocerg 0:63321f7d8d0c 173 if(j>0) {
alcocerg 0:63321f7d8d0c 174 result++;
alcocerg 0:63321f7d8d0c 175 i--;
alcocerg 0:63321f7d8d0c 176 int k=1;
alcocerg 0:63321f7d8d0c 177 while(j>0) {
alcocerg 0:63321f7d8d0c 178 parameter += (line[i]-'0')*k;
alcocerg 0:63321f7d8d0c 179 j--;
alcocerg 0:63321f7d8d0c 180 i--;
alcocerg 0:63321f7d8d0c 181 k=k*10;
alcocerg 0:63321f7d8d0c 182 }
alcocerg 0:63321f7d8d0c 183 }
alcocerg 0:63321f7d8d0c 184 checkCommand(result, command, parameter);
alcocerg 0:63321f7d8d0c 185 }
alcocerg 0:63321f7d8d0c 186
alcocerg 0:63321f7d8d0c 187 void storeC(char c) {
alcocerg 0:63321f7d8d0c 188 if(c==10 || c==13|| commandPosition >= 254) {
alcocerg 0:63321f7d8d0c 189 split(commandLine, commandPosition);
alcocerg 0:63321f7d8d0c 190 commandPosition=0;
alcocerg 0:63321f7d8d0c 191 } else {
alcocerg 0:63321f7d8d0c 192 commandLine[commandPosition++]=c;
alcocerg 0:63321f7d8d0c 193 commandLine[commandPosition]=0;
alcocerg 0:63321f7d8d0c 194 }
alcocerg 0:63321f7d8d0c 195 }
alcocerg 0:63321f7d8d0c 196
alcocerg 0:63321f7d8d0c 197 void getBT() {
alcocerg 0:63321f7d8d0c 198 char c = bt_uart.getc();
alcocerg 0:63321f7d8d0c 199 storeC(c);
alcocerg 0:63321f7d8d0c 200 }
alcocerg 0:63321f7d8d0c 201
alcocerg 0:63321f7d8d0c 202 void getPC() {
alcocerg 0:63321f7d8d0c 203 char c = pc_uart.getc();
alcocerg 0:63321f7d8d0c 204 storeC(c);
alcocerg 0:63321f7d8d0c 205 }
alcocerg 0:63321f7d8d0c 206
alcocerg 0:63321f7d8d0c 207 void endOfMove(int status) {
alcocerg 0:63321f7d8d0c 208 DEBUG("ENDOFMOVE\n\r");
alcocerg 0:63321f7d8d0c 209 state=0;
alcocerg 0:63321f7d8d0c 210 }
alcocerg 0:63321f7d8d0c 211
alcocerg 0:63321f7d8d0c 212 /* Main Routine */
alcocerg 0:63321f7d8d0c 213 int main()
alcocerg 0:63321f7d8d0c 214 {
alcocerg 0:63321f7d8d0c 215 /* Connect EoC button */
alcocerg 0:63321f7d8d0c 216 CATCH_BUTTON(buttonBox,clicked);
alcocerg 0:63321f7d8d0c 217
alcocerg 0:63321f7d8d0c 218 commandPosition=0;
alcocerg 0:63321f7d8d0c 219 bt_uart.attach(getBT);
alcocerg 0:63321f7d8d0c 220 pc_uart.attach(getPC);
alcocerg 0:63321f7d8d0c 221 mybot.setCallBack(endOfMove);
alcocerg 0:63321f7d8d0c 222 mybot.setSpeed(6.5); // max 8 cm.s, average 5 cm.s
alcocerg 0:63321f7d8d0c 223 state=0;
alcocerg 0:63321f7d8d0c 224 receivedCOMMAND = false;
alcocerg 0:63321f7d8d0c 225 DEBUG("CREABOT alpha version\n\r");
alcocerg 0:63321f7d8d0c 226 while(1) {
alcocerg 0:63321f7d8d0c 227 if(state==0 && receivedCOMMAND) {
alcocerg 0:63321f7d8d0c 228 receivedCOMMAND = false;
alcocerg 0:63321f7d8d0c 229 state=1;
alcocerg 0:63321f7d8d0c 230 executeCommand(commandRECEIVED, parameterRECEIVED);
alcocerg 0:63321f7d8d0c 231 }
alcocerg 0:63321f7d8d0c 232 wait(0.1);
alcocerg 0:63321f7d8d0c 233 }
alcocerg 0:63321f7d8d0c 234 }