Micromouse 18 / Mbed 2 deprecated encoder_v2

Dependencies:   QEI RemoteIR mbed

Fork of encoder_v2 by Joshua Cheung

PID_control.cpp@4:90303483fd5f, 2017-11-10 (annotated)

Committer:
kensterino
Date:
Fri Nov 10 23:05:18 2017 +0000
Revision:
4:90303483fd5f
Parent:
2:aa961ba3199e
Child:
5:a49a77ddf4e3
wip

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 4:90303483fd5f 6 double Kp = .005;
kensterino 4:90303483fd5f 7 double Ki = 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 4:90303483fd5f 13 double i_speed = 0.1;
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);
kensterino 4:90303483fd5f 26 //pc.printf("correction is: %d\n", correction);
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;
kensterino 4:90303483fd5f 35
Joshua_Cheung 2:aa961ba3199e 36 return correction;
Joshua_Cheung 2:aa961ba3199e 37 }
Joshua_Cheung 2:aa961ba3199e 38
kensterino 4:90303483fd5f 39 double D_controller(int error)
kensterino 4:90303483fd5f 40 {
Joshua_Cheung 2:aa961ba3199e 41 int dError = error - prevError;
Joshua_Cheung 2:aa961ba3199e 42 int dt = timer.read_us();
Joshua_Cheung 2:aa961ba3199e 43 timer.reset();
Joshua_Cheung 2:aa961ba3199e 44 prevError = error;
kensterino 4:90303483fd5f 45 double correction = Kd*dError/dt;
Joshua_Cheung 2:aa961ba3199e 46 return correction;
Joshua_Cheung 2:aa961ba3199e 47 }
Joshua_Cheung 2:aa961ba3199e 48
Joshua_Cheung 2:aa961ba3199e 49 Ticker systicker;
Joshua_Cheung 2:aa961ba3199e 50 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
kensterino 4:90303483fd5f 51 void systick()
kensterino 4:90303483fd5f 52 {
kensterino 4:90303483fd5f 53 double R_en_count = encoder_Right.getPulses()/100;
kensterino 4:90303483fd5f 54 double L_en_count = encoder_Left.getPulses()/100;
kensterino 4:90303483fd5f 55 Error = R_en_count - L_en_count;
kensterino 4:90303483fd5f 56 double ex = D_controller(Error);
kensterino 4:90303483fd5f 57 // if (ex < 0)
kensterino 4:90303483fd5f 58 // ex = -ex;
kensterino 4:90303483fd5f 59 C_speed = P_controller(Error) + I_controller(Error) + ex;
Joshua_Cheung 2:aa961ba3199e 60 }
Joshua_Cheung 2:aa961ba3199e 61
kensterino 4:90303483fd5f 62 void forward()
kensterino 4:90303483fd5f 63 {
kensterino 4:90303483fd5f 64 double f1_speed = i_speed + C_speed;
kensterino 4:90303483fd5f 65 double f2_speed = i_speed - C_speed;
kensterino 4:90303483fd5f 66
kensterino 4:90303483fd5f 67 if (f2_speed > f1_speed) //if f2>f1, KILL
kensterino 4:90303483fd5f 68 {
kensterino 4:90303483fd5f 69 while(1) {
kensterino 4:90303483fd5f 70 m_Left_F.write(0);
kensterino 4:90303483fd5f 71 m_Right_F.write(0);
kensterino 4:90303483fd5f 72 }
kensterino 4:90303483fd5f 73 }
kensterino 4:90303483fd5f 74
kensterino 4:90303483fd5f 75 if(f1_speed >= 0.7) {
kensterino 4:90303483fd5f 76 f1_speed = 0.7;
kensterino 4:90303483fd5f 77 }
kensterino 4:90303483fd5f 78 if(f2_speed <= 0.2) {
kensterino 4:90303483fd5f 79 f2_speed = 0.2;
kensterino 4:90303483fd5f 80 }
kensterino 4:90303483fd5f 81 //problems when left wheel is held for the + case
kensterino 4:90303483fd5f 82 if (Error > 0) { //right wheel is turning more
kensterino 4:90303483fd5f 83 m_Left_F.write(f1_speed);
kensterino 4:90303483fd5f 84 m_Right_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 85 }
kensterino 4:90303483fd5f 86 if (Error < 0) { //left wheel is turning more
kensterino 4:90303483fd5f 87 m_Right_F.write(f1_speed);
kensterino 4:90303483fd5f 88 m_Left_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 89 }
kensterino 4:90303483fd5f 90 if (Error == 0)
Joshua_Cheung 2:aa961ba3199e 91 {
kensterino 4:90303483fd5f 92 m_Right_F.write(i_speed);
kensterino 4:90303483fd5f 93 m_Left_F.write(i_speed);
Joshua_Cheung 2:aa961ba3199e 94 }
kensterino 4:90303483fd5f 95 }
kensterino 4:90303483fd5f 96
kensterino 4:90303483fd5f 97 void turnRight()
kensterino 4:90303483fd5f 98 {
kensterino 4:90303483fd5f 99
kensterino 4:90303483fd5f 100 }
kensterino 4:90303483fd5f 101
kensterino 4:90303483fd5f 102 void turnLeft()
kensterino 4:90303483fd5f 103 {
kensterino 4:90303483fd5f 104
kensterino 4:90303483fd5f 105 }
kensterino 4:90303483fd5f 106
kensterino 4:90303483fd5f 107 void turnAround()
kensterino 4:90303483fd5f 108 {
kensterino 4:90303483fd5f 109
Joshua_Cheung 2:aa961ba3199e 110 }
kensterino 4:90303483fd5f 111
kensterino 4:90303483fd5f 112 void debugEncoder()
kensterino 4:90303483fd5f 113 {
kensterino 4:90303483fd5f 114 while(1) {
kensterino 4:90303483fd5f 115 wait(1);
kensterino 4:90303483fd5f 116 pc.printf("Right: %i", encoder_Right.getPulses());
kensterino 4:90303483fd5f 117 pc.printf(" Left: %i", encoder_Left.getPulses(), "\n");
kensterino 4:90303483fd5f 118 pc.printf("\n");
kensterino 4:90303483fd5f 119 }
kensterino 4:90303483fd5f 120 }
kensterino 4:90303483fd5f 121
kensterino 4:90303483fd5f 122 void debugError()
kensterino 4:90303483fd5f 123 {
kensterino 4:90303483fd5f 124 while(1) {
kensterino 4:90303483fd5f 125 pc.printf("Error: %i\n", Error);
kensterino 4:90303483fd5f 126 }
kensterino 4:90303483fd5f 127 }
kensterino 4:90303483fd5f 128
kensterino 4:90303483fd5f 129 int main() //only runs once
kensterino 4:90303483fd5f 130 {
kensterino 4:90303483fd5f 131 systicker.attach_us(&systick, 1000); //enable interrupt
kensterino 4:90303483fd5f 132 while (1) {forward();}
kensterino 4:90303483fd5f 133 //
kensterino 4:90303483fd5f 134 }