Joshua Cheung / Mbed 2 deprecated encoder_v2

Dependencies:   QEI RemoteIR mbed

Fork of encoder by Micromouse 18

PID_control.cpp@7:e10dc3cb9212, 2017-11-29 (annotated)

Committer:
Joshua_Cheung
Date:
Wed Nov 29 02:48:57 2017 +0000
Revision:
7:e10dc3cb9212
Parent:
6:71829ae2ee07
Child:
8:6b2f7886768d
kinds of goes through maze, first turn

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joshua_Cheung 7:e10dc3cb9212 1
Joshua_Cheung 2:aa961ba3199e 2 #include "mbed.h"
Joshua_Cheung 2:aa961ba3199e 3 #include "QEI.h"
Joshua_Cheung 2:aa961ba3199e 4
Joshua_Cheung 2:aa961ba3199e 5 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
kensterino 4:90303483fd5f 6 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
Joshua_Cheung 7:e10dc3cb9212 7 double Kp = 0.27;//.005;
kensterino 6:71829ae2ee07 8 double Ki = 0.001;//0.0000001;
kensterino 4:90303483fd5f 9 double Kd = 0.001;
Joshua_Cheung 2:aa961ba3199e 10 PwmOut m_Right_F(PB_10);
Joshua_Cheung 2:aa961ba3199e 11 PwmOut m_Right_B(PC_7);
kensterino 4:90303483fd5f 12 PwmOut m_Left_F(PA_7);
kensterino 4:90303483fd5f 13 PwmOut m_Left_B(PB_6);
Joshua_Cheung 7:e10dc3cb9212 14 double i_speed = 0.15;
kensterino 4:90303483fd5f 15 double C_speed(0);
Joshua_Cheung 2:aa961ba3199e 16 int integrator = 0;
Joshua_Cheung 2:aa961ba3199e 17 int decayFactor = 1;
kensterino 4:90303483fd5f 18 double Error = 0;
kensterino 4:90303483fd5f 19 double prevError = 0;
Joshua_Cheung 2:aa961ba3199e 20 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
kensterino 4:90303483fd5f 21 Timer timer;
kensterino 4:90303483fd5f 22 int counter = 0;
Joshua_Cheung 2:aa961ba3199e 23
kensterino 4:90303483fd5f 24 double P_controller(int error)
kensterino 4:90303483fd5f 25 {
Joshua_Cheung 2:aa961ba3199e 26 double correction = (Kp*error);
Joshua_Cheung 2:aa961ba3199e 27 return correction;
Joshua_Cheung 2:aa961ba3199e 28 }
Joshua_Cheung 2:aa961ba3199e 29
kensterino 4:90303483fd5f 30 double I_controller(int error)
kensterino 4:90303483fd5f 31 {
Joshua_Cheung 2:aa961ba3199e 32 integrator += error;
Joshua_Cheung 2:aa961ba3199e 33 double correction = Ki*integrator;
Joshua_Cheung 2:aa961ba3199e 34 integrator /= decayFactor;
Joshua_Cheung 2:aa961ba3199e 35 return correction;
Joshua_Cheung 2:aa961ba3199e 36 }
Joshua_Cheung 2:aa961ba3199e 37
kensterino 4:90303483fd5f 38 double D_controller(int error)
kensterino 4:90303483fd5f 39 {
Joshua_Cheung 2:aa961ba3199e 40 int dError = error - prevError;
Joshua_Cheung 2:aa961ba3199e 41 int dt = timer.read_us();
Joshua_Cheung 2:aa961ba3199e 42 timer.reset();
Joshua_Cheung 2:aa961ba3199e 43 prevError = error;
kensterino 5:a49a77ddf4e3 44 double correction;
kensterino 5:a49a77ddf4e3 45 if (dt==0)
kensterino 5:a49a77ddf4e3 46 correction=0;
kensterino 5:a49a77ddf4e3 47 else
kensterino 5:a49a77ddf4e3 48 correction = Kd*dError/dt;
Joshua_Cheung 2:aa961ba3199e 49 return correction;
Joshua_Cheung 2:aa961ba3199e 50 }
Joshua_Cheung 2:aa961ba3199e 51
Joshua_Cheung 2:aa961ba3199e 52 Ticker systicker;
Joshua_Cheung 2:aa961ba3199e 53 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
kensterino 4:90303483fd5f 54 void systick()
kensterino 4:90303483fd5f 55 {
kensterino 4:90303483fd5f 56 double R_en_count = encoder_Right.getPulses()/100;
kensterino 4:90303483fd5f 57 double L_en_count = encoder_Left.getPulses()/100;
kensterino 4:90303483fd5f 58 Error = R_en_count - L_en_count;
kensterino 4:90303483fd5f 59 double ex = D_controller(Error);
kensterino 4:90303483fd5f 60 // if (ex < 0)
kensterino 4:90303483fd5f 61 // ex = -ex;
kensterino 4:90303483fd5f 62 C_speed = P_controller(Error) + I_controller(Error) + ex;
kensterino 5:a49a77ddf4e3 63 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 64 C_speed = C_speed*-1;
Joshua_Cheung 2:aa961ba3199e 65 }
Joshua_Cheung 2:aa961ba3199e 66
Joshua_Cheung 7:e10dc3cb9212 67
kensterino 4:90303483fd5f 68 void forward()
kensterino 4:90303483fd5f 69 {
kensterino 4:90303483fd5f 70 double f1_speed = i_speed + C_speed;
kensterino 4:90303483fd5f 71 double f2_speed = i_speed - C_speed;
kensterino 5:a49a77ddf4e3 72
kensterino 5:a49a77ddf4e3 73 /*pc.printf("C: %f", C_speed);
kensterino 5:a49a77ddf4e3 74 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 75 pc.printf("-");
kensterino 5:a49a77ddf4e3 76 if (C_speed > 0)
kensterino 5:a49a77ddf4e3 77 pc.printf("+");
Joshua_Cheung 7:e10dc3cb9212 78 */
Joshua_Cheung 7:e10dc3cb9212 79
kensterino 5:a49a77ddf4e3 80
kensterino 5:a49a77ddf4e3 81 if(f1_speed >= 0.7) { //upper bound, can not go faster
kensterino 4:90303483fd5f 82 f1_speed = 0.7;
kensterino 4:90303483fd5f 83 }
kensterino 5:a49a77ddf4e3 84 if (f1_speed <= 0.05)
kensterino 5:a49a77ddf4e3 85 f1_speed = 0.05;
kensterino 5:a49a77ddf4e3 86
kensterino 5:a49a77ddf4e3 87 if(f2_speed <= 0.05) { //lower bound, should not be slower than this
kensterino 5:a49a77ddf4e3 88 f2_speed = 0.05;
kensterino 4:90303483fd5f 89 }
kensterino 5:a49a77ddf4e3 90
kensterino 5:a49a77ddf4e3 91 pc.printf(" f1: %f", f1_speed);
kensterino 5:a49a77ddf4e3 92 pc.printf(" f2: %f", f2_speed);
kensterino 5:a49a77ddf4e3 93
kensterino 4:90303483fd5f 94 //problems when left wheel is held for the + case
kensterino 4:90303483fd5f 95 if (Error > 0) { //right wheel is turning more
kensterino 4:90303483fd5f 96 m_Left_F.write(f1_speed);
kensterino 4:90303483fd5f 97 m_Right_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 98 }
kensterino 4:90303483fd5f 99 if (Error < 0) { //left wheel is turning more
kensterino 4:90303483fd5f 100 m_Right_F.write(f1_speed);
kensterino 4:90303483fd5f 101 m_Left_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 102 }
kensterino 5:a49a77ddf4e3 103 if (Error == 0) {
kensterino 4:90303483fd5f 104 m_Right_F.write(i_speed);
kensterino 5:a49a77ddf4e3 105 m_Left_F.write(i_speed);
Joshua_Cheung 2:aa961ba3199e 106 }
kensterino 4:90303483fd5f 107 }
kensterino 4:90303483fd5f 108
Joshua_Cheung 7:e10dc3cb9212 109 void backUp()
Joshua_Cheung 7:e10dc3cb9212 110 {
Joshua_Cheung 7:e10dc3cb9212 111 m_Left_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 112 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 113 m_Left_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 114 m_Right_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 115 wait(.15);
Joshua_Cheung 7:e10dc3cb9212 116 }
Joshua_Cheung 7:e10dc3cb9212 117
kensterino 4:90303483fd5f 118 void turnRight()
kensterino 4:90303483fd5f 119 {
Joshua_Cheung 7:e10dc3cb9212 120 m_Left_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 121 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 122 m_Left_F.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 123 m_Right_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 124 wait(.2);
kensterino 4:90303483fd5f 125 }
kensterino 4:90303483fd5f 126
kensterino 4:90303483fd5f 127 void turnLeft()
kensterino 4:90303483fd5f 128 {
Joshua_Cheung 7:e10dc3cb9212 129 m_Left_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 130 m_Right_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 131 m_Right_F.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 132 m_Left_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 133 wait(.2);
kensterino 4:90303483fd5f 134 }
kensterino 4:90303483fd5f 135
kensterino 4:90303483fd5f 136 void turnAround()
kensterino 4:90303483fd5f 137 {
Joshua_Cheung 7:e10dc3cb9212 138 m_Left_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 139 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 140 m_Left_F.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 141 m_Right_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 142 wait(.4);
Joshua_Cheung 7:e10dc3cb9212 143 }
kensterino 4:90303483fd5f 144
Joshua_Cheung 7:e10dc3cb9212 145 void stop()
Joshua_Cheung 7:e10dc3cb9212 146 {
Joshua_Cheung 7:e10dc3cb9212 147 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 148 m_Right_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 149 m_Left_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 150 m_Left_B.write(0);
Joshua_Cheung 2:aa961ba3199e 151 }
kensterino 4:90303483fd5f 152
kensterino 4:90303483fd5f 153 void debugEncoder()
kensterino 4:90303483fd5f 154 {
kensterino 4:90303483fd5f 155 while(1) {
kensterino 4:90303483fd5f 156 wait(1);
kensterino 4:90303483fd5f 157 pc.printf("Right: %i", encoder_Right.getPulses());
kensterino 4:90303483fd5f 158 pc.printf(" Left: %i", encoder_Left.getPulses(), "\n");
kensterino 4:90303483fd5f 159 pc.printf("\n");
kensterino 4:90303483fd5f 160 }
kensterino 4:90303483fd5f 161 }
kensterino 4:90303483fd5f 162
kensterino 4:90303483fd5f 163 void debugError()
kensterino 4:90303483fd5f 164 {
kensterino 4:90303483fd5f 165 while(1) {
kensterino 4:90303483fd5f 166 pc.printf("Error: %i\n", Error);
kensterino 4:90303483fd5f 167 }
kensterino 4:90303483fd5f 168 }
kensterino 4:90303483fd5f 169
Joshua_Cheung 7:e10dc3cb9212 170 AnalogIn RS_IRR(PA_0);
Joshua_Cheung 7:e10dc3cb9212 171 AnalogIn RF_IRR(PA_4); //Right Front
Joshua_Cheung 7:e10dc3cb9212 172 AnalogIn LF_IRR(PC_1); //Left Front
Joshua_Cheung 7:e10dc3cb9212 173 AnalogIn LS_IRR(PC_0); //Left Side
Joshua_Cheung 7:e10dc3cb9212 174
Joshua_Cheung 7:e10dc3cb9212 175 DigitalOut RS_IRE(PC_10);
Joshua_Cheung 7:e10dc3cb9212 176 DigitalOut RF_IRE(PC_11); //Right Front
Joshua_Cheung 7:e10dc3cb9212 177 DigitalOut LF_IRE(PB_0); //Left Front
Joshua_Cheung 7:e10dc3cb9212 178 DigitalOut LS_IRE(PB_7); //Left Side
Joshua_Cheung 7:e10dc3cb9212 179
Joshua_Cheung 7:e10dc3cb9212 180
Joshua_Cheung 7:e10dc3cb9212 181
Joshua_Cheung 7:e10dc3cb9212 182 int main()
Joshua_Cheung 7:e10dc3cb9212 183 {
Joshua_Cheung 7:e10dc3cb9212 184 float threshold = 0.001;
Joshua_Cheung 7:e10dc3cb9212 185 float turnThreshold = 0.001;
Joshua_Cheung 7:e10dc3cb9212 186 printf("\nAnalogIn example\n");
Joshua_Cheung 7:e10dc3cb9212 187 LF_IRE.write(1);
Joshua_Cheung 7:e10dc3cb9212 188 RF_IRE.write(1);
Joshua_Cheung 7:e10dc3cb9212 189 while (1){
Joshua_Cheung 7:e10dc3cb9212 190 while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
Joshua_Cheung 7:e10dc3cb9212 191 forward();
Joshua_Cheung 7:e10dc3cb9212 192 float value1 = LF_IRR.read();
Joshua_Cheung 7:e10dc3cb9212 193 float value2 = RF_IRR.read();
Joshua_Cheung 7:e10dc3cb9212 194 printf("LF Led: %f\n", value1);
Joshua_Cheung 7:e10dc3cb9212 195 wait(0.5);
Joshua_Cheung 7:e10dc3cb9212 196 printf("RF Led: %f\n", value2);
Joshua_Cheung 7:e10dc3cb9212 197 }
Joshua_Cheung 7:e10dc3cb9212 198
Joshua_Cheung 7:e10dc3cb9212 199 backUp();
Joshua_Cheung 7:e10dc3cb9212 200
Joshua_Cheung 7:e10dc3cb9212 201 if (LS_IRR.read() > turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 202 if (RS_IRR.read() < turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 203 turnRight();
Joshua_Cheung 7:e10dc3cb9212 204 else
Joshua_Cheung 7:e10dc3cb9212 205 turnAround();
Joshua_Cheung 7:e10dc3cb9212 206 else if (RS_IRR.read() > turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 207 if (LS_IRR.read() < turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 208 turnRight();
Joshua_Cheung 7:e10dc3cb9212 209 else
Joshua_Cheung 7:e10dc3cb9212 210 turnAround();
Joshua_Cheung 7:e10dc3cb9212 211 else
Joshua_Cheung 7:e10dc3cb9212 212 turnAround();
Joshua_Cheung 7:e10dc3cb9212 213 }
Joshua_Cheung 7:e10dc3cb9212 214 stop();
Joshua_Cheung 7:e10dc3cb9212 215 }
Joshua_Cheung 7:e10dc3cb9212 216 /*while(RF_IRR.read() * 100000 < 175 && LF_IRR.read() * 100000 < 175) {
Joshua_Cheung 7:e10dc3cb9212 217
Joshua_Cheung 7:e10dc3cb9212 218 /*meas = LS_IRR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Joshua_Cheung 7:e10dc3cb9212 219 meas = meas * 100000; // Change the value to be in the 0 to 3300 range
Joshua_Cheung 7:e10dc3cb9212 220 printf("measure = %.0f mV\n", meas);
Joshua_Cheung 7:e10dc3cb9212 221 */
Joshua_Cheung 7:e10dc3cb9212 222 /*if (meas > 2000) { // If the value is greater than 2V then switch the LED on
Joshua_Cheung 7:e10dc3cb9212 223 LS_IRE = 1;
Joshua_Cheung 7:e10dc3cb9212 224 }
Joshua_Cheung 7:e10dc3cb9212 225 else {
Joshua_Cheung 7:e10dc3cb9212 226 LS_IRE = 0;
Joshua_Cheung 7:e10dc3cb9212 227 }
Joshua_Cheung 7:e10dc3cb9212 228 */
Joshua_Cheung 7:e10dc3cb9212 229 /*
Joshua_Cheung 7:e10dc3cb9212 230 forward();
Joshua_Cheung 7:e10dc3cb9212 231 //wait(0.2); // 200 ms
Joshua_Cheung 7:e10dc3cb9212 232 }
Joshua_Cheung 7:e10dc3cb9212 233 stop();
Joshua_Cheung 7:e10dc3cb9212 234 */
Joshua_Cheung 7:e10dc3cb9212 235
Joshua_Cheung 7:e10dc3cb9212 236
Joshua_Cheung 7:e10dc3cb9212 237
Joshua_Cheung 7:e10dc3cb9212 238
Joshua_Cheung 7:e10dc3cb9212 239 /*int main() //only runs once
kensterino 4:90303483fd5f 240 {
kensterino 4:90303483fd5f 241 systicker.attach_us(&systick, 1000); //enable interrupt
kensterino 5:a49a77ddf4e3 242 while (1) {
kensterino 5:a49a77ddf4e3 243 forward();
kensterino 5:a49a77ddf4e3 244 }
kensterino 4:90303483fd5f 245 //
kensterino 5:a49a77ddf4e3 246 //debugEncoder();
kensterino 4:90303483fd5f 247 }
Joshua_Cheung 7:e10dc3cb9212 248 */