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@9:d04d028ccfe8, 2015-10-20 (annotated)

Committer:
AeroKev
Date:
Tue Oct 20 14:30:35 2015 +0000
Revision:
9:d04d028ccfe8
Parent:
8:ef6b758b73da
Child:
10:f05bd773b66c
a

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 9:d04d028ccfe8 8 #include "compute.h"
AeroKev 0:d38eb4622914 9
AeroKev 0:d38eb4622914 10 Serial pc(USBTX, USBRX);
AeroKev 0:d38eb4622914 11
AeroKev 5:937b2f34a1ca 12 PinName m1_enc_a = D12;
AeroKev 5:937b2f34a1ca 13 PinName m1_enc_b = D13;
AeroKev 5:937b2f34a1ca 14 PinName m1_pwm = D6;
AeroKev 5:937b2f34a1ca 15 PinName m1_dir = D7;
AeroKev 5:937b2f34a1ca 16
AeroKev 5:937b2f34a1ca 17 PinName m2_enc_a = D11;
AeroKev 5:937b2f34a1ca 18 PinName m2_enc_b = D10;
AeroKev 5:937b2f34a1ca 19 PinName m2_pwm = D5;
AeroKev 5:937b2f34a1ca 20 PinName m2_dir = D4;
AeroKev 5:937b2f34a1ca 21
AeroKev 5:937b2f34a1ca 22 PinName m2_pot = A0;
AeroKev 5:937b2f34a1ca 23 PinName m1_pot = A1;
AeroKev 5:937b2f34a1ca 24
AeroKev 5:937b2f34a1ca 25 const double pid_upper_limit = 1, pid_lower_limit = 0;
AeroKev 5:937b2f34a1ca 26
AeroKev 5:937b2f34a1ca 27 const double motor1_kp = 0.75f, motor1_ki = 0.001f, motor1_kd = 0.005f;
AeroKev 5:937b2f34a1ca 28 const double motor2_kp = 0.75f, motor2_ki = 0.001f, motor2_kd = 0.005f;
AeroKev 5:937b2f34a1ca 29
AeroKev 5:937b2f34a1ca 30 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 31 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 32
AeroKev 5:937b2f34a1ca 33 int sigPerRev = 4192;
AeroKev 5:937b2f34a1ca 34 float tickRate = 0.01f;
AeroKev 5:937b2f34a1ca 35 float graphTickRate = 0.01f;
AeroKev 5:937b2f34a1ca 36
AeroKev 9:d04d028ccfe8 37 double offsetA;
AeroKev 9:d04d028ccfe8 38 double offsetB;
AeroKev 9:d04d028ccfe8 39
AeroKev 5:937b2f34a1ca 40 float toRadians(int pulses);
AeroKev 5:937b2f34a1ca 41
AeroKev 0:d38eb4622914 42 AnalogIn pot1(m1_pot);
AeroKev 0:d38eb4622914 43 AnalogIn pot2(m2_pot);
AeroKev 0:d38eb4622914 44
AeroKev 0:d38eb4622914 45 // PWM Speed Control：
AeroKev 0:d38eb4622914 46 DigitalOut dir1(m1_dir);
AeroKev 0:d38eb4622914 47 PwmOut pwm1(m1_pwm);
AeroKev 0:d38eb4622914 48 DigitalOut dir2(m2_dir);
AeroKev 0:d38eb4622914 49 PwmOut pwm2(m2_pwm);
AeroKev 0:d38eb4622914 50
AeroKev 0:d38eb4622914 51 QEI enc1(m1_enc_a, m1_enc_b, NC, 1);
AeroKev 0:d38eb4622914 52 QEI enc2(m2_enc_a, m2_enc_b, NC, 1);
AeroKev 0:d38eb4622914 53
AeroKev 0:d38eb4622914 54 Ticker potTicker;
AeroKev 0:d38eb4622914 55 Ticker motorTicker;
AeroKev 0:d38eb4622914 56
AeroKev 0:d38eb4622914 57 Ticker graphTicker;
AeroKev 1:fe23126b0389 58 //HIDScope grapher(4);
AeroKev 0:d38eb4622914 59
AeroKev 0:d38eb4622914 60 float currentRotation1 = 0, currentRotation2 = 0;
AeroKev 0:d38eb4622914 61 float desiredRotation1 = 0, desiredRotation2 = 0;
AeroKev 0:d38eb4622914 62 double error1 = 0, error2 = 0;
AeroKev 0:d38eb4622914 63
AeroKev 0:d38eb4622914 64 double m1_error_integral = 0, m2_error_integral = 0;
AeroKev 0:d38eb4622914 65 double m1_error_derivative = 0, m2_error_derivative = 0;
AeroKev 0:d38eb4622914 66 biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
AeroKev 0:d38eb4622914 67 biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);
AeroKev 0:d38eb4622914 68
AeroKev 0:d38eb4622914 69 bool shutup = true;
AeroKev 0:d38eb4622914 70 bool go_pot = false;
AeroKev 0:d38eb4622914 71 bool go_motor = false;
AeroKev 0:d38eb4622914 72 bool go_graph = false;
AeroKev 0:d38eb4622914 73
AeroKev 0:d38eb4622914 74 float getPotRad(AnalogIn pot)
AeroKev 0:d38eb4622914 75 {
AeroKev 0:d38eb4622914 76 return pot.read() * 4.0f - 2.0f;
AeroKev 0:d38eb4622914 77 }
AeroKev 0:d38eb4622914 78
AeroKev 0:d38eb4622914 79 float toRadians(int pulses)
AeroKev 0:d38eb4622914 80 {
AeroKev 0:d38eb4622914 81 int remaining = pulses;// % sigPerRev;
AeroKev 0:d38eb4622914 82 float percent = (float) remaining / (float) sigPerRev;
AeroKev 0:d38eb4622914 83 return percent * 2.0f;
AeroKev 0:d38eb4622914 84 }
AeroKev 0:d38eb4622914 85
AeroKev 0:d38eb4622914 86 void readPot()
AeroKev 0:d38eb4622914 87 {
AeroKev 0:d38eb4622914 88 go_pot = true;
AeroKev 0:d38eb4622914 89 }
AeroKev 0:d38eb4622914 90
AeroKev 0:d38eb4622914 91 void getMotorRotation()
AeroKev 0:d38eb4622914 92 {
AeroKev 0:d38eb4622914 93 go_motor = true;
AeroKev 0:d38eb4622914 94 }
AeroKev 0:d38eb4622914 95
AeroKev 0:d38eb4622914 96 void sendGraph()
AeroKev 0:d38eb4622914 97 {
AeroKev 0:d38eb4622914 98 go_graph = true;
AeroKev 0:d38eb4622914 99 }
AeroKev 0:d38eb4622914 100
AeroKev 0:d38eb4622914 101 double p_control(double error, double kp)
AeroKev 0:d38eb4622914 102 {
AeroKev 0:d38eb4622914 103 return kp * error;
AeroKev 0:d38eb4622914 104 }
AeroKev 0:d38eb4622914 105
AeroKev 0:d38eb4622914 106 double pi_control(double error, double kp, double ki, double ts, double &error_integral)
AeroKev 0:d38eb4622914 107 {
AeroKev 0:d38eb4622914 108 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 109 double result = kp * error + ki * error_integral;
AeroKev 0:d38eb4622914 110 return result;
AeroKev 0:d38eb4622914 111 }
AeroKev 0:d38eb4622914 112
AeroKev 7:0fb420b3434f 113 double pid_control(double error, double kp, double ki, double ts, double &error_integral,
AeroKev 7:0fb420b3434f 114 double kd, double previous_error, double &error_derivative, biquadFilter filter)
AeroKev 0:d38eb4622914 115 {
AeroKev 0:d38eb4622914 116 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 117 error_derivative = (error - previous_error) / ts;
AeroKev 0:d38eb4622914 118 // error_derivative = filter.step(error_derivative);
AeroKev 7:0fb420b3434f 119
AeroKev 0:d38eb4622914 120 double result = kp * error + ki * error_integral + kd * error_derivative;
AeroKev 0:d38eb4622914 121 return result;
AeroKev 0:d38eb4622914 122 }
AeroKev 0:d38eb4622914 123
AeroKev 1:fe23126b0389 124 int getPDirection(double control, int motor)
AeroKev 0:d38eb4622914 125 {
AeroKev 0:d38eb4622914 126 if (control >= 0)
AeroKev 1:fe23126b0389 127 return (motor == 1)?1:0;
AeroKev 0:d38eb4622914 128 else
AeroKev 1:fe23126b0389 129 return (motor == 1)?0:1;
AeroKev 0:d38eb4622914 130 }
AeroKev 0:d38eb4622914 131
AeroKev 9:d04d028ccfe8 132 void PID_init(double angleA, double angleB)
AeroKev 0:d38eb4622914 133 {
AeroKev 0:d38eb4622914 134 pc.printf("Initializing...\r\n");
AeroKev 0:d38eb4622914 135
AeroKev 0:d38eb4622914 136 // Set proper baudrate
AeroKev 0:d38eb4622914 137 // pc.baud(115200);
AeroKev 0:d38eb4622914 138
AeroKev 0:d38eb4622914 139 // Reset encoders
AeroKev 0:d38eb4622914 140 enc1.reset();
AeroKev 1:fe23126b0389 141 enc2.reset();
AeroKev 0:d38eb4622914 142 pc.printf("Encoders reset\r\n");
AeroKev 0:d38eb4622914 143
AeroKev 7:0fb420b3434f 144 go_motor = true;
AeroKev 9:d04d028ccfe8 145 shutup = false;
AeroKev 7:0fb420b3434f 146
AeroKev 0:d38eb4622914 147 // Start tickers
AeroKev 0:d38eb4622914 148 potTicker.attach(&readPot, tickRate);
AeroKev 0:d38eb4622914 149 motorTicker.attach(&getMotorRotation, tickRate);
AeroKev 0:d38eb4622914 150 graphTicker.attach(&sendGraph, graphTickRate);
AeroKev 0:d38eb4622914 151 pc.printf("Tickers attached\r\n");
AeroKev 9:d04d028ccfe8 152
AeroKev 9:d04d028ccfe8 153 bool cont = true;
AeroKev 9:d04d028ccfe8 154 double a =0.0;
AeroKev 9:d04d028ccfe8 155 double b = 0.0;
AeroKev 9:d04d028ccfe8 156 double prev = -1.0;
AeroKev 9:d04d028ccfe8 157 while(cont) {
AeroKev 9:d04d028ccfe8 158 a -= 0.1;
AeroKev 9:d04d028ccfe8 159 pc.printf("%f\r\n",a);
AeroKev 9:d04d028ccfe8 160 moveOneMotor(1,a);
AeroKev 9:d04d028ccfe8 161 wait(1);
AeroKev 9:d04d028ccfe8 162 double temp = toRadians(enc1.getPulses());
AeroKev 9:d04d028ccfe8 163 pc.printf("PREV | TEMP = %f | %f\r\n",prev,temp);
AeroKev 9:d04d028ccfe8 164 if(fabs(prev-temp) < 0.005) {
AeroKev 9:d04d028ccfe8 165 pwm1 = 0;
AeroKev 9:d04d028ccfe8 166 cont = false;
AeroKev 9:d04d028ccfe8 167 offsetA = deg2rad(200)-temp;
AeroKev 9:d04d028ccfe8 168 }
AeroKev 9:d04d028ccfe8 169 else
AeroKev 9:d04d028ccfe8 170 prev = temp;
AeroKev 9:d04d028ccfe8 171 }
AeroKev 9:d04d028ccfe8 172
AeroKev 9:d04d028ccfe8 173 pc.printf("\r\nOFFSET: %f | OFFSET: %d\r\n",offsetA, rad2deg(offsetA));
AeroKev 9:d04d028ccfe8 174 pc.printf("\r\nTO: %f | TO: %d",deg2rad(135)-offsetA, rad2deg(deg2rad(135)-offsetA));
AeroKev 9:d04d028ccfe8 175
AeroKev 9:d04d028ccfe8 176 moveOneMotor(1,deg2rad(135)-offsetA);
AeroKev 9:d04d028ccfe8 177 wait(3);
AeroKev 9:d04d028ccfe8 178
AeroKev 9:d04d028ccfe8 179 cont = true;
AeroKev 9:d04d028ccfe8 180 while(cont) {
AeroKev 9:d04d028ccfe8 181 pwm1 = 0;
AeroKev 9:d04d028ccfe8 182 b -= 0.1;
AeroKev 9:d04d028ccfe8 183 moveOneMotor(2,b);
AeroKev 9:d04d028ccfe8 184 wait(1);
AeroKev 9:d04d028ccfe8 185 double temp = toRadians(enc2.getPulses());
AeroKev 9:d04d028ccfe8 186 if(fabs(prev-temp) < 0.005) {
AeroKev 9:d04d028ccfe8 187 pwm2 = 0;
AeroKev 9:d04d028ccfe8 188 cont = false;
AeroKev 9:d04d028ccfe8 189 offsetB = deg2rad(-45)-temp;
AeroKev 9:d04d028ccfe8 190 }
AeroKev 9:d04d028ccfe8 191 else
AeroKev 9:d04d028ccfe8 192 prev = temp;
AeroKev 9:d04d028ccfe8 193 }
AeroKev 9:d04d028ccfe8 194
AeroKev 9:d04d028ccfe8 195 //rotate(offsetA+angleA,offsetB+angleB);
AeroKev 0:d38eb4622914 196
AeroKev 0:d38eb4622914 197 pc.printf("Initialized\r\n");
AeroKev 0:d38eb4622914 198 }
AeroKev 0:d38eb4622914 199
AeroKev 7:0fb420b3434f 200 void getCurrent(double& a, double& b)
AeroKev 7:0fb420b3434f 201 {
AeroKev 6:48bb8aa4888b 202 a = toRadians(enc1.getPulses());
AeroKev 7:0fb420b3434f 203 b = toRadians(enc2.getPulses());
AeroKev 6:48bb8aa4888b 204 }
AeroKev 6:48bb8aa4888b 205
AeroKev 9:d04d028ccfe8 206 void rotate(double a, double b)
AeroKev 7:0fb420b3434f 207 {
AeroKev 7:0fb420b3434f 208 if (shutup) {
AeroKev 7:0fb420b3434f 209 pwm1 = 0;
AeroKev 7:0fb420b3434f 210 pwm2 = 0;
AeroKev 7:0fb420b3434f 211 } else {
AeroKev 7:0fb420b3434f 212 desiredRotation1 = a;
AeroKev 7:0fb420b3434f 213 desiredRotation2 = b;
AeroKev 0:d38eb4622914 214
AeroKev 7:0fb420b3434f 215 currentRotation1 = toRadians(enc1.getPulses());
AeroKev 7:0fb420b3434f 216 currentRotation2 = toRadians(enc2.getPulses());
AeroKev 0:d38eb4622914 217
AeroKev 7:0fb420b3434f 218 double previous_error1 = error1;
AeroKev 7:0fb420b3434f 219 double previous_error2 = error2;
AeroKev 0:d38eb4622914 220
AeroKev 7:0fb420b3434f 221 error1 = desiredRotation1 - currentRotation1;
AeroKev 7:0fb420b3434f 222 error2 = desiredRotation2 - currentRotation2;
AeroKev 7:0fb420b3434f 223
AeroKev 7:0fb420b3434f 224 // PID control
AeroKev 7:0fb420b3434f 225 double control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
AeroKev 7:0fb420b3434f 226 double control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
AeroKev 9:d04d028ccfe8 227
AeroKev 9:d04d028ccfe8 228 pc.printf("CUR ROT 1: %f\r\nCUR ROT 2: %f\r\n",control1, control2);
AeroKev 0:d38eb4622914 229
AeroKev 7:0fb420b3434f 230 int d1 = getPDirection(control1,1);
AeroKev 7:0fb420b3434f 231 int d2 = getPDirection(control2,2);
AeroKev 7:0fb420b3434f 232 float speed1 = fabs(control1);
AeroKev 7:0fb420b3434f 233 float speed2 = fabs(control2);
AeroKev 0:d38eb4622914 234
AeroKev 7:0fb420b3434f 235 if (speed1 < 0.1f) speed1 = 0.0f;
AeroKev 7:0fb420b3434f 236 if (speed2 < 0.1f) speed2 = 0.0f;
AeroKev 7:0fb420b3434f 237 dir1 = d1;
AeroKev 7:0fb420b3434f 238 dir2 = d2;
AeroKev 7:0fb420b3434f 239 pwm1 = speed1;
AeroKev 7:0fb420b3434f 240 pwm2 = speed2;
AeroKev 5:937b2f34a1ca 241 }
AeroKev 9:d04d028ccfe8 242 }
AeroKev 9:d04d028ccfe8 243
AeroKev 9:d04d028ccfe8 244 void moveOneMotor(int num, double angle) {
AeroKev 9:d04d028ccfe8 245 if(num == 1) {
AeroKev 9:d04d028ccfe8 246 desiredRotation1 = angle;
AeroKev 9:d04d028ccfe8 247 currentRotation1 = toRadians(enc1.getPulses());
AeroKev 9:d04d028ccfe8 248 double previous_error1 = error1;
AeroKev 9:d04d028ccfe8 249 error1 = desiredRotation1 - currentRotation1;
AeroKev 9:d04d028ccfe8 250
AeroKev 9:d04d028ccfe8 251 double control = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
AeroKev 9:d04d028ccfe8 252
AeroKev 9:d04d028ccfe8 253 int d = getPDirection(control,1);
AeroKev 9:d04d028ccfe8 254 float speed = fabs(control);
AeroKev 9:d04d028ccfe8 255 pc.printf("SPEED: %f\r\n",speed);
AeroKev 9:d04d028ccfe8 256 if (speed <= 0.1f) speed = 0.0f;
AeroKev 9:d04d028ccfe8 257 if(speed > 0.35f) speed = 0.35f;
AeroKev 9:d04d028ccfe8 258 speed = 0.3;
AeroKev 9:d04d028ccfe8 259
AeroKev 9:d04d028ccfe8 260 dir1 = d;
AeroKev 9:d04d028ccfe8 261 pwm1 = speed;
AeroKev 9:d04d028ccfe8 262
AeroKev 9:d04d028ccfe8 263 pc.printf("DIR 1: %f | PWM 2: %f\r\n",d, speed);
AeroKev 9:d04d028ccfe8 264 }
AeroKev 9:d04d028ccfe8 265 else {
AeroKev 9:d04d028ccfe8 266 desiredRotation2 = angle;
AeroKev 9:d04d028ccfe8 267 currentRotation2 = toRadians(enc2.getPulses());
AeroKev 9:d04d028ccfe8 268 double previous_error2 = error2;
AeroKev 9:d04d028ccfe8 269 error2 = desiredRotation2 - currentRotation2;
AeroKev 9:d04d028ccfe8 270
AeroKev 9:d04d028ccfe8 271 double control = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
AeroKev 9:d04d028ccfe8 272
AeroKev 9:d04d028ccfe8 273 int d = getPDirection(control,1);
AeroKev 9:d04d028ccfe8 274 float speed = fabs(control);
AeroKev 9:d04d028ccfe8 275 if (speed <= 0.1f) speed = 0.0f;
AeroKev 9:d04d028ccfe8 276 if(speed > 0.35f) speed = 0.35f;
AeroKev 9:d04d028ccfe8 277 speed = 0.3;
AeroKev 9:d04d028ccfe8 278
AeroKev 9:d04d028ccfe8 279 dir2 = d;
AeroKev 9:d04d028ccfe8 280 pwm2 = speed;
AeroKev 9:d04d028ccfe8 281
AeroKev 9:d04d028ccfe8 282 pc.printf("DIR 2: %f | PWM 2: %f\r\n",d, speed);
AeroKev 9:d04d028ccfe8 283 }
AeroKev 5:937b2f34a1ca 284 }