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@12:84f2c63f9b98, 2015-10-26 (annotated)

Committer:
AeroKev
Date:
Mon Oct 26 12:13:12 2015 +0000
Revision:
12:84f2c63f9b98
Parent:
11:7f41fac17c48
Child:
13:41d897396fb5
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 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 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;
AeroKev 9:d04d028ccfe8 124 double prev = -1.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);
AeroKev 10:f05bd773b66c 194 }
AeroKev 7:0fb420b3434f 195
AeroKev 12:84f2c63f9b98 196 /*
AeroKev 12:84f2c63f9b98 197 * Rotates the motors to certain rotations, while using the PID Controller used for the pushing motion
AeroKev 12:84f2c63f9b98 198 * double a: The desired rotation of motor 1
AeroKev 12:84f2c63f9b98 199 * double b: The desired rotation of motor 2
AeroKev 12:84f2c63f9b98 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 12:84f2c63f9b98 208 /*
AeroKev 12:84f2c63f9b98 209 * Returns the offset of the motors calulated in the initialization phase.
AeroKev 12:84f2c63f9b98 210 * int a: The id of the motor [1 == motor 1 | 2 == motor 2]
AeroKev 12:84f2c63f9b98 211 */
AeroKev 11:7f41fac17c48 212 double getOffset(int a)
AeroKev 11:7f41fac17c48 213 {
AeroKev 10:f05bd773b66c 214 if(a == 1) return offsetA;
AeroKev 10:f05bd773b66c 215 else return offsetB;
AeroKev 10:f05bd773b66c 216 }
AeroKev 11:7f41fac17c48 217
AeroKev 12:84f2c63f9b98 218 /*
AeroKev 12:84f2c63f9b98 219 * Stops the motors.
AeroKev 12:84f2c63f9b98 220 */
AeroKev 11:7f41fac17c48 221 void PID_stop()
AeroKev 11:7f41fac17c48 222 {
AeroKev 10:f05bd773b66c 223 pwm1 = 0;
AeroKev 11:7f41fac17c48 224 pwm2 = 0;
AeroKev 9:d04d028ccfe8 225 }
AeroKev 9:d04d028ccfe8 226
AeroKev 12:84f2c63f9b98 227 /*
AeroKev 12:84f2c63f9b98 228 * 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 229 */
AeroKev 10:f05bd773b66c 230 void moveTick()
AeroKev 10:f05bd773b66c 231 {
AeroKev 10:f05bd773b66c 232 currentRotation1 = toRadians(enc1.getPulses());
AeroKev 10:f05bd773b66c 233 currentRotation2 = toRadians(enc2.getPulses());
AeroKev 10:f05bd773b66c 234
AeroKev 10:f05bd773b66c 235 double previous_error1 = error1;
AeroKev 10:f05bd773b66c 236 double previous_error2 = error2;
AeroKev 10:f05bd773b66c 237
AeroKev 10:f05bd773b66c 238 error1 = desiredRotation1 - currentRotation1;
AeroKev 10:f05bd773b66c 239 error2 = desiredRotation2 - currentRotation2;
AeroKev 10:f05bd773b66c 240
AeroKev 10:f05bd773b66c 241 // PID control
AeroKev 11:7f41fac17c48 242 double control1, control2;
AeroKev 11:7f41fac17c48 243 if(pushing) {
AeroKev 11:7f41fac17c48 244 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 245 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 246 } else {
AeroKev 11:7f41fac17c48 247 control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
AeroKev 11:7f41fac17c48 248 control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
AeroKev 11:7f41fac17c48 249 }
AeroKev 12:84f2c63f9b98 250
AeroKev 10:f05bd773b66c 251 int d1 = getPDirection(control1,1);
AeroKev 10:f05bd773b66c 252 int d2 = getPDirection(control2,2);
AeroKev 10:f05bd773b66c 253 float speed1 = fabs(control1);
AeroKev 10:f05bd773b66c 254 float speed2 = fabs(control2);
AeroKev 9:d04d028ccfe8 255
AeroKev 11:7f41fac17c48 256 if (speed1 < 0.02f) speed1 = 0.0f;
AeroKev 11:7f41fac17c48 257 if (speed2 < 0.02f) speed2 = 0.0f;
AeroKev 12:84f2c63f9b98 258
AeroKev 10:f05bd773b66c 259 dir1 = d1;
AeroKev 10:f05bd773b66c 260 dir2 = d2;
AeroKev 10:f05bd773b66c 261 pwm1 = speed1;
AeroKev 10:f05bd773b66c 262 pwm2 = speed2;
AeroKev 10:f05bd773b66c 263 }
AeroKev 9:d04d028ccfe8 264
AeroKev 12:84f2c63f9b98 265 /*
AeroKev 12:84f2c63f9b98 266 * Moves one motor with a speed of 0.2.
AeroKev 12:84f2c63f9b98 267 * int num: the ID of the motor [1 == motor 1 | 2 == motor 2]
AeroKev 12:84f2c63f9b98 268 * int dir: the direction of the motor
AeroKev 12:84f2c63f9b98 269 */
AeroKev 10:f05bd773b66c 270 void moveOneMotor(int num, int dir)
AeroKev 10:f05bd773b66c 271 {
AeroKev 10:f05bd773b66c 272 if(num == 1) {
AeroKev 10:f05bd773b66c 273 dir1 = dir;
AeroKev 10:f05bd773b66c 274 pwm1 = 0.2;
AeroKev 10:f05bd773b66c 275 } else {
AeroKev 10:f05bd773b66c 276 dir2 = dir;
AeroKev 10:f05bd773b66c 277 pwm2 = 0.2;
AeroKev 9:d04d028ccfe8 278 }
AeroKev 5:937b2f34a1ca 279 }