MaszeSolver
Dependencies: mbed FileSystem_POPS m3pi
main3.cpp
- Committer:
- sylia
- Date:
- 2019-01-28
- Revision:
- 10:aba1c7cff0cf
- Parent:
- 9:5701616e3a2c
File content as of revision 10:aba1c7cff0cf:
#include "mbed.h" #include "m3pi.h" #include "MSCFileSystem.h" m3pi m3pi; // Initialise the m3pi char *replaceWord(const char *s, const char *oldW, const char *newW); Serial xbee(p28,p27); DigitalOut resetxbee(p26); Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging Timer t; Ticker tick1; char affichage[3]={0}; char maze[100]={0}; char *optm; BusOut myleds(LED1, LED2, LED3, LED4); #define D_TERM 0.0 #define I_TERM 0.1 #define I_TERMO 0.1 #define P_TERM 0.9 #define MAX 0.3 #define MIN -0.2 #define seuil(x) (x>300 ? 1 : 0) float current_pos_of_line, previous_pos_of_line, derivate, proportional, power, integral, left, right, speed=0.3; char chain[10]; int j = 0; unsigned short tabsensor[5]; volatile unsigned char sensors; volatile char flag10ms; char command=1; void inter1() { flag10ms=1; } void current_state() { unsigned char i; sensors=0; m3pi.calibrated_sensors(tabsensor); for(i=0; i<5; i++) { sensors = (sensors << 1) | seuil(tabsensor[i]); } } void step() { m3pi.forward(0.12); wait(0.22); } void step2() { m3pi.forward(0.12); wait(0.06); } /* * Results * 1 -> PID * 2 -> Turn back * 3 -> Turn left * 4 -> Turn right */ char PIDf(char commande) { if(commande==1) { char result; current_state(); switch(sensors) { case 0x00: // Deadend // Back m3pi.cls(); strcat(maze,"B"); m3pi.locate(0,1); m3pi.printf(maze); m3pi.stop(); result = 2; break; case 0x1C: case 0x18: case 0x10: // Forward/Left or Left Only step(); current_state(); if ((sensors == 0x00) || (sensors == 0x10)) { // Turn Left m3pi.cls(); strcat(maze,"L"); m3pi.locate(0,1); m3pi.printf(maze); result = 3; } else { // Forward m3pi.cls(); strcat(maze,"F"); m3pi.locate(0,1); m3pi.printf(maze); step(); result = 1; } break; case 0x07: case 0x03: case 0x01: // Forward/Right or Right Only m3pi.cls(); strcat(maze,"R"); m3pi.locate(0,1); m3pi.printf(maze); step(); result = 4; break; case 0x1F: // 'T' or Intersection or End step(); current_state(); if (sensors == 0x1F) { // End m3pi.cls(); strcat(maze,"E"); m3pi.locate(0,1); m3pi.printf(maze); m3pi.stop(); result = 5; } else { // 'T' -> Turn Right m3pi.cls(); strcat(maze, "R"); m3pi.locate(0,1); m3pi.printf(maze); result = 4; } break; case 0x0F: case 0x1E: // 'T' or Intersection or End -> LR or RFL step2(); current_state(); if (sensors == 0x1F) { // End m3pi.cls(); strcat(maze,"E"); m3pi.locate(0,1); m3pi.printf(maze); m3pi.stop(); result = 5; } else if ((sensors == 0x10) || (sensors == 0x18)) { // Turn Left m3pi.cls(); strcat(maze,"L"); m3pi.locate(0,1); m3pi.printf(maze); step2(); step2(); result = 3; } else if ((sensors == 0x14) || (sensors == 0x16) || (sensors == 0x06) || (sensors == 0x1C)) { // Forward m3pi.cls(); strcat(maze,"F"); m3pi.locate(0,1); m3pi.printf(maze); step2(); step2(); result = 1; } else { // Turn Right m3pi.cls(); strcat(maze, "R"); m3pi.locate(0,1); m3pi.printf(maze); step2(); step2(); result = 4; } break; case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08: //PID // Get the position of the line current_pos_of_line = m3pi.line_position(); proportional = current_pos_of_line; // Compute the derivate derivate = current_pos_of_line - previous_pos_of_line; // Compute the integral integral = (integral+I_TERMO*proportional)/(1+I_TERMO); // Remember the last postion previous_pos_of_line = current_pos_of_line; // Compute the power power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM)); // Compute new speeds right = speed-(power*MAX); left = speed+(power*MAX); // Limits checks on motor control right = (right>MAX ? MAX :(right<MIN ? MIN : right)); left = (left>MAX ? MAX :(left<MIN ? MIN : left)); // Send command to motors m3pi.left_motor(left); m3pi.right_motor(right); result = 1; break; default: // Faire rien m3pi.stop(); result = 0; break; } return result; } } /* * Results * 1 -> PID * 2 -> Turn back * 3 -> Turn left * 4 -> Turn right */ char turn(char command) { if(command > 1 && command < 5) { char result; current_state(); switch(command) { case 2: // Turn Back if(sensors != 0x01) { m3pi.right(speed); result = 2; } else { do{ current_state(); m3pi.right(0.4*speed); }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); m3pi.stop(); result = 1; } break; case 3: // Turn Left if(sensors != 0x10) { m3pi.left(speed); result = 3; } else { do{ current_state(); m3pi.left(0.4*speed); }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); m3pi.stop(); result = 1; } break; case 4: // Turn Right if(sensors != 0x01) { m3pi.right(speed); result = 4; } else { do{ current_state(); m3pi.right(0.4*speed); }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); m3pi.stop(); result = 1; } break; } return result; } } char find(char str[], char c) { unsigned char len = strlen(str); int i; for(i = 0; i < len; i++) { if(str[i] == c) return 1; } return 0; } char *replaceWord(const char *s, const char *oldW, const char *newW) { char *result; int i, cnt = 0; int newWlen = strlen(newW); int oldWlen = strlen(oldW); // Counting the number of times old word // occur in the string for (i = 0; s[i] != '\0'; i++) { if (strstr(&s[i], oldW) == &s[i]) { cnt++; // Jumping to index after the old word. i += oldWlen - 1; } } // Making new string of enough length result = (char *)malloc(i + cnt * (newWlen - oldWlen) + 1); i = 0; while (*s) { // compare the substring with the result if (strstr(s, oldW) == s) { strcpy(&result[i], newW); i += newWlen; s += oldWlen; } else result[i++] = *s++; } result[i] = '\0'; return result; } char *optimiser(char *str) { char *buf1 = str; char *buf2; while(find(buf1,'B')) { buf2 = replaceWord(buf1, "RBR", "F"); buf1 = replaceWord(buf2, "RBL", "B"); buf2 = replaceWord(buf1, "RBF", "L"); buf1 = replaceWord(buf2, "LBR", "B"); buf2 = replaceWord(buf1, "LBL", "F"); buf1 = replaceWord(buf2, "LBF", "R"); buf2 = replaceWord(buf1, "FBR", "L"); buf1 = replaceWord(buf2, "FBL", "R"); buf2 = replaceWord(buf1, "FBF", "B"); buf1 = buf2; } return buf1; } /* 1 -> PID * 2 -> Turn back * 3 -> Turn left * 4 -> Turn right */ char labyrinth(char commande) { if(commande==6) { int result = 6; m3pi.cls(); m3pi.locate(0, 1); m3pi.printf(optm); current_state(); switch(sensors) { case 0x00: case 0x1C: case 0x18: case 0x10: case 0x07: case 0x03: case 0x1F: case 0x0F: case 0x1E: switch (optm[j]) { case 'B': m3pi.cls(); m3pi.locate(0, 1); m3pi.printf("B"); step(); do{ m3pi.right(speed); current_state(); }while(sensors!=0x01); do{ m3pi.right(0.4*speed); current_state(); }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); m3pi.stop(); break; case 'R': m3pi.cls(); m3pi.locate(0, 1); m3pi.printf("R"); step(); do{ m3pi.right(speed); current_state(); }while(sensors!=0x01); do{ m3pi.right(0.4*speed); current_state(); }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); //wait(0.1); m3pi.stop(); break; case 'L': m3pi.cls(); m3pi.locate(0, 1); m3pi.printf("L"); step(); do{ m3pi.left(speed); current_state(); }while(sensors!=0x10); do{ m3pi.left(0.4*speed); current_state(); }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); //wait(0.1); m3pi.stop(); break; case 'F': m3pi.cls(); m3pi.locate(0, 1); m3pi.printf("F"); step(); break; default: result = 0; break; } j++; break; case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08: //PID // Get the position of the line current_pos_of_line = m3pi.line_position(); proportional = current_pos_of_line; // Compute the derivate derivate = current_pos_of_line - previous_pos_of_line; // Compute the integral integral = (integral+I_TERMO*proportional)/(1+I_TERMO); // Remember the last postion previous_pos_of_line = current_pos_of_line; // Compute the power power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM)); // Compute new speeds right = speed-(power*MAX); left = speed+(power*MAX); // Limits checks on motor control right = (right>MAX ? MAX :(right<MIN ? MIN : right)); left = (left>MAX ? MAX :(left<MIN ? MIN : left)); // Send command to motors m3pi.left_motor(left); m3pi.right_motor(right); break; } return result; } } int main() { resetxbee=0; wait(0.01); resetxbee =1; FILE *p= fopen("/fs/tt.txt","a+"); m3pi.sensor_auto_calibrate(); wait(1.); tick1.attach(&inter1,0.01); // fprintf(p,"ecrire dans la cle USB\r\n"); // fclose(p); while(1) { if(flag10ms==1) { switch(command) { case 0: // Faire Rien m3pi.stop(); break; case 1: // PID command = PIDf(command); break; case 2: case 3: case 4: // 2 -> Back // 3 -> Left // 4 -> Right command = turn(command); break; case 5: // Optimisation optm = optimiser(maze); wait(5.0); m3pi.cls(); m3pi.locate(0, 1); m3pi.printf(optm); command = 6; break; case 6: // Labyrinth command = labyrinth(command); break; } } } }