Micromouse 18 / Mbed 2 deprecated encoder_v2

Dependencies:   QEI RemoteIR mbed

Fork of encoder_v2 by Joshua Cheung

PID_control.cpp@2:aa961ba3199e, 2017-11-10 (annotated)

Committer:
Joshua_Cheung
Date:
Fri Nov 10 19:23:08 2017 +0000
Revision:
2:aa961ba3199e
Child:
4:90303483fd5f
PID control

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);
Joshua_Cheung 2:aa961ba3199e 5 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
Joshua_Cheung 2:aa961ba3199e 6 double Kp = .02;
Joshua_Cheung 2:aa961ba3199e 7 double Ki = .0000001;
Joshua_Cheung 2:aa961ba3199e 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);
Joshua_Cheung 2:aa961ba3199e 11 PwmOut m_Left_F(PA_7);
Joshua_Cheung 2:aa961ba3199e 12 PwmOut m_Left_B(PB_6);
Joshua_Cheung 2:aa961ba3199e 13 double i_speed = 0.3;
Joshua_Cheung 2:aa961ba3199e 14 int integrator = 0;
Joshua_Cheung 2:aa961ba3199e 15 int decayFactor = 1;
Joshua_Cheung 2:aa961ba3199e 16 int prevError = 0;
Joshua_Cheung 2:aa961ba3199e 17 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
Joshua_Cheung 2:aa961ba3199e 18 Timer timer;
Joshua_Cheung 2:aa961ba3199e 19
Joshua_Cheung 2:aa961ba3199e 20 double P_controller(int error) {
Joshua_Cheung 2:aa961ba3199e 21 double correction = (Kp*error);
Joshua_Cheung 2:aa961ba3199e 22 return correction;
Joshua_Cheung 2:aa961ba3199e 23 }
Joshua_Cheung 2:aa961ba3199e 24
Joshua_Cheung 2:aa961ba3199e 25 double I_controller(int error) {
Joshua_Cheung 2:aa961ba3199e 26 integrator += error;
Joshua_Cheung 2:aa961ba3199e 27 double correction = Ki*integrator;
Joshua_Cheung 2:aa961ba3199e 28 integrator /= decayFactor;
Joshua_Cheung 2:aa961ba3199e 29
Joshua_Cheung 2:aa961ba3199e 30 return correction;
Joshua_Cheung 2:aa961ba3199e 31 }
Joshua_Cheung 2:aa961ba3199e 32
Joshua_Cheung 2:aa961ba3199e 33 double D_controller(int error) {
Joshua_Cheung 2:aa961ba3199e 34 int dError = error - prevError;
Joshua_Cheung 2:aa961ba3199e 35 int dt = timer.read_us();
Joshua_Cheung 2:aa961ba3199e 36 timer.reset();
Joshua_Cheung 2:aa961ba3199e 37 prevError = error;
Joshua_Cheung 2:aa961ba3199e 38 int correction = Kd*dError/dt;
Joshua_Cheung 2:aa961ba3199e 39 return correction;
Joshua_Cheung 2:aa961ba3199e 40 }
Joshua_Cheung 2:aa961ba3199e 41
Joshua_Cheung 2:aa961ba3199e 42 Ticker systicker;
Joshua_Cheung 2:aa961ba3199e 43 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
Joshua_Cheung 2:aa961ba3199e 44 void systick() {
Joshua_Cheung 2:aa961ba3199e 45 int R_en_count = encoder_Right.getPulses()/100;
Joshua_Cheung 2:aa961ba3199e 46 int L_en_count = encoder_Left.getPulses()/100;
Joshua_Cheung 2:aa961ba3199e 47 int error = R_en_count - L_en_count;
Joshua_Cheung 2:aa961ba3199e 48 double C_speed = P_controller(error) + I_controller(error) + D_controller(error);
Joshua_Cheung 2:aa961ba3199e 49 double f1_speed = C_speed + i_speed;
Joshua_Cheung 2:aa961ba3199e 50 double f2_speed = C_speed - i_speed;
Joshua_Cheung 2:aa961ba3199e 51 if(f1_speed >= 0.7)
Joshua_Cheung 2:aa961ba3199e 52 {
Joshua_Cheung 2:aa961ba3199e 53 f1_speed = 0.7;
Joshua_Cheung 2:aa961ba3199e 54 }
Joshua_Cheung 2:aa961ba3199e 55 if(f2_speed <= 0.2)
Joshua_Cheung 2:aa961ba3199e 56 {
Joshua_Cheung 2:aa961ba3199e 57 f2_speed = 0.2;
Joshua_Cheung 2:aa961ba3199e 58 }
Joshua_Cheung 2:aa961ba3199e 59
Joshua_Cheung 2:aa961ba3199e 60 if (error > 0)
Joshua_Cheung 2:aa961ba3199e 61 {
Joshua_Cheung 2:aa961ba3199e 62 m_Left_F.write(f1_speed);
Joshua_Cheung 2:aa961ba3199e 63 m_Right_F.write(f2_speed);
Joshua_Cheung 2:aa961ba3199e 64 }
Joshua_Cheung 2:aa961ba3199e 65 else
Joshua_Cheung 2:aa961ba3199e 66 {
Joshua_Cheung 2:aa961ba3199e 67 m_Right_F.write(f1_speed);
Joshua_Cheung 2:aa961ba3199e 68 m_Left_F.write(f2_speed);
Joshua_Cheung 2:aa961ba3199e 69 }
Joshua_Cheung 2:aa961ba3199e 70
Joshua_Cheung 2:aa961ba3199e 71
Joshua_Cheung 2:aa961ba3199e 72 }
Joshua_Cheung 2:aa961ba3199e 73
Joshua_Cheung 2:aa961ba3199e 74 int main() {
Joshua_Cheung 2:aa961ba3199e 75 systicker.attach_us(&systick, 1000);
Joshua_Cheung 2:aa961ba3199e 76 m_Left_F.write(i_speed);
Joshua_Cheung 2:aa961ba3199e 77 m_Right_F.write(i_speed);
Joshua_Cheung 2:aa961ba3199e 78 while(1)
Joshua_Cheung 2:aa961ba3199e 79 {
Joshua_Cheung 2:aa961ba3199e 80 wait(.1);
Joshua_Cheung 2:aa961ba3199e 81 pc.printf("Right Pulses sre: %i\n", encoder_Right.getPulses());
Joshua_Cheung 2:aa961ba3199e 82 pc.printf("Left Pulses are: %i\n", encoder_Left.getPulses());
Joshua_Cheung 2:aa961ba3199e 83 }
Joshua_Cheung 2:aa961ba3199e 84
Joshua_Cheung 2:aa961ba3199e 85 }