Diogo Kawamoto / Mbed 2 deprecated m3PI_TP_SETI

Final version of Polulu M2 SETI

Dependencies:   FileSystem_POPS m3PI_TP_POPS_II2015v0 m3pi mbed

Fork of m3PI_TP_POPS_II2015v0 by Samir Bouaziz

main3.cpp@6:9690e497319b, 2018-02-22 (annotated)

Committer:
diogohideki
Date:
Thu Feb 22 09:36:38 2018 +0000
Revision:
6:9690e497319b
Parent:
5:bc86a4fb4784
Child:
7:ca53c1bde37c
before change left case

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouaziz 0:398afdd73d9e 1 #include "mbed.h"
bouaziz 0:398afdd73d9e 2 #include "m3pi.h"
bouaziz 0:398afdd73d9e 3 #include "MSCFileSystem.h"
bouaziz 0:398afdd73d9e 4
bouaziz 0:398afdd73d9e 5 m3pi m3pi; // Initialise the m3pi
bouaziz 0:398afdd73d9e 6
bouaziz 0:398afdd73d9e 7 Serial xbee(p28,p27);
bouaziz 0:398afdd73d9e 8 DigitalOut resetxbee(p26);
bouaziz 0:398afdd73d9e 9 Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
diogohideki 4:cdd8e61cd8c7 10 //MSCFileSystem fs("fs");
diogohideki 4:cdd8e61cd8c7 11 Timer t;
diogohideki 4:cdd8e61cd8c7 12 Ticker tick1;
bouaziz 0:398afdd73d9e 13
bouaziz 0:398afdd73d9e 14 BusOut myleds(LED1, LED2, LED3, LED4);
bouaziz 0:398afdd73d9e 15
diogohideki 4:cdd8e61cd8c7 16 #define D_TERM 0.0
diogohideki 4:cdd8e61cd8c7 17 #define I_TERM 0.1
diogohideki 4:cdd8e61cd8c7 18 #define I_TERMO 0.1
diogohideki 4:cdd8e61cd8c7 19 #define P_TERM 0.9
diogohideki 4:cdd8e61cd8c7 20 #define MAX 0.3
diogohideki 4:cdd8e61cd8c7 21 #define MIN -0.2
diogohideki 4:cdd8e61cd8c7 22
diogohideki 6:9690e497319b 23 #define seuil(x) (x>300 ? 1 : 0)
diogohideki 4:cdd8e61cd8c7 24
diogohideki 4:cdd8e61cd8c7 25 float current_pos_of_line,
diogohideki 4:cdd8e61cd8c7 26 previous_pos_of_line,
diogohideki 4:cdd8e61cd8c7 27 derivate,
diogohideki 4:cdd8e61cd8c7 28 proportional,
diogohideki 4:cdd8e61cd8c7 29 power,
diogohideki 4:cdd8e61cd8c7 30 integral,
diogohideki 4:cdd8e61cd8c7 31 left,
diogohideki 4:cdd8e61cd8c7 32 right,
diogohideki 4:cdd8e61cd8c7 33 speed=0.3;
diogohideki 4:cdd8e61cd8c7 34
diogohideki 4:cdd8e61cd8c7 35 char chain[10];
diogohideki 4:cdd8e61cd8c7 36 int v;
diogohideki 4:cdd8e61cd8c7 37
diogohideki 4:cdd8e61cd8c7 38 unsigned short tabsensor[5];
diogohideki 4:cdd8e61cd8c7 39 volatile unsigned char sensors;
diogohideki 4:cdd8e61cd8c7 40
diogohideki 4:cdd8e61cd8c7 41 volatile char flag10ms;
diogohideki 4:cdd8e61cd8c7 42
diogohideki 4:cdd8e61cd8c7 43 char command=1;
diogohideki 4:cdd8e61cd8c7 44
diogohideki 4:cdd8e61cd8c7 45 void inter1() {
diogohideki 4:cdd8e61cd8c7 46 flag10ms=1;
diogohideki 4:cdd8e61cd8c7 47 }
diogohideki 4:cdd8e61cd8c7 48
diogohideki 4:cdd8e61cd8c7 49 void current_state() {
diogohideki 4:cdd8e61cd8c7 50 unsigned char i;
diogohideki 4:cdd8e61cd8c7 51 sensors=0;
diogohideki 5:bc86a4fb4784 52 m3pi.calibrated_sensors(tabsensor);
diogohideki 4:cdd8e61cd8c7 53 for(i=0; i<5; i++) {
diogohideki 4:cdd8e61cd8c7 54 sensors = (sensors << 1) | seuil(tabsensor[i]);
diogohideki 4:cdd8e61cd8c7 55 }
diogohideki 4:cdd8e61cd8c7 56 }
diogohideki 4:cdd8e61cd8c7 57
diogohideki 4:cdd8e61cd8c7 58 void step() {
diogohideki 4:cdd8e61cd8c7 59 m3pi.forward(0.4*speed);
diogohideki 5:bc86a4fb4784 60 wait(0.22);
diogohideki 4:cdd8e61cd8c7 61 // m3pi.stop();
diogohideki 4:cdd8e61cd8c7 62 }
diogohideki 4:cdd8e61cd8c7 63
diogohideki 4:cdd8e61cd8c7 64 /*
diogohideki 4:cdd8e61cd8c7 65 * Results
diogohideki 4:cdd8e61cd8c7 66 * 1 -> PID
diogohideki 4:cdd8e61cd8c7 67 * 2 -> Turn back
diogohideki 4:cdd8e61cd8c7 68 * 3 -> Turn left
diogohideki 4:cdd8e61cd8c7 69 * 4 -> Turn right
diogohideki 4:cdd8e61cd8c7 70 */
diogohideki 4:cdd8e61cd8c7 71 char PIDf(char commande) {
diogohideki 4:cdd8e61cd8c7 72 if(commande==1) {
diogohideki 4:cdd8e61cd8c7 73 char result;
diogohideki 4:cdd8e61cd8c7 74 current_state();
diogohideki 4:cdd8e61cd8c7 75 switch(sensors) {
diogohideki 4:cdd8e61cd8c7 76 case 0x00:
diogohideki 4:cdd8e61cd8c7 77 // Deadend
diogohideki 5:bc86a4fb4784 78 // Back
diogohideki 5:bc86a4fb4784 79 m3pi.cls();
diogohideki 5:bc86a4fb4784 80 m3pi.print("Case 0", 7);
diogohideki 4:cdd8e61cd8c7 81 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 82 result = 2;
diogohideki 4:cdd8e61cd8c7 83 break;
diogohideki 4:cdd8e61cd8c7 84 case 0x1C: case 0x18: case 0x1E:
diogohideki 4:cdd8e61cd8c7 85 // Forward/Left or Left Only
diogohideki 4:cdd8e61cd8c7 86 // m3pi.stop();
diogohideki 4:cdd8e61cd8c7 87 step();
diogohideki 4:cdd8e61cd8c7 88 current_state();
diogohideki 6:9690e497319b 89 if ((sensors == 0x10) || (sensors == 0x18)) {
diogohideki 4:cdd8e61cd8c7 90 // Turn Left
diogohideki 5:bc86a4fb4784 91 m3pi.cls();
diogohideki 5:bc86a4fb4784 92 m3pi.print("Left", 5);
diogohideki 4:cdd8e61cd8c7 93 result = 3;
diogohideki 6:9690e497319b 94 } else if (sensors == 0x1F) {
diogohideki 6:9690e497319b 95 // End
diogohideki 6:9690e497319b 96 m3pi.cls();
diogohideki 6:9690e497319b 97 m3pi.print("The End2", 8);
diogohideki 6:9690e497319b 98 result = 0;
diogohideki 6:9690e497319b 99 } else if((sensors == 0x01) || (sensors == 0x03)) {
diogohideki 6:9690e497319b 100 // Turn right
diogohideki 6:9690e497319b 101 m3pi.cls();
diogohideki 6:9690e497319b 102 m3pi.print("Right2", 7);
diogohideki 6:9690e497319b 103 result = 1;
diogohideki 4:cdd8e61cd8c7 104 } else {
diogohideki 4:cdd8e61cd8c7 105 // Forward
diogohideki 5:bc86a4fb4784 106 m3pi.cls();
diogohideki 5:bc86a4fb4784 107 m3pi.print("Fowar", 5);
diogohideki 4:cdd8e61cd8c7 108 result = 1;
diogohideki 4:cdd8e61cd8c7 109 }
diogohideki 4:cdd8e61cd8c7 110 break;
diogohideki 6:9690e497319b 111 case 0x07: case 0x03:
diogohideki 4:cdd8e61cd8c7 112 // Forward/Right or Right Only -> RF
diogohideki 4:cdd8e61cd8c7 113 // m3pi.stop();
diogohideki 5:bc86a4fb4784 114 m3pi.cls();
diogohideki 5:bc86a4fb4784 115 m3pi.print("Right", 6);
diogohideki 4:cdd8e61cd8c7 116 step();
diogohideki 4:cdd8e61cd8c7 117 result = 4;
diogohideki 4:cdd8e61cd8c7 118 break;
diogohideki 6:9690e497319b 119 case 0x1F: case 0x0F:
diogohideki 4:cdd8e61cd8c7 120 // 'T' or Intersection or End -> LR or RFL
diogohideki 4:cdd8e61cd8c7 121 // m3pi.stop();
diogohideki 4:cdd8e61cd8c7 122 step();
diogohideki 4:cdd8e61cd8c7 123 current_state();
diogohideki 5:bc86a4fb4784 124 m3pi.print("End", 4);
diogohideki 4:cdd8e61cd8c7 125 if (sensors == 0x1F) {
diogohideki 4:cdd8e61cd8c7 126 // End
diogohideki 5:bc86a4fb4784 127 m3pi.cls();
diogohideki 5:bc86a4fb4784 128 m3pi.print("The End", 8);
diogohideki 4:cdd8e61cd8c7 129 result = 0;
diogohideki 4:cdd8e61cd8c7 130 } else {
diogohideki 4:cdd8e61cd8c7 131 // Turn Right
diogohideki 5:bc86a4fb4784 132 m3pi.cls();
diogohideki 5:bc86a4fb4784 133 m3pi.print("Inter", 6);
diogohideki 4:cdd8e61cd8c7 134 result = 4;
diogohideki 4:cdd8e61cd8c7 135 }
diogohideki 4:cdd8e61cd8c7 136 break;
diogohideki 4:cdd8e61cd8c7 137 case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08:
diogohideki 4:cdd8e61cd8c7 138 //PID
diogohideki 4:cdd8e61cd8c7 139 // Get the position of the line
diogohideki 4:cdd8e61cd8c7 140 current_pos_of_line = m3pi.line_position();
diogohideki 4:cdd8e61cd8c7 141 proportional = current_pos_of_line;
diogohideki 4:cdd8e61cd8c7 142 // Compute the derivate
diogohideki 4:cdd8e61cd8c7 143 derivate = current_pos_of_line - previous_pos_of_line;
diogohideki 4:cdd8e61cd8c7 144 // Compute the integral
diogohideki 4:cdd8e61cd8c7 145 integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
diogohideki 4:cdd8e61cd8c7 146 // Remember the last postion
diogohideki 4:cdd8e61cd8c7 147 previous_pos_of_line = current_pos_of_line;
diogohideki 4:cdd8e61cd8c7 148 // Compute the power
diogohideki 4:cdd8e61cd8c7 149 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM));
diogohideki 4:cdd8e61cd8c7 150 // Compute new speeds
diogohideki 4:cdd8e61cd8c7 151 right = speed-(power*MAX);
diogohideki 4:cdd8e61cd8c7 152 left = speed+(power*MAX);
diogohideki 4:cdd8e61cd8c7 153 // Limits checks on motor control
diogohideki 4:cdd8e61cd8c7 154 right = (right>MAX ? MAX :(right<MIN ? MIN : right));
diogohideki 4:cdd8e61cd8c7 155 left = (left>MAX ? MAX :(left<MIN ? MIN : left));
diogohideki 4:cdd8e61cd8c7 156 // Send command to motors
diogohideki 4:cdd8e61cd8c7 157 m3pi.left_motor(left);
diogohideki 4:cdd8e61cd8c7 158 m3pi.right_motor(right);
diogohideki 4:cdd8e61cd8c7 159 result = 1;
diogohideki 4:cdd8e61cd8c7 160 break;
diogohideki 4:cdd8e61cd8c7 161 default:
diogohideki 4:cdd8e61cd8c7 162 // Faire rien
diogohideki 4:cdd8e61cd8c7 163 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 164 result = 0;
diogohideki 4:cdd8e61cd8c7 165 break;
diogohideki 4:cdd8e61cd8c7 166 }
diogohideki 4:cdd8e61cd8c7 167 return result;
diogohideki 4:cdd8e61cd8c7 168 }
diogohideki 4:cdd8e61cd8c7 169 }
diogohideki 4:cdd8e61cd8c7 170
diogohideki 4:cdd8e61cd8c7 171
diogohideki 4:cdd8e61cd8c7 172 /*
diogohideki 4:cdd8e61cd8c7 173 * Results
diogohideki 4:cdd8e61cd8c7 174 * 1 -> PID
diogohideki 4:cdd8e61cd8c7 175 * 2 -> Turn back
diogohideki 4:cdd8e61cd8c7 176 * 3 -> Turn left
diogohideki 4:cdd8e61cd8c7 177 * 4 -> Turn right
diogohideki 4:cdd8e61cd8c7 178 */
diogohideki 4:cdd8e61cd8c7 179 char turn(char command) {
diogohideki 4:cdd8e61cd8c7 180 if(command > 1 && command < 5) {
diogohideki 4:cdd8e61cd8c7 181 char result;
diogohideki 4:cdd8e61cd8c7 182 current_state();
diogohideki 4:cdd8e61cd8c7 183 switch(command) {
diogohideki 4:cdd8e61cd8c7 184 case 2:
diogohideki 4:cdd8e61cd8c7 185 // Turn Back
diogohideki 5:bc86a4fb4784 186 //m3pi.cls();
diogohideki 5:bc86a4fb4784 187 //m3pi.print("Back", 5);
diogohideki 4:cdd8e61cd8c7 188 if(sensors != 0x01) {
diogohideki 4:cdd8e61cd8c7 189 m3pi.right(speed);
diogohideki 4:cdd8e61cd8c7 190 result = 2;
diogohideki 4:cdd8e61cd8c7 191 } else {
diogohideki 4:cdd8e61cd8c7 192 m3pi.right(0.4*speed);
diogohideki 4:cdd8e61cd8c7 193 wait(0.12);
diogohideki 4:cdd8e61cd8c7 194 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 195 result = 1;
diogohideki 4:cdd8e61cd8c7 196 }
diogohideki 4:cdd8e61cd8c7 197 break;
diogohideki 4:cdd8e61cd8c7 198 case 3:
diogohideki 4:cdd8e61cd8c7 199 // Turn Left
diogohideki 5:bc86a4fb4784 200 //m3pi.cls();
diogohideki 5:bc86a4fb4784 201 //m3pi.print("Left", 5);
diogohideki 4:cdd8e61cd8c7 202 if(sensors != 0x10) {
diogohideki 4:cdd8e61cd8c7 203 m3pi.left(speed);
diogohideki 4:cdd8e61cd8c7 204 result = 3;
diogohideki 4:cdd8e61cd8c7 205 } else {
diogohideki 4:cdd8e61cd8c7 206 m3pi.left(0.4*speed);
diogohideki 4:cdd8e61cd8c7 207 wait(0.1);
diogohideki 4:cdd8e61cd8c7 208 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 209 result = 1;
diogohideki 4:cdd8e61cd8c7 210 }
diogohideki 4:cdd8e61cd8c7 211 break;
diogohideki 4:cdd8e61cd8c7 212 case 4:
diogohideki 4:cdd8e61cd8c7 213 // Turn Right
diogohideki 5:bc86a4fb4784 214 //m3pi.cls();
diogohideki 5:bc86a4fb4784 215 //m3pi.print("Right", 6);
diogohideki 4:cdd8e61cd8c7 216 if(sensors != 0x01) {
diogohideki 4:cdd8e61cd8c7 217 m3pi.right(speed);
diogohideki 4:cdd8e61cd8c7 218 result = 4;
diogohideki 4:cdd8e61cd8c7 219 } else {
diogohideki 4:cdd8e61cd8c7 220 m3pi.right(0.4*speed);
diogohideki 4:cdd8e61cd8c7 221 wait(0.1);
diogohideki 4:cdd8e61cd8c7 222 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 223 result = 1;
diogohideki 4:cdd8e61cd8c7 224 }
diogohideki 4:cdd8e61cd8c7 225 break;
diogohideki 4:cdd8e61cd8c7 226 }
diogohideki 4:cdd8e61cd8c7 227 return result;
diogohideki 4:cdd8e61cd8c7 228 }
diogohideki 4:cdd8e61cd8c7 229 }
diogohideki 4:cdd8e61cd8c7 230
diogohideki 4:cdd8e61cd8c7 231
bouaziz 0:398afdd73d9e 232 int main() {
diogohideki 4:cdd8e61cd8c7 233 resetxbee=0;
bouaziz 0:398afdd73d9e 234 wait(0.01);
bouaziz 0:398afdd73d9e 235 resetxbee =1;
bouaziz 0:398afdd73d9e 236
diogohideki 4:cdd8e61cd8c7 237 // FILE *p= fopen("/fs/tt.txt","a+");
bouaziz 0:398afdd73d9e 238 m3pi.sensor_auto_calibrate();
bouaziz 0:398afdd73d9e 239 wait(1.);
diogohideki 4:cdd8e61cd8c7 240 tick1.attach(&inter1,0.01);
bouaziz 0:398afdd73d9e 241
diogohideki 4:cdd8e61cd8c7 242 // fprintf(p,"ecrire dans la cle USB\r\n");
diogohideki 4:cdd8e61cd8c7 243 // fclose(p);
diogohideki 4:cdd8e61cd8c7 244
bouaziz 0:398afdd73d9e 245 while(1) {
diogohideki 4:cdd8e61cd8c7 246 if(flag10ms==1) {
diogohideki 4:cdd8e61cd8c7 247 switch(command) {
diogohideki 4:cdd8e61cd8c7 248 case 0:
diogohideki 4:cdd8e61cd8c7 249 // Faire Rien
diogohideki 4:cdd8e61cd8c7 250 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 251 break;
diogohideki 4:cdd8e61cd8c7 252 case 1:
diogohideki 4:cdd8e61cd8c7 253 // PID
diogohideki 4:cdd8e61cd8c7 254 command = PIDf(command);
diogohideki 4:cdd8e61cd8c7 255 break;
diogohideki 4:cdd8e61cd8c7 256 case 2: case 3: case 4:
diogohideki 4:cdd8e61cd8c7 257 // 2 -> Back
diogohideki 4:cdd8e61cd8c7 258 // 3 -> Left
diogohideki 4:cdd8e61cd8c7 259 // 4 -> Right
diogohideki 4:cdd8e61cd8c7 260 command = turn(command);
diogohideki 4:cdd8e61cd8c7 261 break;
diogohideki 4:cdd8e61cd8c7 262 }
diogohideki 4:cdd8e61cd8c7 263 }
bouaziz 0:398afdd73d9e 264 }
diogohideki 4:cdd8e61cd8c7 265 }