Kevin Hetterscheid / PIDController

PID Controller

Dependencies:   HIDScope MODSERIAL PinDetect QEI biquadFilter

pidControl.cpp@4:a722452d8fd1, 2015-10-20 (annotated)

Committer:
dolcaer
Date:
Tue Oct 20 08:35:36 2015 +0000
Revision:
4:a722452d8fd1
Parent:
2:53d2f9b8fed1 
Initial Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AeroKev 0:d38eb4622914 1 #include "mbed.h"
AeroKev 0:d38eb4622914 2 #include "QEI.h"
AeroKev 0:d38eb4622914 3 #include "HIDScope.h"
AeroKev 0:d38eb4622914 4 #include "MODSERIAL.h"
AeroKev 0:d38eb4622914 5 #include "PinDetect.h"
AeroKev 2:53d2f9b8fed1 6 #include "pidControl.h"
AeroKev 0:d38eb4622914 7 #include "biquadFilter.h"
AeroKev 0:d38eb4622914 8
AeroKev 0:d38eb4622914 9 Serial pc(USBTX, USBRX);
AeroKev 0:d38eb4622914 10
AeroKev 0:d38eb4622914 11 PinDetect stop(SW2);
AeroKev 0:d38eb4622914 12 PinDetect start(SW3);
AeroKev 0:d38eb4622914 13 AnalogIn pot1(m1_pot);
AeroKev 0:d38eb4622914 14 AnalogIn pot2(m2_pot);
AeroKev 0:d38eb4622914 15
AeroKev 0:d38eb4622914 16 // PWM Speed Control：
AeroKev 0:d38eb4622914 17 DigitalOut dir1(m1_dir);
AeroKev 0:d38eb4622914 18 PwmOut pwm1(m1_pwm);
AeroKev 0:d38eb4622914 19 DigitalOut dir2(m2_dir);
AeroKev 0:d38eb4622914 20 PwmOut pwm2(m2_pwm);
AeroKev 0:d38eb4622914 21
AeroKev 0:d38eb4622914 22 QEI enc1(m1_enc_a, m1_enc_b, NC, 1);
AeroKev 0:d38eb4622914 23 QEI enc2(m2_enc_a, m2_enc_b, NC, 1);
AeroKev 0:d38eb4622914 24
AeroKev 0:d38eb4622914 25 Ticker potTicker;
AeroKev 0:d38eb4622914 26 Ticker motorTicker;
AeroKev 0:d38eb4622914 27
AeroKev 0:d38eb4622914 28 Ticker graphTicker;
AeroKev 1:fe23126b0389 29 //HIDScope grapher(4);
AeroKev 0:d38eb4622914 30
AeroKev 0:d38eb4622914 31 float currentRotation1 = 0, currentRotation2 = 0;
AeroKev 0:d38eb4622914 32 float desiredRotation1 = 0, desiredRotation2 = 0;
AeroKev 0:d38eb4622914 33 double error1 = 0, error2 = 0;
AeroKev 0:d38eb4622914 34
AeroKev 0:d38eb4622914 35 double m1_error_integral = 0, m2_error_integral = 0;
AeroKev 0:d38eb4622914 36 double m1_error_derivative = 0, m2_error_derivative = 0;
AeroKev 0:d38eb4622914 37 biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
AeroKev 0:d38eb4622914 38 biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);
AeroKev 0:d38eb4622914 39
AeroKev 0:d38eb4622914 40 bool shutup = true;
AeroKev 0:d38eb4622914 41 bool go_pot = false;
AeroKev 0:d38eb4622914 42 bool go_motor = false;
AeroKev 0:d38eb4622914 43 bool go_graph = false;
AeroKev 0:d38eb4622914 44
AeroKev 0:d38eb4622914 45 float getPotRad(AnalogIn pot)
AeroKev 0:d38eb4622914 46 {
AeroKev 0:d38eb4622914 47 return pot.read() * 4.0f - 2.0f;
AeroKev 0:d38eb4622914 48 }
AeroKev 0:d38eb4622914 49
AeroKev 0:d38eb4622914 50 float toRadians(int pulses)
AeroKev 0:d38eb4622914 51 {
AeroKev 0:d38eb4622914 52 int remaining = pulses;// % sigPerRev;
AeroKev 0:d38eb4622914 53 float percent = (float) remaining / (float) sigPerRev;
AeroKev 0:d38eb4622914 54 return percent * 2.0f;
AeroKev 0:d38eb4622914 55 }
AeroKev 0:d38eb4622914 56
AeroKev 0:d38eb4622914 57 void readPot()
AeroKev 0:d38eb4622914 58 {
AeroKev 0:d38eb4622914 59 go_pot = true;
AeroKev 0:d38eb4622914 60 }
AeroKev 0:d38eb4622914 61
AeroKev 0:d38eb4622914 62 void getMotorRotation()
AeroKev 0:d38eb4622914 63 {
AeroKev 0:d38eb4622914 64 go_motor = true;
AeroKev 0:d38eb4622914 65 }
AeroKev 0:d38eb4622914 66
AeroKev 0:d38eb4622914 67 void stopMotors()
AeroKev 0:d38eb4622914 68 {
AeroKev 0:d38eb4622914 69 shutup = true;
AeroKev 0:d38eb4622914 70 }
AeroKev 0:d38eb4622914 71
AeroKev 0:d38eb4622914 72 void startMotors()
AeroKev 0:d38eb4622914 73 {
AeroKev 0:d38eb4622914 74 shutup = false;
AeroKev 0:d38eb4622914 75 }
AeroKev 0:d38eb4622914 76
AeroKev 0:d38eb4622914 77 void sendGraph()
AeroKev 0:d38eb4622914 78 {
AeroKev 0:d38eb4622914 79 go_graph = true;
AeroKev 0:d38eb4622914 80 }
AeroKev 0:d38eb4622914 81
AeroKev 0:d38eb4622914 82 double p_control(double error, double kp)
AeroKev 0:d38eb4622914 83 {
AeroKev 0:d38eb4622914 84 return kp * error;
AeroKev 0:d38eb4622914 85 }
AeroKev 0:d38eb4622914 86
AeroKev 0:d38eb4622914 87 double pi_control(double error, double kp, double ki, double ts, double &error_integral)
AeroKev 0:d38eb4622914 88 {
AeroKev 0:d38eb4622914 89 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 90 double result = kp * error + ki * error_integral;
AeroKev 0:d38eb4622914 91 return result;
AeroKev 0:d38eb4622914 92 }
AeroKev 0:d38eb4622914 93
AeroKev 0:d38eb4622914 94 double pid_control(double error, double kp, double ki, double ts, double &error_integral,
AeroKev 0:d38eb4622914 95 double kd, double previous_error, double &error_derivative, biquadFilter filter)
AeroKev 0:d38eb4622914 96 {
AeroKev 0:d38eb4622914 97 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 98 error_derivative = (error - previous_error) / ts;
AeroKev 0:d38eb4622914 99 // error_derivative = filter.step(error_derivative);
AeroKev 0:d38eb4622914 100
AeroKev 0:d38eb4622914 101 double result = kp * error + ki * error_integral + kd * error_derivative;
AeroKev 0:d38eb4622914 102 return result;
AeroKev 0:d38eb4622914 103 }
AeroKev 0:d38eb4622914 104
AeroKev 1:fe23126b0389 105 int getPDirection(double control, int motor)
AeroKev 0:d38eb4622914 106 {
AeroKev 0:d38eb4622914 107 if (control >= 0)
AeroKev 1:fe23126b0389 108 return (motor == 1)?1:0;
AeroKev 0:d38eb4622914 109 else
AeroKev 1:fe23126b0389 110 return (motor == 1)?0:1;
AeroKev 0:d38eb4622914 111 }
AeroKev 0:d38eb4622914 112
AeroKev 0:d38eb4622914 113 void initialize()
AeroKev 0:d38eb4622914 114 {
AeroKev 0:d38eb4622914 115 pc.printf("Initializing...\r\n");
AeroKev 0:d38eb4622914 116
AeroKev 0:d38eb4622914 117 // Set the shutup and start buttons
AeroKev 0:d38eb4622914 118 stop.mode(PullUp);
AeroKev 0:d38eb4622914 119 stop.attach_deasserted(&stopMotors);
AeroKev 0:d38eb4622914 120 stop.setSampleFrequency();
AeroKev 0:d38eb4622914 121 start.mode(PullUp);
AeroKev 0:d38eb4622914 122 start.attach_deasserted(&startMotors);
AeroKev 0:d38eb4622914 123 start.setSampleFrequency();
AeroKev 0:d38eb4622914 124
AeroKev 0:d38eb4622914 125 pc.printf("Buttons done\r\n");
AeroKev 0:d38eb4622914 126
AeroKev 0:d38eb4622914 127 // Set proper baudrate
AeroKev 0:d38eb4622914 128 // pc.baud(115200);
AeroKev 0:d38eb4622914 129
AeroKev 0:d38eb4622914 130 // Reset encoders
AeroKev 0:d38eb4622914 131 enc1.reset();
AeroKev 1:fe23126b0389 132 enc2.reset();
AeroKev 0:d38eb4622914 133 pc.printf("Encoders reset\r\n");
AeroKev 0:d38eb4622914 134
AeroKev 0:d38eb4622914 135 // Start tickers
AeroKev 0:d38eb4622914 136 potTicker.attach(&readPot, tickRate);
AeroKev 0:d38eb4622914 137 motorTicker.attach(&getMotorRotation, tickRate);
AeroKev 0:d38eb4622914 138 graphTicker.attach(&sendGraph, graphTickRate);
AeroKev 0:d38eb4622914 139 pc.printf("Tickers attached\r\n");
AeroKev 0:d38eb4622914 140
AeroKev 0:d38eb4622914 141 pc.printf("Initialized\r\n");
AeroKev 0:d38eb4622914 142 }
dolcaer 4:a722452d8fd1 143 /*
AeroKev 0:d38eb4622914 144 int main()
AeroKev 0:d38eb4622914 145 {
AeroKev 0:d38eb4622914 146 pc.printf("Started\r\n");
AeroKev 0:d38eb4622914 147 initialize();
AeroKev 0:d38eb4622914 148
AeroKev 0:d38eb4622914 149 while (true) {
AeroKev 0:d38eb4622914 150 if (shutup) {
AeroKev 0:d38eb4622914 151 pwm1 = 0;
AeroKev 0:d38eb4622914 152 pwm2 = 0;
AeroKev 0:d38eb4622914 153 } else {
AeroKev 0:d38eb4622914 154 if (go_pot) {
AeroKev 0:d38eb4622914 155 desiredRotation1 = getPotRad(pot1);
AeroKev 1:fe23126b0389 156 desiredRotation2 = getPotRad(pot2);
AeroKev 0:d38eb4622914 157
AeroKev 0:d38eb4622914 158 go_pot = false;
AeroKev 0:d38eb4622914 159 }
AeroKev 0:d38eb4622914 160
AeroKev 0:d38eb4622914 161 if (go_motor) {
AeroKev 0:d38eb4622914 162 currentRotation1 = toRadians(enc1.getPulses());
AeroKev 1:fe23126b0389 163 currentRotation2 = toRadians(enc2.getPulses());
AeroKev 0:d38eb4622914 164
AeroKev 1:fe23126b0389 165 pc.printf("PULSE: %i | CUR ROT: %f | ERR: %f | DES ROT: %f\r\n",enc2.getPulses(),currentRotation2, error2,desiredRotation2);
AeroKev 0:d38eb4622914 166
AeroKev 0:d38eb4622914 167 double previous_error1 = error1;
AeroKev 1:fe23126b0389 168 double previous_error2 = error2;
AeroKev 0:d38eb4622914 169
AeroKev 0:d38eb4622914 170 error1 = desiredRotation1 - currentRotation1;
AeroKev 1:fe23126b0389 171 error2 = desiredRotation2 - currentRotation2;
AeroKev 0:d38eb4622914 172 // P control
AeroKev 0:d38eb4622914 173 // double control1 = p_control(error1, motor1_kp);
AeroKev 0:d38eb4622914 174 // double control2 = p_control(error2, motor2_kp);
AeroKev 0:d38eb4622914 175
AeroKev 0:d38eb4622914 176 // PI control
AeroKev 0:d38eb4622914 177 //double control1 = pi_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral);
AeroKev 0:d38eb4622914 178 // double control2 = pi_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral);
AeroKev 0:d38eb4622914 179
AeroKev 0:d38eb4622914 180 // PID control
AeroKev 0:d38eb4622914 181 double control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
AeroKev 1:fe23126b0389 182 double control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
AeroKev 0:d38eb4622914 183
AeroKev 1:fe23126b0389 184 int d1 = getPDirection(control1,1);
AeroKev 1:fe23126b0389 185 int d2 = getPDirection(control2,2);
AeroKev 0:d38eb4622914 186 float speed1 = fabs(control1);
AeroKev 1:fe23126b0389 187 float speed2 = fabs(control2);
AeroKev 0:d38eb4622914 188
AeroKev 0:d38eb4622914 189 if (speed1 < 0.1f) speed1 = 0.0f;
AeroKev 1:fe23126b0389 190 if (speed2 < 0.1f) speed2 = 0.0f;
AeroKev 0:d38eb4622914 191 dir1 = d1;
AeroKev 1:fe23126b0389 192 dir2 = d2;
AeroKev 0:d38eb4622914 193 pwm1 = speed1;
AeroKev 1:fe23126b0389 194 pwm2 = speed2;
AeroKev 1:fe23126b0389 195 pc.printf("SPEED: %f | DIR: %i\r\n",speed2,d2);
AeroKev 0:d38eb4622914 196
AeroKev 0:d38eb4622914 197 go_motor = false;
AeroKev 0:d38eb4622914 198 }
dolcaer 4:a722452d8fd1 199 */
AeroKev 1:fe23126b0389 200 /*if (go_graph) {
AeroKev 0:d38eb4622914 201 pc.printf("Trying to send graph\r\n");
AeroKev 0:d38eb4622914 202 pc.printf("Desired Rotation: %f %f\r\n", desiredRotation1, desiredRotation2);
AeroKev 0:d38eb4622914 203 pc.printf("Rotation: %f %f\r\n", currentRotation1, desiredRotation2);
AeroKev 0:d38eb4622914 204 grapher.set(0, desiredRotation1);
AeroKev 0:d38eb4622914 205 grapher.set(1, currentRotation1);
AeroKev 0:d38eb4622914 206 grapher.set(2, desiredRotation2);
AeroKev 0:d38eb4622914 207 grapher.set(3, currentRotation2);
AeroKev 0:d38eb4622914 208 grapher.send();
AeroKev 0:d38eb4622914 209 go_graph = false;
AeroKev 1:fe23126b0389 210 }*/
dolcaer 4:a722452d8fd1 211 // }
dolcaer 4:a722452d8fd1 212 // }
dolcaer 4:a722452d8fd1 213 //}