belkaid sylia / Mbed 2 deprecated revision

MaszeSolver

Dependencies:   mbed FileSystem_POPS m3pi

main3.cpp@4:cdd8e61cd8c7, 2018-02-22 (annotated)

Committer:
diogohideki
Date:
Thu Feb 22 08:21:12 2018 +0000
Revision:
4:cdd8e61cd8c7
Parent:
0:398afdd73d9e
Child:
5:bc86a4fb4784
initial state - last class

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