belkaid sylia / Mbed 2 deprecated revision

MaszeSolver

Dependencies:   mbed FileSystem_POPS m3pi

main3.cpp@7:ca53c1bde37c, 2018-03-06 (annotated)

Committer:
diogohideki
Date:
Tue Mar 06 08:35:43 2018 +0000
Revision:
7:ca53c1bde37c
Parent:
6:9690e497319b
Child:
8:de6ca481b136
before 6 mars

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 7:ca53c1bde37c 23 #define seuil(x) (x>400 ? 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 7:ca53c1bde37c 59 m3pi.forward(0.12);
diogohideki 7:ca53c1bde37c 60 wait(0.18);
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 7:ca53c1bde37c 80 m3pi.print("Back", 5);
diogohideki 4:cdd8e61cd8c7 81 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 82 result = 2;
diogohideki 4:cdd8e61cd8c7 83 break;
diogohideki 7:ca53c1bde37c 84 case 0x18: case 0x1C: case 0x1E:
diogohideki 4:cdd8e61cd8c7 85 // Forward/Left or Left Only
diogohideki 4:cdd8e61cd8c7 86 step();
diogohideki 4:cdd8e61cd8c7 87 current_state();
diogohideki 7:ca53c1bde37c 88 if ((sensors == 0x04) || (sensors == 0x06) || (sensors == 0x0C) || (sensors == 0x14) || (sensors == 0x16) || (sensors == 0x07)) {
diogohideki 7:ca53c1bde37c 89 // Forward
diogohideki 5:bc86a4fb4784 90 m3pi.cls();
diogohideki 7:ca53c1bde37c 91 m3pi.print("Forw2", 5);
diogohideki 7:ca53c1bde37c 92 result = 1;
diogohideki 6:9690e497319b 93 } else if (sensors == 0x1F) {
diogohideki 6:9690e497319b 94 // End
diogohideki 6:9690e497319b 95 m3pi.cls();
diogohideki 6:9690e497319b 96 m3pi.print("The End2", 8);
diogohideki 6:9690e497319b 97 result = 0;
diogohideki 7:ca53c1bde37c 98 } else if((sensors == 0x00) || (sensors == 0x10) || (sensors == 0x18)) {
diogohideki 7:ca53c1bde37c 99 // Turn Left
diogohideki 6:9690e497319b 100 m3pi.cls();
diogohideki 7:ca53c1bde37c 101 m3pi.print("Left", 5);
diogohideki 7:ca53c1bde37c 102 result = 3;
diogohideki 4:cdd8e61cd8c7 103 } else {
diogohideki 7:ca53c1bde37c 104 // Turn Right
diogohideki 5:bc86a4fb4784 105 m3pi.cls();
diogohideki 7:ca53c1bde37c 106 m3pi.print("Right2", 6);
diogohideki 7:ca53c1bde37c 107 result = 4;
diogohideki 4:cdd8e61cd8c7 108 }
diogohideki 4:cdd8e61cd8c7 109 break;
diogohideki 6:9690e497319b 110 case 0x07: case 0x03:
diogohideki 4:cdd8e61cd8c7 111 // Forward/Right or Right Only -> RF
diogohideki 4:cdd8e61cd8c7 112 // m3pi.stop();
diogohideki 5:bc86a4fb4784 113 m3pi.cls();
diogohideki 5:bc86a4fb4784 114 m3pi.print("Right", 6);
diogohideki 4:cdd8e61cd8c7 115 step();
diogohideki 4:cdd8e61cd8c7 116 result = 4;
diogohideki 4:cdd8e61cd8c7 117 break;
diogohideki 6:9690e497319b 118 case 0x1F: case 0x0F:
diogohideki 4:cdd8e61cd8c7 119 // 'T' or Intersection or End -> LR or RFL
diogohideki 4:cdd8e61cd8c7 120 // m3pi.stop();
diogohideki 4:cdd8e61cd8c7 121 step();
diogohideki 4:cdd8e61cd8c7 122 current_state();
diogohideki 4:cdd8e61cd8c7 123 if (sensors == 0x1F) {
diogohideki 4:cdd8e61cd8c7 124 // End
diogohideki 5:bc86a4fb4784 125 m3pi.cls();
diogohideki 5:bc86a4fb4784 126 m3pi.print("The End", 8);
diogohideki 4:cdd8e61cd8c7 127 result = 0;
diogohideki 4:cdd8e61cd8c7 128 } else {
diogohideki 4:cdd8e61cd8c7 129 // Turn Right
diogohideki 5:bc86a4fb4784 130 m3pi.cls();
diogohideki 5:bc86a4fb4784 131 m3pi.print("Inter", 6);
diogohideki 4:cdd8e61cd8c7 132 result = 4;
diogohideki 4:cdd8e61cd8c7 133 }
diogohideki 4:cdd8e61cd8c7 134 break;
diogohideki 4:cdd8e61cd8c7 135 case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08:
diogohideki 4:cdd8e61cd8c7 136 //PID
diogohideki 4:cdd8e61cd8c7 137 // Get the position of the line
diogohideki 4:cdd8e61cd8c7 138 current_pos_of_line = m3pi.line_position();
diogohideki 4:cdd8e61cd8c7 139 proportional = current_pos_of_line;
diogohideki 4:cdd8e61cd8c7 140 // Compute the derivate
diogohideki 4:cdd8e61cd8c7 141 derivate = current_pos_of_line - previous_pos_of_line;
diogohideki 4:cdd8e61cd8c7 142 // Compute the integral
diogohideki 4:cdd8e61cd8c7 143 integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
diogohideki 4:cdd8e61cd8c7 144 // Remember the last postion
diogohideki 4:cdd8e61cd8c7 145 previous_pos_of_line = current_pos_of_line;
diogohideki 4:cdd8e61cd8c7 146 // Compute the power
diogohideki 4:cdd8e61cd8c7 147 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM));
diogohideki 4:cdd8e61cd8c7 148 // Compute new speeds
diogohideki 4:cdd8e61cd8c7 149 right = speed-(power*MAX);
diogohideki 4:cdd8e61cd8c7 150 left = speed+(power*MAX);
diogohideki 4:cdd8e61cd8c7 151 // Limits checks on motor control
diogohideki 4:cdd8e61cd8c7 152 right = (right>MAX ? MAX :(right<MIN ? MIN : right));
diogohideki 4:cdd8e61cd8c7 153 left = (left>MAX ? MAX :(left<MIN ? MIN : left));
diogohideki 4:cdd8e61cd8c7 154 // Send command to motors
diogohideki 4:cdd8e61cd8c7 155 m3pi.left_motor(left);
diogohideki 4:cdd8e61cd8c7 156 m3pi.right_motor(right);
diogohideki 4:cdd8e61cd8c7 157 result = 1;
diogohideki 4:cdd8e61cd8c7 158 break;
diogohideki 4:cdd8e61cd8c7 159 default:
diogohideki 4:cdd8e61cd8c7 160 // Faire rien
diogohideki 4:cdd8e61cd8c7 161 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 162 result = 0;
diogohideki 4:cdd8e61cd8c7 163 break;
diogohideki 4:cdd8e61cd8c7 164 }
diogohideki 4:cdd8e61cd8c7 165 return result;
diogohideki 4:cdd8e61cd8c7 166 }
diogohideki 4:cdd8e61cd8c7 167 }
diogohideki 4:cdd8e61cd8c7 168
diogohideki 4:cdd8e61cd8c7 169
diogohideki 4:cdd8e61cd8c7 170 /*
diogohideki 4:cdd8e61cd8c7 171 * Results
diogohideki 4:cdd8e61cd8c7 172 * 1 -> PID
diogohideki 4:cdd8e61cd8c7 173 * 2 -> Turn back
diogohideki 4:cdd8e61cd8c7 174 * 3 -> Turn left
diogohideki 4:cdd8e61cd8c7 175 * 4 -> Turn right
diogohideki 4:cdd8e61cd8c7 176 */
diogohideki 4:cdd8e61cd8c7 177 char turn(char command) {
diogohideki 4:cdd8e61cd8c7 178 if(command > 1 && command < 5) {
diogohideki 4:cdd8e61cd8c7 179 char result;
diogohideki 4:cdd8e61cd8c7 180 current_state();
diogohideki 4:cdd8e61cd8c7 181 switch(command) {
diogohideki 4:cdd8e61cd8c7 182 case 2:
diogohideki 4:cdd8e61cd8c7 183 // Turn Back
diogohideki 4:cdd8e61cd8c7 184 if(sensors != 0x01) {
diogohideki 4:cdd8e61cd8c7 185 m3pi.right(speed);
diogohideki 4:cdd8e61cd8c7 186 result = 2;
diogohideki 4:cdd8e61cd8c7 187 } else {
diogohideki 7:ca53c1bde37c 188 m3pi.right(0.12);
diogohideki 4:cdd8e61cd8c7 189 wait(0.12);
diogohideki 4:cdd8e61cd8c7 190 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 191 result = 1;
diogohideki 4:cdd8e61cd8c7 192 }
diogohideki 4:cdd8e61cd8c7 193 break;
diogohideki 4:cdd8e61cd8c7 194 case 3:
diogohideki 4:cdd8e61cd8c7 195 // Turn Left
diogohideki 4:cdd8e61cd8c7 196 if(sensors != 0x10) {
diogohideki 4:cdd8e61cd8c7 197 m3pi.left(speed);
diogohideki 4:cdd8e61cd8c7 198 result = 3;
diogohideki 4:cdd8e61cd8c7 199 } else {
diogohideki 7:ca53c1bde37c 200 m3pi.left(0.12);
diogohideki 4:cdd8e61cd8c7 201 wait(0.1);
diogohideki 4:cdd8e61cd8c7 202 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 203 result = 1;
diogohideki 4:cdd8e61cd8c7 204 }
diogohideki 4:cdd8e61cd8c7 205 break;
diogohideki 4:cdd8e61cd8c7 206 case 4:
diogohideki 4:cdd8e61cd8c7 207 // Turn Right
diogohideki 4:cdd8e61cd8c7 208 if(sensors != 0x01) {
diogohideki 4:cdd8e61cd8c7 209 m3pi.right(speed);
diogohideki 4:cdd8e61cd8c7 210 result = 4;
diogohideki 4:cdd8e61cd8c7 211 } else {
diogohideki 7:ca53c1bde37c 212 m3pi.right(0.12);
diogohideki 4:cdd8e61cd8c7 213 wait(0.1);
diogohideki 4:cdd8e61cd8c7 214 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 215 result = 1;
diogohideki 4:cdd8e61cd8c7 216 }
diogohideki 4:cdd8e61cd8c7 217 break;
diogohideki 4:cdd8e61cd8c7 218 }
diogohideki 4:cdd8e61cd8c7 219 return result;
diogohideki 4:cdd8e61cd8c7 220 }
diogohideki 4:cdd8e61cd8c7 221 }
diogohideki 4:cdd8e61cd8c7 222
diogohideki 4:cdd8e61cd8c7 223
bouaziz 0:398afdd73d9e 224 int main() {
diogohideki 4:cdd8e61cd8c7 225 resetxbee=0;
bouaziz 0:398afdd73d9e 226 wait(0.01);
bouaziz 0:398afdd73d9e 227 resetxbee =1;
bouaziz 0:398afdd73d9e 228
diogohideki 4:cdd8e61cd8c7 229 // FILE *p= fopen("/fs/tt.txt","a+");
bouaziz 0:398afdd73d9e 230 m3pi.sensor_auto_calibrate();
bouaziz 0:398afdd73d9e 231 wait(1.);
diogohideki 4:cdd8e61cd8c7 232 tick1.attach(&inter1,0.01);
bouaziz 0:398afdd73d9e 233
diogohideki 4:cdd8e61cd8c7 234 // fprintf(p,"ecrire dans la cle USB\r\n");
diogohideki 4:cdd8e61cd8c7 235 // fclose(p);
diogohideki 4:cdd8e61cd8c7 236
bouaziz 0:398afdd73d9e 237 while(1) {
diogohideki 4:cdd8e61cd8c7 238 if(flag10ms==1) {
diogohideki 4:cdd8e61cd8c7 239 switch(command) {
diogohideki 4:cdd8e61cd8c7 240 case 0:
diogohideki 4:cdd8e61cd8c7 241 // Faire Rien
diogohideki 4:cdd8e61cd8c7 242 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 243 break;
diogohideki 4:cdd8e61cd8c7 244 case 1:
diogohideki 4:cdd8e61cd8c7 245 // PID
diogohideki 4:cdd8e61cd8c7 246 command = PIDf(command);
diogohideki 4:cdd8e61cd8c7 247 break;
diogohideki 4:cdd8e61cd8c7 248 case 2: case 3: case 4:
diogohideki 4:cdd8e61cd8c7 249 // 2 -> Back
diogohideki 4:cdd8e61cd8c7 250 // 3 -> Left
diogohideki 4:cdd8e61cd8c7 251 // 4 -> Right
diogohideki 4:cdd8e61cd8c7 252 command = turn(command);
diogohideki 4:cdd8e61cd8c7 253 break;
diogohideki 4:cdd8e61cd8c7 254 }
diogohideki 4:cdd8e61cd8c7 255 }
bouaziz 0:398afdd73d9e 256 }
diogohideki 4:cdd8e61cd8c7 257 }