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@17:7c54c7fcf9c5, 2015-10-29 (annotated)

Committer:
AeroKev
Date:
Thu Oct 29 17:09:55 2015 +0000
Revision:
17:7c54c7fcf9c5
Parent:
14:c5233d2b1286
Child:
18:12a542fa857e
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
dolcaer 14:c5233d2b1286 25 const double motor1_kp = 0.35f, motor1_ki = 0.005f, motor1_kd = 0.04f;
dolcaer 14:c5233d2b1286 26 const double motor2_kp = 0.35f, motor2_ki = 0.005f, motor2_kd = 0.04f;
AeroKev 5:937b2f34a1ca 27
dolcaer 14:c5233d2b1286 28 const double motor1_push_kp = 0.9f, motor1_push_ki = 0.009f, motor1_push_kd = 0.05f;
dolcaer 14:c5233d2b1286 29 const double motor2_push_kp = 0.9f, motor2_push_ki = 0.009f, motor2_push_kd = 0.05f;
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 12:84f2c63f9b98 38 double offsetA, offsetB;
AeroKev 5:937b2f34a1ca 39
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 float currentRotation1 = 0, currentRotation2 = 0;
AeroKev 0:d38eb4622914 53 float desiredRotation1 = 0, desiredRotation2 = 0;
AeroKev 0:d38eb4622914 54 double error1 = 0, error2 = 0;
AeroKev 0:d38eb4622914 55
AeroKev 0:d38eb4622914 56 double m1_error_integral = 0, m2_error_integral = 0;
AeroKev 0:d38eb4622914 57 double m1_error_derivative = 0, m2_error_derivative = 0;
AeroKev 0:d38eb4622914 58 biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
AeroKev 0:d38eb4622914 59 biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);
AeroKev 0:d38eb4622914 60
AeroKev 11:7f41fac17c48 61 bool pushing;
AeroKev 11:7f41fac17c48 62
AeroKev 12:84f2c63f9b98 63 /*
AeroKev 12:84f2c63f9b98 64 * Calculates the number of radians given a number of pulses of an encoder
AeroKev 12:84f2c63f9b98 65 * int pulses: The number of pulses to convert into radians
AeroKev 12:84f2c63f9b98 66 */
AeroKev 0:d38eb4622914 67 float toRadians(int pulses)
AeroKev 0:d38eb4622914 68 {
AeroKev 12:84f2c63f9b98 69 int remaining = pulses;
AeroKev 0:d38eb4622914 70 float percent = (float) remaining / (float) sigPerRev;
AeroKev 0:d38eb4622914 71 return percent * 2.0f;
AeroKev 0:d38eb4622914 72 }
AeroKev 0:d38eb4622914 73
AeroKev 12:84f2c63f9b98 74 /*
AeroKev 12:84f2c63f9b98 75 * Calculates the result of the PID Controller
AeroKev 12:84f2c63f9b98 76 */
AeroKev 7:0fb420b3434f 77 double pid_control(double error, double kp, double ki, double ts, double &error_integral,
AeroKev 7:0fb420b3434f 78 double kd, double previous_error, double &error_derivative, biquadFilter filter)
AeroKev 0:d38eb4622914 79 {
AeroKev 0:d38eb4622914 80 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 81 error_derivative = (error - previous_error) / ts;
AeroKev 0:d38eb4622914 82 // error_derivative = filter.step(error_derivative);
AeroKev 7:0fb420b3434f 83
AeroKev 0:d38eb4622914 84 double result = kp * error + ki * error_integral + kd * error_derivative;
AeroKev 0:d38eb4622914 85 return result;
AeroKev 0:d38eb4622914 86 }
AeroKev 0:d38eb4622914 87
AeroKev 12:84f2c63f9b98 88 /*
AeroKev 12:84f2c63f9b98 89 * Get the direction of the motor
AeroKev 12:84f2c63f9b98 90 * double control: The desired direction
AeroKev 12:84f2c63f9b98 91 * int motor: The ID of the motor [1 == motor 1 | 2 == motor 2]
AeroKev 12:84f2c63f9b98 92 */
AeroKev 1:fe23126b0389 93 int getPDirection(double control, int motor)
AeroKev 0:d38eb4622914 94 {
AeroKev 0:d38eb4622914 95 if (control >= 0)
AeroKev 1:fe23126b0389 96 return (motor == 1)?1:0;
AeroKev 0:d38eb4622914 97 else
AeroKev 1:fe23126b0389 98 return (motor == 1)?0:1;
AeroKev 0:d38eb4622914 99 }
AeroKev 0:d38eb4622914 100
AeroKev 12:84f2c63f9b98 101 /*
AeroKev 12:84f2c63f9b98 102 * The initializator of the PID Controller
AeroKev 12:84f2c63f9b98 103 */
AeroKev 12:84f2c63f9b98 104 void PID_init()
AeroKev 0:d38eb4622914 105 {
AeroKev 12:84f2c63f9b98 106 pc.printf("Initializing PID Controller...\r\n");
AeroKev 11:7f41fac17c48 107
AeroKev 12:84f2c63f9b98 108 /* Setting the pulse width modulation */
AeroKev 11:7f41fac17c48 109 pwm1.period_ms(0.01);
AeroKev 11:7f41fac17c48 110 pwm1.pulsewidth_ms(0.005);
AeroKev 11:7f41fac17c48 111 pwm2.period_ms(0.01);
AeroKev 11:7f41fac17c48 112 pwm2.pulsewidth_ms(0.005);
AeroKev 12:84f2c63f9b98 113
AeroKev 12:84f2c63f9b98 114 /* Resetting the encoders */
AeroKev 0:d38eb4622914 115 enc1.reset();
AeroKev 1:fe23126b0389 116 enc2.reset();
AeroKev 0:d38eb4622914 117 pc.printf("Encoders reset\r\n");
AeroKev 0:d38eb4622914 118
AeroKev 12:84f2c63f9b98 119 /*
AeroKev 12:84f2c63f9b98 120 * Calibrate the first motor
AeroKev 12:84f2c63f9b98 121 * Rotate it until it hits the base, then reset the encoder and calculate the offset.
AeroKev 12:84f2c63f9b98 122 */
AeroKev 9:d04d028ccfe8 123 bool cont = true;
dolcaer 14:c5233d2b1286 124 double prev = 0;
AeroKev 9:d04d028ccfe8 125 while(cont) {
AeroKev 10:f05bd773b66c 126 moveOneMotor(1,0);
AeroKev 11:7f41fac17c48 127 wait(.1);
AeroKev 9:d04d028ccfe8 128 double temp = toRadians(enc1.getPulses());
AeroKev 9:d04d028ccfe8 129 if(fabs(prev-temp) < 0.005) {
AeroKev 9:d04d028ccfe8 130 pwm1 = 0;
AeroKev 9:d04d028ccfe8 131 cont = false;
AeroKev 10:f05bd773b66c 132 } else
AeroKev 9:d04d028ccfe8 133 prev = temp;
AeroKev 9:d04d028ccfe8 134 }
AeroKev 10:f05bd773b66c 135 enc1.reset();
AeroKev 10:f05bd773b66c 136
AeroKev 12:84f2c63f9b98 137 /* Move the first motor to a better starting position */
AeroKev 10:f05bd773b66c 138 moveOneMotor(1,1);
AeroKev 11:7f41fac17c48 139 wait(1.1);
AeroKev 10:f05bd773b66c 140 pwm1 = 0;
AeroKev 10:f05bd773b66c 141
AeroKev 12:84f2c63f9b98 142 /*
AeroKev 12:84f2c63f9b98 143 * Calibrate the second motor
AeroKev 12:84f2c63f9b98 144 * Rotate it until it hits the base, then reset the encoder and calculate the offset.
AeroKev 12:84f2c63f9b98 145 */
AeroKev 9:d04d028ccfe8 146 cont = true;
AeroKev 9:d04d028ccfe8 147 while(cont) {
AeroKev 10:f05bd773b66c 148 moveOneMotor(2,0);
AeroKev 11:7f41fac17c48 149 wait(.1);
AeroKev 9:d04d028ccfe8 150 double temp = toRadians(enc2.getPulses());
AeroKev 9:d04d028ccfe8 151 if(fabs(prev-temp) < 0.005) {
AeroKev 9:d04d028ccfe8 152 pwm2 = 0;
AeroKev 9:d04d028ccfe8 153 cont = false;
AeroKev 10:f05bd773b66c 154 } else
AeroKev 9:d04d028ccfe8 155 prev = temp;
AeroKev 9:d04d028ccfe8 156 }
AeroKev 9:d04d028ccfe8 157
AeroKev 10:f05bd773b66c 158 enc2.reset();
AeroKev 10:f05bd773b66c 159
AeroKev 12:84f2c63f9b98 160 /* Move the first motor to a better starting position */
AeroKev 10:f05bd773b66c 161 moveOneMotor(2,1);
AeroKev 11:7f41fac17c48 162 wait(1);
AeroKev 10:f05bd773b66c 163 pwm2 = 0;
AeroKev 10:f05bd773b66c 164
AeroKev 12:84f2c63f9b98 165 /* Calculate both offsets */
AeroKev 10:f05bd773b66c 166 offsetA = deg2rad(243); // 243
AeroKev 10:f05bd773b66c 167 offsetB = deg2rad(-63); // -63
AeroKev 11:7f41fac17c48 168
AeroKev 12:84f2c63f9b98 169 /* Set the default desired rotation */
AeroKev 10:f05bd773b66c 170 desiredRotation1 = toRadians(enc1.getPulses());
AeroKev 10:f05bd773b66c 171 desiredRotation2 = toRadians(enc2.getPulses());
AeroKev 0:d38eb4622914 172 }
AeroKev 0:d38eb4622914 173
AeroKev 12:84f2c63f9b98 174 /*
AeroKev 12:84f2c63f9b98 175 * Gives the current rotation of the motors.
AeroKev 12:84f2c63f9b98 176 * double& a: The variable to store the rotation of motor 1
AeroKev 12:84f2c63f9b98 177 * double& b: The variable to store the rotation of motor 2
AeroKev 12:84f2c63f9b98 178 */
AeroKev 7:0fb420b3434f 179 void getCurrent(double& a, double& b)
AeroKev 7:0fb420b3434f 180 {
AeroKev 6:48bb8aa4888b 181 a = toRadians(enc1.getPulses());
AeroKev 7:0fb420b3434f 182 b = toRadians(enc2.getPulses());
AeroKev 6:48bb8aa4888b 183 }
AeroKev 6:48bb8aa4888b 184
AeroKev 12:84f2c63f9b98 185 /*
AeroKev 12:84f2c63f9b98 186 * Rotates the motors to certain rotations
AeroKev 12:84f2c63f9b98 187 * double a: The desired rotation of motor 1
AeroKev 12:84f2c63f9b98 188 * double b: The desired rotation of motor 2
AeroKev 12:84f2c63f9b98 189 */
AeroKev 9:d04d028ccfe8 190 void rotate(double a, double b)
AeroKev 7:0fb420b3434f 191 {
AeroKev 10:f05bd773b66c 192 desiredRotation1 = -(a - offsetA);
AeroKev 11:7f41fac17c48 193 desiredRotation2 = -(b - offsetB);
dolcaer 14:c5233d2b1286 194 pushing = false;
AeroKev 10:f05bd773b66c 195 }
AeroKev 7:0fb420b3434f 196
AeroKev 12:84f2c63f9b98 197 /*
AeroKev 12:84f2c63f9b98 198 * Rotates the motors to certain rotations, while using the PID Controller used for the pushing motion
AeroKev 12:84f2c63f9b98 199 * double a: The desired rotation of motor 1
AeroKev 12:84f2c63f9b98 200 * double b: The desired rotation of motor 2
AeroKev 12:84f2c63f9b98 201 */
AeroKev 11:7f41fac17c48 202 void push(double a, double b)
AeroKev 11:7f41fac17c48 203 {
AeroKev 11:7f41fac17c48 204 pushing = true;
AeroKev 11:7f41fac17c48 205 desiredRotation1 = -(a - offsetA);
AeroKev 11:7f41fac17c48 206 desiredRotation2 = -(b - offsetB);
AeroKev 11:7f41fac17c48 207 }
AeroKev 11:7f41fac17c48 208
AeroKev 12:84f2c63f9b98 209 /*
AeroKev 12:84f2c63f9b98 210 * Returns the offset of the motors calulated in the initialization phase.
AeroKev 12:84f2c63f9b98 211 * int a: The id of the motor [1 == motor 1 | 2 == motor 2]
AeroKev 12:84f2c63f9b98 212 */
AeroKev 11:7f41fac17c48 213 double getOffset(int a)
AeroKev 11:7f41fac17c48 214 {
AeroKev 10:f05bd773b66c 215 if(a == 1) return offsetA;
AeroKev 10:f05bd773b66c 216 else return offsetB;
AeroKev 10:f05bd773b66c 217 }
AeroKev 11:7f41fac17c48 218
AeroKev 12:84f2c63f9b98 219 /*
AeroKev 12:84f2c63f9b98 220 * Stops the motors.
AeroKev 12:84f2c63f9b98 221 */
AeroKev 11:7f41fac17c48 222 void PID_stop()
AeroKev 11:7f41fac17c48 223 {
AeroKev 10:f05bd773b66c 224 pwm1 = 0;
AeroKev 11:7f41fac17c48 225 pwm2 = 0;
AeroKev 9:d04d028ccfe8 226 }
AeroKev 9:d04d028ccfe8 227
AeroKev 12:84f2c63f9b98 228 /*
AeroKev 12:84f2c63f9b98 229 * Calculates the speed and direction of the motors using a PID Controller and the current rotation of the motors, given a desired rotation.
AeroKev 12:84f2c63f9b98 230 */
AeroKev 13:41d897396fb5 231 void moveTick(double maxSpeed)
AeroKev 10:f05bd773b66c 232 {
AeroKev 10:f05bd773b66c 233 currentRotation1 = toRadians(enc1.getPulses());
AeroKev 10:f05bd773b66c 234 currentRotation2 = toRadians(enc2.getPulses());
AeroKev 10:f05bd773b66c 235
AeroKev 10:f05bd773b66c 236 double previous_error1 = error1;
AeroKev 10:f05bd773b66c 237 double previous_error2 = error2;
AeroKev 10:f05bd773b66c 238
AeroKev 10:f05bd773b66c 239 error1 = desiredRotation1 - currentRotation1;
AeroKev 10:f05bd773b66c 240 error2 = desiredRotation2 - currentRotation2;
AeroKev 10:f05bd773b66c 241
AeroKev 10:f05bd773b66c 242 // PID control
AeroKev 11:7f41fac17c48 243 double control1, control2;
AeroKev 11:7f41fac17c48 244 if(pushing) {
AeroKev 17:7c54c7fcf9c5 245 if(error1 < 0.1f && error2 < 0.01f) {
AeroKev 17:7c54c7fcf9c5 246 control1 = 0;
AeroKev 17:7c54c7fcf9c5 247 control2 = 0;
AeroKev 17:7c54c7fcf9c5 248 }
AeroKev 17:7c54c7fcf9c5 249 else {
AeroKev 17:7c54c7fcf9c5 250 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 17:7c54c7fcf9c5 251 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 17:7c54c7fcf9c5 252 }
AeroKev 11:7f41fac17c48 253 } else {
AeroKev 11:7f41fac17c48 254 control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
AeroKev 11:7f41fac17c48 255 control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
AeroKev 11:7f41fac17c48 256 }
AeroKev 12:84f2c63f9b98 257
AeroKev 10:f05bd773b66c 258 int d1 = getPDirection(control1,1);
AeroKev 10:f05bd773b66c 259 int d2 = getPDirection(control2,2);
AeroKev 10:f05bd773b66c 260 float speed1 = fabs(control1);
AeroKev 10:f05bd773b66c 261 float speed2 = fabs(control2);
AeroKev 9:d04d028ccfe8 262
AeroKev 11:7f41fac17c48 263 if (speed1 < 0.02f) speed1 = 0.0f;
AeroKev 11:7f41fac17c48 264 if (speed2 < 0.02f) speed2 = 0.0f;
AeroKev 12:84f2c63f9b98 265
AeroKev 10:f05bd773b66c 266 dir1 = d1;
AeroKev 10:f05bd773b66c 267 dir2 = d2;
AeroKev 13:41d897396fb5 268 if(speed1 > speed2) {
AeroKev 13:41d897396fb5 269 speed1 = maxSpeed;
AeroKev 13:41d897396fb5 270 speed2 = (speed2/speed1)*maxSpeed;
AeroKev 13:41d897396fb5 271 }
AeroKev 13:41d897396fb5 272 else if(speed2 > speed1) {
AeroKev 13:41d897396fb5 273 speed2 = maxSpeed;
AeroKev 13:41d897396fb5 274 speed1 = (speed1/speed2)*maxSpeed;
AeroKev 13:41d897396fb5 275 }
AeroKev 13:41d897396fb5 276 else if(speed1 != 0 && speed2 != 0 && speed1 == speed2) {
AeroKev 13:41d897396fb5 277 speed1 = maxSpeed;
AeroKev 13:41d897396fb5 278 speed2 = maxSpeed;
AeroKev 13:41d897396fb5 279 }
AeroKev 10:f05bd773b66c 280 pwm1 = speed1;
AeroKev 13:41d897396fb5 281 pwm2 = speed2;
AeroKev 10:f05bd773b66c 282 }
AeroKev 9:d04d028ccfe8 283
AeroKev 12:84f2c63f9b98 284 /*
AeroKev 12:84f2c63f9b98 285 * Moves one motor with a speed of 0.2.
AeroKev 12:84f2c63f9b98 286 * int num: the ID of the motor [1 == motor 1 | 2 == motor 2]
AeroKev 12:84f2c63f9b98 287 * int dir: the direction of the motor
AeroKev 12:84f2c63f9b98 288 */
AeroKev 10:f05bd773b66c 289 void moveOneMotor(int num, int dir)
AeroKev 10:f05bd773b66c 290 {
AeroKev 10:f05bd773b66c 291 if(num == 1) {
AeroKev 10:f05bd773b66c 292 dir1 = dir;
AeroKev 10:f05bd773b66c 293 pwm1 = 0.2;
AeroKev 10:f05bd773b66c 294 } else {
AeroKev 10:f05bd773b66c 295 dir2 = dir;
AeroKev 10:f05bd773b66c 296 pwm2 = 0.2;
AeroKev 9:d04d028ccfe8 297 }
AeroKev 5:937b2f34a1ca 298 }