Joshua Cheung / Mbed 2 deprecated encoder_v2

Dependencies:   QEI RemoteIR mbed

Fork of encoder by Micromouse 18

PID_control.cpp@6:71829ae2ee07, 2017-11-11 (annotated)

Committer:
kensterino
Date:
Sat Nov 11 03:05:20 2017 +0000
Revision:
6:71829ae2ee07
Parent:
5:a49a77ddf4e3
Child:
7:e10dc3cb9212
got checked off. mostly straight. should be optimized;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joshua_Cheung 2:aa961ba3199e 1 #include "mbed.h"
Joshua_Cheung 2:aa961ba3199e 2 #include "QEI.h"
Joshua_Cheung 2:aa961ba3199e 3
Joshua_Cheung 2:aa961ba3199e 4 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
kensterino 4:90303483fd5f 5 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
kensterino 6:71829ae2ee07 6 double Kp = 0.3;//.005;
kensterino 6:71829ae2ee07 7 double Ki = 0.001;//0.0000001;
kensterino 4:90303483fd5f 8 double Kd = 0.001;
Joshua_Cheung 2:aa961ba3199e 9 PwmOut m_Right_F(PB_10);
Joshua_Cheung 2:aa961ba3199e 10 PwmOut m_Right_B(PC_7);
kensterino 4:90303483fd5f 11 PwmOut m_Left_F(PA_7);
kensterino 4:90303483fd5f 12 PwmOut m_Left_B(PB_6);
kensterino 6:71829ae2ee07 13 double i_speed = 0.3;
kensterino 4:90303483fd5f 14 double C_speed(0);
Joshua_Cheung 2:aa961ba3199e 15 int integrator = 0;
Joshua_Cheung 2:aa961ba3199e 16 int decayFactor = 1;
kensterino 4:90303483fd5f 17 double Error = 0;
kensterino 4:90303483fd5f 18 double prevError = 0;
Joshua_Cheung 2:aa961ba3199e 19 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
kensterino 4:90303483fd5f 20 Timer timer;
kensterino 4:90303483fd5f 21 int counter = 0;
Joshua_Cheung 2:aa961ba3199e 22
kensterino 4:90303483fd5f 23 double P_controller(int error)
kensterino 4:90303483fd5f 24 {
Joshua_Cheung 2:aa961ba3199e 25 double correction = (Kp*error);
Joshua_Cheung 2:aa961ba3199e 26 return correction;
Joshua_Cheung 2:aa961ba3199e 27 }
Joshua_Cheung 2:aa961ba3199e 28
kensterino 4:90303483fd5f 29 double I_controller(int error)
kensterino 4:90303483fd5f 30 {
Joshua_Cheung 2:aa961ba3199e 31 integrator += error;
Joshua_Cheung 2:aa961ba3199e 32 double correction = Ki*integrator;
Joshua_Cheung 2:aa961ba3199e 33 integrator /= decayFactor;
Joshua_Cheung 2:aa961ba3199e 34 return correction;
Joshua_Cheung 2:aa961ba3199e 35 }
Joshua_Cheung 2:aa961ba3199e 36
kensterino 4:90303483fd5f 37 double D_controller(int error)
kensterino 4:90303483fd5f 38 {
Joshua_Cheung 2:aa961ba3199e 39 int dError = error - prevError;
Joshua_Cheung 2:aa961ba3199e 40 int dt = timer.read_us();
Joshua_Cheung 2:aa961ba3199e 41 timer.reset();
Joshua_Cheung 2:aa961ba3199e 42 prevError = error;
kensterino 5:a49a77ddf4e3 43 double correction;
kensterino 5:a49a77ddf4e3 44 if (dt==0)
kensterino 5:a49a77ddf4e3 45 correction=0;
kensterino 5:a49a77ddf4e3 46 else
kensterino 5:a49a77ddf4e3 47 correction = Kd*dError/dt;
Joshua_Cheung 2:aa961ba3199e 48 return correction;
Joshua_Cheung 2:aa961ba3199e 49 }
Joshua_Cheung 2:aa961ba3199e 50
Joshua_Cheung 2:aa961ba3199e 51 Ticker systicker;
Joshua_Cheung 2:aa961ba3199e 52 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
kensterino 4:90303483fd5f 53 void systick()
kensterino 4:90303483fd5f 54 {
kensterino 4:90303483fd5f 55 double R_en_count = encoder_Right.getPulses()/100;
kensterino 4:90303483fd5f 56 double L_en_count = encoder_Left.getPulses()/100;
kensterino 4:90303483fd5f 57 Error = R_en_count - L_en_count;
kensterino 4:90303483fd5f 58 double ex = D_controller(Error);
kensterino 4:90303483fd5f 59 // if (ex < 0)
kensterino 4:90303483fd5f 60 // ex = -ex;
kensterino 4:90303483fd5f 61 C_speed = P_controller(Error) + I_controller(Error) + ex;
kensterino 5:a49a77ddf4e3 62 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 63 C_speed = C_speed*-1;
Joshua_Cheung 2:aa961ba3199e 64 }
Joshua_Cheung 2:aa961ba3199e 65
kensterino 4:90303483fd5f 66 void forward()
kensterino 4:90303483fd5f 67 {
kensterino 4:90303483fd5f 68 double f1_speed = i_speed + C_speed;
kensterino 4:90303483fd5f 69 double f2_speed = i_speed - C_speed;
kensterino 5:a49a77ddf4e3 70
kensterino 5:a49a77ddf4e3 71 /*pc.printf("C: %f", C_speed);
kensterino 5:a49a77ddf4e3 72 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 73 pc.printf("-");
kensterino 5:a49a77ddf4e3 74 if (C_speed > 0)
kensterino 5:a49a77ddf4e3 75 pc.printf("+");
kensterino 5:a49a77ddf4e3 76 */
kensterino 5:a49a77ddf4e3 77
kensterino 5:a49a77ddf4e3 78 if(f1_speed >= 0.7) { //upper bound, can not go faster
kensterino 4:90303483fd5f 79 f1_speed = 0.7;
kensterino 4:90303483fd5f 80 }
kensterino 5:a49a77ddf4e3 81 if (f1_speed <= 0.05)
kensterino 5:a49a77ddf4e3 82 f1_speed = 0.05;
kensterino 5:a49a77ddf4e3 83
kensterino 5:a49a77ddf4e3 84 if(f2_speed <= 0.05) { //lower bound, should not be slower than this
kensterino 5:a49a77ddf4e3 85 f2_speed = 0.05;
kensterino 4:90303483fd5f 86 }
kensterino 5:a49a77ddf4e3 87
kensterino 5:a49a77ddf4e3 88 pc.printf(" f1: %f", f1_speed);
kensterino 5:a49a77ddf4e3 89 pc.printf(" f2: %f", f2_speed);
kensterino 5:a49a77ddf4e3 90
kensterino 4:90303483fd5f 91 //problems when left wheel is held for the + case
kensterino 4:90303483fd5f 92 if (Error > 0) { //right wheel is turning more
kensterino 4:90303483fd5f 93 m_Left_F.write(f1_speed);
kensterino 4:90303483fd5f 94 m_Right_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 95 }
kensterino 4:90303483fd5f 96 if (Error < 0) { //left wheel is turning more
kensterino 4:90303483fd5f 97 m_Right_F.write(f1_speed);
kensterino 4:90303483fd5f 98 m_Left_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 99 }
kensterino 5:a49a77ddf4e3 100 if (Error == 0) {
kensterino 4:90303483fd5f 101 m_Right_F.write(i_speed);
kensterino 5:a49a77ddf4e3 102 m_Left_F.write(i_speed);
Joshua_Cheung 2:aa961ba3199e 103 }
kensterino 4:90303483fd5f 104 }
kensterino 4:90303483fd5f 105
kensterino 4:90303483fd5f 106 void turnRight()
kensterino 4:90303483fd5f 107 {
kensterino 4:90303483fd5f 108
kensterino 4:90303483fd5f 109 }
kensterino 4:90303483fd5f 110
kensterino 4:90303483fd5f 111 void turnLeft()
kensterino 4:90303483fd5f 112 {
kensterino 4:90303483fd5f 113
kensterino 4:90303483fd5f 114 }
kensterino 4:90303483fd5f 115
kensterino 4:90303483fd5f 116 void turnAround()
kensterino 4:90303483fd5f 117 {
kensterino 4:90303483fd5f 118
Joshua_Cheung 2:aa961ba3199e 119 }
kensterino 4:90303483fd5f 120
kensterino 4:90303483fd5f 121 void debugEncoder()
kensterino 4:90303483fd5f 122 {
kensterino 4:90303483fd5f 123 while(1) {
kensterino 4:90303483fd5f 124 wait(1);
kensterino 4:90303483fd5f 125 pc.printf("Right: %i", encoder_Right.getPulses());
kensterino 4:90303483fd5f 126 pc.printf(" Left: %i", encoder_Left.getPulses(), "\n");
kensterino 4:90303483fd5f 127 pc.printf("\n");
kensterino 4:90303483fd5f 128 }
kensterino 4:90303483fd5f 129 }
kensterino 4:90303483fd5f 130
kensterino 4:90303483fd5f 131 void debugError()
kensterino 4:90303483fd5f 132 {
kensterino 4:90303483fd5f 133 while(1) {
kensterino 4:90303483fd5f 134 pc.printf("Error: %i\n", Error);
kensterino 4:90303483fd5f 135 }
kensterino 4:90303483fd5f 136 }
kensterino 4:90303483fd5f 137
kensterino 4:90303483fd5f 138 int main() //only runs once
kensterino 4:90303483fd5f 139 {
kensterino 4:90303483fd5f 140 systicker.attach_us(&systick, 1000); //enable interrupt
kensterino 5:a49a77ddf4e3 141 while (1) {
kensterino 5:a49a77ddf4e3 142 forward();
kensterino 5:a49a77ddf4e3 143 }
kensterino 4:90303483fd5f 144 //
kensterino 5:a49a77ddf4e3 145 //debugEncoder();
kensterino 4:90303483fd5f 146 }