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@11:7f41fac17c48, 2015-10-22 (annotated)

Committer:
AeroKev
Date:
Thu Oct 22 14:44:18 2015 +0000
Revision:
11:7f41fac17c48
Parent:
10:f05bd773b66c
Child:
12:84f2c63f9b98
da

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