Kevin Hetterscheid / PIDController

PID Controller

Dependencies:   HIDScope MODSERIAL PinDetect QEI biquadFilter

main.cpp@0:d38eb4622914, 2015-10-14 (annotated)

Committer:
AeroKev
Date:
Wed Oct 14 13:47:18 2015 +0000
Revision:
0:d38eb4622914
Child:
1:fe23126b0389
Works;

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 0:d38eb4622914 6 #include "main.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 0:d38eb4622914 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 double result = kp * error;
AeroKev 0:d38eb4622914 85 return kp * error;
AeroKev 0:d38eb4622914 86 }
AeroKev 0:d38eb4622914 87
AeroKev 0:d38eb4622914 88 double pi_control(double error, double kp, double ki, double ts, double &error_integral)
AeroKev 0:d38eb4622914 89 {
AeroKev 0:d38eb4622914 90 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 91 double result = kp * error + ki * error_integral;
AeroKev 0:d38eb4622914 92 return result;
AeroKev 0:d38eb4622914 93 }
AeroKev 0:d38eb4622914 94
AeroKev 0:d38eb4622914 95 double pid_control(double error, double kp, double ki, double ts, double &error_integral,
AeroKev 0:d38eb4622914 96 double kd, double previous_error, double &error_derivative, biquadFilter filter)
AeroKev 0:d38eb4622914 97 {
AeroKev 0:d38eb4622914 98 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 99 error_derivative = (error - previous_error) / ts;
AeroKev 0:d38eb4622914 100 // error_derivative = filter.step(error_derivative);
AeroKev 0:d38eb4622914 101
AeroKev 0:d38eb4622914 102 double result = kp * error + ki * error_integral + kd * error_derivative;
AeroKev 0:d38eb4622914 103 return result;
AeroKev 0:d38eb4622914 104 }
AeroKev 0:d38eb4622914 105
AeroKev 0:d38eb4622914 106 int getPDirection(double control)
AeroKev 0:d38eb4622914 107 {
AeroKev 0:d38eb4622914 108 if (control >= 0)
AeroKev 0:d38eb4622914 109 return 1;
AeroKev 0:d38eb4622914 110 else
AeroKev 0:d38eb4622914 111 return 0;
AeroKev 0:d38eb4622914 112 }
AeroKev 0:d38eb4622914 113
AeroKev 0:d38eb4622914 114 void initialize()
AeroKev 0:d38eb4622914 115 {
AeroKev 0:d38eb4622914 116 pc.printf("Initializing...\r\n");
AeroKev 0:d38eb4622914 117
AeroKev 0:d38eb4622914 118 // Set the shutup and start buttons
AeroKev 0:d38eb4622914 119 stop.mode(PullUp);
AeroKev 0:d38eb4622914 120 stop.attach_deasserted(&stopMotors);
AeroKev 0:d38eb4622914 121 stop.setSampleFrequency();
AeroKev 0:d38eb4622914 122 start.mode(PullUp);
AeroKev 0:d38eb4622914 123 start.attach_deasserted(&startMotors);
AeroKev 0:d38eb4622914 124 start.setSampleFrequency();
AeroKev 0:d38eb4622914 125
AeroKev 0:d38eb4622914 126 pc.printf("Buttons done\r\n");
AeroKev 0:d38eb4622914 127
AeroKev 0:d38eb4622914 128 // Set proper baudrate
AeroKev 0:d38eb4622914 129 // pc.baud(115200);
AeroKev 0:d38eb4622914 130
AeroKev 0:d38eb4622914 131 // Reset encoders
AeroKev 0:d38eb4622914 132 enc1.reset();
AeroKev 0:d38eb4622914 133 // enc2.reset();
AeroKev 0:d38eb4622914 134 pc.printf("Encoders reset\r\n");
AeroKev 0:d38eb4622914 135
AeroKev 0:d38eb4622914 136 // Start tickers
AeroKev 0:d38eb4622914 137 potTicker.attach(&readPot, tickRate);
AeroKev 0:d38eb4622914 138 motorTicker.attach(&getMotorRotation, tickRate);
AeroKev 0:d38eb4622914 139 graphTicker.attach(&sendGraph, graphTickRate);
AeroKev 0:d38eb4622914 140 pc.printf("Tickers attached\r\n");
AeroKev 0:d38eb4622914 141
AeroKev 0:d38eb4622914 142 pc.printf("Initialized\r\n");
AeroKev 0:d38eb4622914 143 }
AeroKev 0:d38eb4622914 144
AeroKev 0:d38eb4622914 145 int main()
AeroKev 0:d38eb4622914 146 {
AeroKev 0:d38eb4622914 147 pc.printf("Started\r\n");
AeroKev 0:d38eb4622914 148 initialize();
AeroKev 0:d38eb4622914 149
AeroKev 0:d38eb4622914 150 while (true) {
AeroKev 0:d38eb4622914 151 if (shutup) {
AeroKev 0:d38eb4622914 152 pwm1 = 0;
AeroKev 0:d38eb4622914 153 pwm2 = 0;
AeroKev 0:d38eb4622914 154 } else {
AeroKev 0:d38eb4622914 155 if (go_pot) {
AeroKev 0:d38eb4622914 156 desiredRotation1 = getPotRad(pot1);
AeroKev 0:d38eb4622914 157 //desiredRotation2 = getPotRad(pot2);
AeroKev 0:d38eb4622914 158
AeroKev 0:d38eb4622914 159 go_pot = false;
AeroKev 0:d38eb4622914 160 }
AeroKev 0:d38eb4622914 161
AeroKev 0:d38eb4622914 162 if (go_motor) {
AeroKev 0:d38eb4622914 163 currentRotation1 = toRadians(enc1.getPulses());
AeroKev 0:d38eb4622914 164 //currentRotation2 = toRadians(enc2.getPulses());
AeroKev 0:d38eb4622914 165
AeroKev 0:d38eb4622914 166 pc.printf("PULSE: %i | CUR ROT: %f | ERR: %f | DES ROT: %f\r\n",enc1.getPulses(),currentRotation1, error1,desiredRotation1);
AeroKev 0:d38eb4622914 167
AeroKev 0:d38eb4622914 168 double previous_error1 = error1;
AeroKev 0:d38eb4622914 169 //double previous_error2 = error2;
AeroKev 0:d38eb4622914 170
AeroKev 0:d38eb4622914 171 error1 = desiredRotation1 - currentRotation1;
AeroKev 0:d38eb4622914 172 // error2 = desiredRotation2 - currentRotation2;
AeroKev 0:d38eb4622914 173 // P control
AeroKev 0:d38eb4622914 174 // double control1 = p_control(error1, motor1_kp);
AeroKev 0:d38eb4622914 175 // double control2 = p_control(error2, motor2_kp);
AeroKev 0:d38eb4622914 176
AeroKev 0:d38eb4622914 177 // PI control
AeroKev 0:d38eb4622914 178 //double control1 = pi_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral);
AeroKev 0:d38eb4622914 179 // double control2 = pi_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral);
AeroKev 0:d38eb4622914 180
AeroKev 0:d38eb4622914 181 // PID control
AeroKev 0:d38eb4622914 182 double control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
AeroKev 0:d38eb4622914 183 //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 184
AeroKev 0:d38eb4622914 185 int d1 = getPDirection(control1);
AeroKev 0:d38eb4622914 186 //int d2 = getPDirection(control2);
AeroKev 0:d38eb4622914 187 float speed1 = fabs(control1);
AeroKev 0:d38eb4622914 188 // float speed2 = fabs(control2);
AeroKev 0:d38eb4622914 189
AeroKev 0:d38eb4622914 190 if (speed1 < 0.1f) speed1 = 0.0f;
AeroKev 0:d38eb4622914 191 // if (speed2 < 0.1f) speed2 = 0.0f;
AeroKev 0:d38eb4622914 192 dir1 = d1;
AeroKev 0:d38eb4622914 193 // dir2 = d2;
AeroKev 0:d38eb4622914 194 pwm1 = speed1;
AeroKev 0:d38eb4622914 195 //pwm2 = speed2;
AeroKev 0:d38eb4622914 196 pc.printf("SPEED: %f | DIR: %i\r\n",speed1,d1);
AeroKev 0:d38eb4622914 197
AeroKev 0:d38eb4622914 198 go_motor = false;
AeroKev 0:d38eb4622914 199 }
AeroKev 0:d38eb4622914 200
AeroKev 0:d38eb4622914 201 if (go_graph) {
AeroKev 0:d38eb4622914 202 pc.printf("Trying to send graph\r\n");
AeroKev 0:d38eb4622914 203 pc.printf("Desired Rotation: %f %f\r\n", desiredRotation1, desiredRotation2);
AeroKev 0:d38eb4622914 204 pc.printf("Rotation: %f %f\r\n", currentRotation1, desiredRotation2);
AeroKev 0:d38eb4622914 205 grapher.set(0, desiredRotation1);
AeroKev 0:d38eb4622914 206 grapher.set(1, currentRotation1);
AeroKev 0:d38eb4622914 207 grapher.set(2, desiredRotation2);
AeroKev 0:d38eb4622914 208 grapher.set(3, currentRotation2);
AeroKev 0:d38eb4622914 209 grapher.send();
AeroKev 0:d38eb4622914 210 go_graph = false;
AeroKev 0:d38eb4622914 211 }
AeroKev 0:d38eb4622914 212 }
AeroKev 0:d38eb4622914 213 }
AeroKev 0:d38eb4622914 214 }