Final version of Polulu M2 SETI
Dependencies: FileSystem_POPS m3PI_TP_POPS_II2015v0 m3pi mbed
Fork of m3PI_TP_POPS_II2015v0 by
main3.cpp
- Committer:
- diogohideki
- Date:
- 2018-03-07
- Revision:
- 8:de6ca481b136
- Parent:
- 7:ca53c1bde37c
- Child:
- 9:5701616e3a2c
File content as of revision 8:de6ca481b136:
#include "mbed.h" #include "m3pi.h" #include "MSCFileSystem.h" m3pi m3pi; // Initialise the m3pi Serial xbee(p28,p27); DigitalOut resetxbee(p26); Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging //MSCFileSystem fs("fs"); Timer t; Ticker tick1; int cpt=0; char affichage[3]={0}; char maze[100]={0}; 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 v; 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"); cpt++; //sprintf(affichage,"%d",cpt); m3pi.locate(0,1); m3pi.printf(maze); //m3pi.print("Case 7", 7); m3pi.stop(); result = 2; break; case 0x1C: case 0x18: case 0x10: // Forward/Left or Left Only // m3pi.stop(); step(); current_state(); if ((sensors == 0x00) || (sensors == 0x10)) { // Turn Left m3pi.cls(); strcat(maze,"L"); //sprintf(affichage,"%d",cpt); m3pi.locate(0,1); m3pi.printf(maze); //m3pi.print("Left", 5); result = 3; } else { // Forward m3pi.cls(); strcat(maze,"F"); m3pi.locate(0,1); m3pi.printf(maze); result = 1; } break; case 0x07: case 0x03: case 0x01: // Forward/Right or Right Only -> RF // m3pi.stop(); m3pi.cls(); strcat(maze,"R"); //sprintf(affichage,"%d",cpt); m3pi.locate(0,1); m3pi.printf(maze); //m3pi.print("Right", 6); step(); result = 4; break; case 0x1F: // 'T' or Intersection or End -> LR or RFL // m3pi.stop(); step(); current_state(); // m3pi.print("End", 4); if (sensors == 0x1F) { // End m3pi.cls(); strcat(maze,"E"); cpt++; m3pi.locate(0,1); m3pi.printf(maze); result = 6; } else { // 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 // m3pi.stop(); step2(); current_state(); // m3pi.print("End", 4); if (sensors == 0x1F) { // End step2(); step2(); m3pi.cls(); strcat(maze,"E"); cpt++; m3pi.locate(0,1); m3pi.printf(maze); result = 0; } else if ((sensors == 0x10) || (sensors == 0x18)) { // Turn Left step2(); step2(); m3pi.cls(); strcat(maze,"L"); m3pi.locate(0,1); m3pi.printf(maze); result = 3; } else if ((sensors == 0x14) || (sensors == 0x16) || (sensors == 0x06) || (sensors == 0x1C)) { // Forward step2(); step2(); m3pi.cls(); strcat(maze,"F"); m3pi.locate(0,1); m3pi.printf(maze); result = 1; } else { // Turn Right step2(); step2(); m3pi.cls(); strcat(maze, "R"); m3pi.locate(0,1); m3pi.printf(maze); 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 //m3pi.cls(); //m3pi.print("Back", 5); 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); //wait(0.12); m3pi.stop(); result = 1; } break; case 3: // Turn Left //m3pi.cls(); //m3pi.print("Left", 5); if(sensors != 0x10) { m3pi.left(speed); result = 3; } else { do{ current_state(); m3pi.left(0.4*speed); }while(sensors!=0x04); //wait(0.1); m3pi.stop(); result = 1; } break; case 4: // Turn Right //m3pi.cls(); //m3pi.print("Right", 6); 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); //wait(0.1); m3pi.stop(); result = 1; } break; } return result; } } void optimiser(char s[]) { char buffer[3]; for(int i = 0; i < 1; i++) { } } /* 1 -> PID * 2 -> Turn back * 3 -> Turn left * 4 -> Turn right */ char labyrinth(char result) { if(result==6) { char s[100]; int j=0; strcpy(s,maze); optimiser(s); current_state(); if((sensors == 0x00) ||(sensors == 0x1C) ||(sensors == 0x18) ||(sensors == 0x10) ||(sensors == 0x07) ||(sensors == 0x03) ||(sensors == 0x1F) ||(sensors == 0x0F) ||(sensors == 0x1E) ) { switch (s[j]) { case 'R': result=4; break; case 'B': result=2; break; case 'L': result=3; break; case 'F': result=1; break; default: result=0; break; } j++; } } return result; } 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; } int 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 *optimisation(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; } int main() { #if 1 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 6: command = labyrinth(command); break; } } } #else char str[] = "xxforxx"; char c[] = "xx"; char d[] = "y"; char *result = NULL; // oldW string m3pi.cls(); m3pi.printf("Old: %sn", str); result = replaceWord(str, c, d); m3pi.cls(); m3pi.printf("%s", result); free(result); return 0; #endif }