Module 9 Super Team / PIDController1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of PIDController by Kevin Hetterscheid

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
Child:
5:937b2f34a1ca
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 //}