MEGACARAZ / Mbed 2 deprecated ILC

Iterative Learning Control

Dependencies:   btbee m3pi_ng mbed

main.cpp@5:77f8d658adc3, 2015-06-10 (annotated)

Committer:
morganrose402
Date:
Wed Jun 10 12:37:50 2015 +0000
Revision:
5:77f8d658adc3
Parent:
4:8918484e8125 
l

Who changed what in which revision?

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