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@6:48bb8aa4888b, 2015-10-20 (annotated)

Committer:
AeroKev
Date:
Tue Oct 20 09:56:40 2015 +0000
Revision:
6:48bb8aa4888b
Parent:
5:937b2f34a1ca
Child:
7:0fb420b3434f
Changed some stuff

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