MEGACARAZ / Mbed 2 deprecated PID

PIDs and fun

Dependencies:   btbee m3pi_ng mbed FatFileSystemCpp

main.cpp@2:1e947dcc40cb, 2015-05-27 (annotated)

Committer:
crmackey
Date:
Wed May 27 11:38:48 2015 +0000
Revision:
2:1e947dcc40cb
Parent:
1:936e329e840a
Child:
3:ebe015c72bba
Infrared sensor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
crmackey 0:4d1ae7eb0070 1 #include "mbed.h"
crmackey 0:4d1ae7eb0070 2 #include "m3pi_ng.h"
crmackey 0:4d1ae7eb0070 3 #include "time.h"
crmackey 0:4d1ae7eb0070 4 #include "btbee.h"
crmackey 0:4d1ae7eb0070 5
crmackey 0:4d1ae7eb0070 6 m3pi m3pi;
crmackey 0:4d1ae7eb0070 7 btbee btbee;
crmackey 0:4d1ae7eb0070 8 DigitalIn m3pi_pb(p21);
crmackey 1:936e329e840a 9 DigitalIn m3pi_IN[] = {(p12)};
crmackey 0:4d1ae7eb0070 10 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
crmackey 0:4d1ae7eb0070 11 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
crmackey 0:4d1ae7eb0070 12
crmackey 0:4d1ae7eb0070 13 // Minimum and maximum motor speeds
crmackey 0:4d1ae7eb0070 14 #define MAX 1.0
crmackey 0:4d1ae7eb0070 15 #define MIN 0
crmackey 0:4d1ae7eb0070 16
crmackey 0:4d1ae7eb0070 17 // PID terms
crmackey 0:4d1ae7eb0070 18 #define P_TERM 1.5
crmackey 0:4d1ae7eb0070 19 #define I_TERM 0
crmackey 0:4d1ae7eb0070 20 #define D_TERM 25
crmackey 0:4d1ae7eb0070 21
crmackey 0:4d1ae7eb0070 22 int main() {
crmackey 0:4d1ae7eb0070 23
crmackey 1:936e329e840a 24
crmackey 1:936e329e840a 25 while(1) {
crmackey 0:4d1ae7eb0070 26
crmackey 0:4d1ae7eb0070 27 btbee.reset();
crmackey 0:4d1ae7eb0070 28 m3pi_pb.mode(PullUp);
crmackey 0:4d1ae7eb0070 29
crmackey 0:4d1ae7eb0070 30 m3pi.printf("Wait 4");
crmackey 0:4d1ae7eb0070 31 m3pi.locate(0,1);
crmackey 0:4d1ae7eb0070 32 m3pi.printf("PC");
crmackey 0:4d1ae7eb0070 33
crmackey 0:4d1ae7eb0070 34 while(m3pi_pb) {
crmackey 0:4d1ae7eb0070 35 m3pi_led[0]=!m3pi_led[0];
crmackey 0:4d1ae7eb0070 36 wait(3);
crmackey 0:4d1ae7eb0070 37 btbee.printf("\n");
crmackey 0:4d1ae7eb0070 38 btbee.printf("PC connected. Press the button \n");
crmackey 0:4d1ae7eb0070 39 }
crmackey 0:4d1ae7eb0070 40
crmackey 1:936e329e840a 41 m3pi.cls();
crmackey 0:4d1ae7eb0070 42
crmackey 0:4d1ae7eb0070 43 Timer LapTimer;
crmackey 0:4d1ae7eb0070 44 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 0:4d1ae7eb0070 45 m3pi.print(Bat,7);
crmackey 0:4d1ae7eb0070 46 wait(0.75);
crmackey 0:4d1ae7eb0070 47 m3pi.cls();
crmackey 0:4d1ae7eb0070 48 float batteryvoltage = m3pi.battery();
crmackey 0:4d1ae7eb0070 49 char* str = new char[30];
crmackey 0:4d1ae7eb0070 50 sprintf(str, "%.4g", batteryvoltage);
crmackey 0:4d1ae7eb0070 51 m3pi.print(str,6);
crmackey 0:4d1ae7eb0070 52 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 0:4d1ae7eb0070 53 wait(0.75);
crmackey 0:4d1ae7eb0070 54 m3pi.cls();
crmackey 0:4d1ae7eb0070 55
crmackey 0:4d1ae7eb0070 56 if (batteryvoltage < 4.2)
crmackey 0:4d1ae7eb0070 57 {
crmackey 0:4d1ae7eb0070 58 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 0:4d1ae7eb0070 59 m3pi.print(low,7);
crmackey 0:4d1ae7eb0070 60 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 0:4d1ae7eb0070 61 m3pi.playtune(ExitSound,11);
crmackey 0:4d1ae7eb0070 62 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 0:4d1ae7eb0070 63 exit(1);
crmackey 0:4d1ae7eb0070 64 }
crmackey 0:4d1ae7eb0070 65
crmackey 0:4d1ae7eb0070 66 m3pi.locate(0,1);
crmackey 0:4d1ae7eb0070 67 m3pi.printf("Line PID");
crmackey 0:4d1ae7eb0070 68
crmackey 0:4d1ae7eb0070 69 wait(2.0);
crmackey 0:4d1ae7eb0070 70
crmackey 0:4d1ae7eb0070 71 btbee.printf("Now calibrating \n");
crmackey 0:4d1ae7eb0070 72
crmackey 0:4d1ae7eb0070 73 m3pi.sensor_auto_calibrate();
crmackey 0:4d1ae7eb0070 74
crmackey 0:4d1ae7eb0070 75 btbee.printf("Finished calibrating \n");
crmackey 0:4d1ae7eb0070 76
crmackey 0:4d1ae7eb0070 77
crmackey 0:4d1ae7eb0070 78 float right;
crmackey 0:4d1ae7eb0070 79 float left;
crmackey 0:4d1ae7eb0070 80 float current_pos_of_line = 0.0;
crmackey 0:4d1ae7eb0070 81 float previous_pos_of_line = 0.0;
crmackey 0:4d1ae7eb0070 82 float derivative,proportional,integral = 0;
crmackey 0:4d1ae7eb0070 83 float power;
crmackey 0:4d1ae7eb0070 84 float speed = MAX;
crmackey 0:4d1ae7eb0070 85 int LapTest[5];
crmackey 0:4d1ae7eb0070 86 int s1, s2, s3, s4, s5;
crmackey 0:4d1ae7eb0070 87 int counter = -1;
crmackey 0:4d1ae7eb0070 88 int checkvar = 0;
crmackey 0:4d1ae7eb0070 89 double Time1, Time2, Time3, Time4, Time5, TimeAve;
crmackey 0:4d1ae7eb0070 90 btbee.printf("\n");
crmackey 0:4d1ae7eb0070 91 btbee.printf("Now starting \n");
crmackey 0:4d1ae7eb0070 92
crmackey 0:4d1ae7eb0070 93
crmackey 1:936e329e840a 94
crmackey 0:4d1ae7eb0070 95
crmackey 1:936e329e840a 96 while (1) {
crmackey 1:936e329e840a 97
crmackey 2:1e947dcc40cb 98 if(m3pi_IN [0] == 0) {
crmackey 2:1e947dcc40cb 99 while (m3pi_IN [0] == 0) {
crmackey 2:1e947dcc40cb 100 btbee.printf("Obstacle detected. Please remove to continue");
crmackey 2:1e947dcc40cb 101 m3pi.stop();
crmackey 2:1e947dcc40cb 102 wait(3.0);
crmackey 2:1e947dcc40cb 103 }
crmackey 2:1e947dcc40cb 104 }
crmackey 2:1e947dcc40cb 105
crmackey 2:1e947dcc40cb 106 // Get the position of the line.
crmackey 0:4d1ae7eb0070 107 current_pos_of_line = m3pi.line_position();
crmackey 0:4d1ae7eb0070 108 proportional = current_pos_of_line;
crmackey 0:4d1ae7eb0070 109
crmackey 0:4d1ae7eb0070 110 // Compute the derivative
crmackey 0:4d1ae7eb0070 111 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 0:4d1ae7eb0070 112
crmackey 0:4d1ae7eb0070 113 // Compute the integral
crmackey 0:4d1ae7eb0070 114 integral += proportional;
crmackey 0:4d1ae7eb0070 115
crmackey 0:4d1ae7eb0070 116 // Remember the last position.
crmackey 0:4d1ae7eb0070 117 previous_pos_of_line = current_pos_of_line;
crmackey 0:4d1ae7eb0070 118
crmackey 0:4d1ae7eb0070 119 // Compute the power
crmackey 0:4d1ae7eb0070 120 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 0:4d1ae7eb0070 121
crmackey 0:4d1ae7eb0070 122 // Compute new speeds
crmackey 0:4d1ae7eb0070 123 right = speed+power;
crmackey 0:4d1ae7eb0070 124 left = speed-power;
crmackey 0:4d1ae7eb0070 125
crmackey 0:4d1ae7eb0070 126 // limit checks
crmackey 0:4d1ae7eb0070 127 if (right < MIN)
crmackey 0:4d1ae7eb0070 128 right = MIN;
crmackey 0:4d1ae7eb0070 129 else if (right > MAX)
crmackey 0:4d1ae7eb0070 130 right = MAX;
crmackey 0:4d1ae7eb0070 131
crmackey 0:4d1ae7eb0070 132 if (left < MIN)
crmackey 0:4d1ae7eb0070 133 left = MIN;
crmackey 0:4d1ae7eb0070 134 else if (left > MAX)
crmackey 0:4d1ae7eb0070 135 left = MAX;
crmackey 0:4d1ae7eb0070 136
crmackey 0:4d1ae7eb0070 137 // set speed
crmackey 0:4d1ae7eb0070 138 m3pi.left_motor(left);
crmackey 0:4d1ae7eb0070 139 m3pi.right_motor(right);
crmackey 0:4d1ae7eb0070 140
crmackey 0:4d1ae7eb0070 141 m3pi.calibrated_sensor(LapTest);
crmackey 0:4d1ae7eb0070 142
crmackey 0:4d1ae7eb0070 143 s1 = LapTest[0];
crmackey 0:4d1ae7eb0070 144 s2 = LapTest[1];
crmackey 0:4d1ae7eb0070 145 s3 = LapTest[2];
crmackey 0:4d1ae7eb0070 146 s4 = LapTest[3];
crmackey 0:4d1ae7eb0070 147 s5 = LapTest[4];
crmackey 0:4d1ae7eb0070 148
crmackey 0:4d1ae7eb0070 149 if (s1 > 500 and s5 > 500) {
crmackey 0:4d1ae7eb0070 150 checkvar = 1;
crmackey 0:4d1ae7eb0070 151 }
crmackey 0:4d1ae7eb0070 152
crmackey 0:4d1ae7eb0070 153 if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
crmackey 0:4d1ae7eb0070 154 LapTimer.start();
crmackey 0:4d1ae7eb0070 155 }
crmackey 0:4d1ae7eb0070 156
crmackey 0:4d1ae7eb0070 157 if (s1 < 500 and s5 < 500 and checkvar == 1) {
crmackey 0:4d1ae7eb0070 158 counter = counter + 1;
crmackey 0:4d1ae7eb0070 159 m3pi.cls();
crmackey 0:4d1ae7eb0070 160 m3pi.printf("%d", counter);
crmackey 0:4d1ae7eb0070 161 checkvar = 0;
crmackey 0:4d1ae7eb0070 162 switch (counter) {
crmackey 0:4d1ae7eb0070 163 case 1: {
crmackey 0:4d1ae7eb0070 164 Time1 = LapTimer.read();
crmackey 0:4d1ae7eb0070 165 LapTimer.reset();
crmackey 0:4d1ae7eb0070 166 LapTimer.start();
crmackey 0:4d1ae7eb0070 167 btbee.printf("Lap 1: %f \n", Time1);
crmackey 0:4d1ae7eb0070 168 break;
crmackey 0:4d1ae7eb0070 169 }
crmackey 0:4d1ae7eb0070 170 case 2: {
crmackey 0:4d1ae7eb0070 171 Time2 = LapTimer.read();
crmackey 0:4d1ae7eb0070 172 LapTimer.reset();
crmackey 0:4d1ae7eb0070 173 LapTimer.start();
crmackey 0:4d1ae7eb0070 174 btbee.printf("Lap 2: %f \n", Time2);
crmackey 0:4d1ae7eb0070 175 break;
crmackey 0:4d1ae7eb0070 176 }
crmackey 0:4d1ae7eb0070 177 case 3: {
crmackey 0:4d1ae7eb0070 178 Time3 = LapTimer.read();
crmackey 0:4d1ae7eb0070 179 LapTimer.reset();
crmackey 0:4d1ae7eb0070 180 LapTimer.start();
crmackey 0:4d1ae7eb0070 181 btbee.printf("Lap 3: %f \n", Time3);
crmackey 0:4d1ae7eb0070 182 break;
crmackey 0:4d1ae7eb0070 183 }
crmackey 0:4d1ae7eb0070 184 case 4: {
crmackey 0:4d1ae7eb0070 185 Time4 = LapTimer.read();
crmackey 0:4d1ae7eb0070 186 LapTimer.reset();
crmackey 0:4d1ae7eb0070 187 LapTimer.start();
crmackey 0:4d1ae7eb0070 188 btbee.printf("Lap 4: %f \n", Time4);
crmackey 0:4d1ae7eb0070 189 break;
crmackey 0:4d1ae7eb0070 190 }
crmackey 0:4d1ae7eb0070 191 case 5: {
crmackey 0:4d1ae7eb0070 192 Time5 = LapTimer.read();
crmackey 0:4d1ae7eb0070 193 LapTimer.reset();
crmackey 0:4d1ae7eb0070 194 LapTimer.start();
crmackey 0:4d1ae7eb0070 195 btbee.printf("Lap 5: %f \n", Time5);
crmackey 0:4d1ae7eb0070 196 break;
crmackey 0:4d1ae7eb0070 197 }
crmackey 0:4d1ae7eb0070 198 }
crmackey 0:4d1ae7eb0070 199
crmackey 0:4d1ae7eb0070 200
crmackey 0:4d1ae7eb0070 201 }
crmackey 0:4d1ae7eb0070 202
crmackey 0:4d1ae7eb0070 203 if (counter == 5) {
crmackey 0:4d1ae7eb0070 204 m3pi.stop();
crmackey 0:4d1ae7eb0070 205 m3pi.cls();
crmackey 0:4d1ae7eb0070 206 m3pi.locate(0,0);
crmackey 0:4d1ae7eb0070 207 m3pi.printf("Ave time");
crmackey 0:4d1ae7eb0070 208 TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
crmackey 0:4d1ae7eb0070 209 m3pi.locate(0,1);
crmackey 0:4d1ae7eb0070 210 m3pi.printf("%f", TimeAve);
crmackey 0:4d1ae7eb0070 211 btbee.printf("Average time per lap: %f ", TimeAve);
crmackey 0:4d1ae7eb0070 212 exit(1);
crmackey 0:4d1ae7eb0070 213 }
crmackey 0:4d1ae7eb0070 214
crmackey 0:4d1ae7eb0070 215
crmackey 0:4d1ae7eb0070 216 }
crmackey 0:4d1ae7eb0070 217
crmackey 1:936e329e840a 218 }
crmackey 1:936e329e840a 219
crmackey 1:936e329e840a 220 }
crmackey 2:1e947dcc40cb 221
crmackey 2:1e947dcc40cb 222