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

Committer:
AeroKev
Date:
Tue Oct 20 11:38:06 2015 +0000
Revision:
8:ef6b758b73da
Parent:
7:0fb420b3434f
Child:
9:d04d028ccfe8
ja ja

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 AnalogIn pot1(m1_pot);
AeroKev 0:d38eb4622914 39 AnalogIn pot2(m2_pot);
AeroKev 0:d38eb4622914 40
AeroKev 0:d38eb4622914 41 // PWM Speed Control：
AeroKev 0:d38eb4622914 42 DigitalOut dir1(m1_dir);
AeroKev 0:d38eb4622914 43 PwmOut pwm1(m1_pwm);
AeroKev 0:d38eb4622914 44 DigitalOut dir2(m2_dir);
AeroKev 0:d38eb4622914 45 PwmOut pwm2(m2_pwm);
AeroKev 0:d38eb4622914 46
AeroKev 0:d38eb4622914 47 QEI enc1(m1_enc_a, m1_enc_b, NC, 1);
AeroKev 0:d38eb4622914 48 QEI enc2(m2_enc_a, m2_enc_b, NC, 1);
AeroKev 0:d38eb4622914 49
AeroKev 0:d38eb4622914 50 Ticker potTicker;
AeroKev 0:d38eb4622914 51 Ticker motorTicker;
AeroKev 0:d38eb4622914 52
AeroKev 0:d38eb4622914 53 Ticker graphTicker;
AeroKev 1:fe23126b0389 54 //HIDScope grapher(4);
AeroKev 0:d38eb4622914 55
AeroKev 0:d38eb4622914 56 float currentRotation1 = 0, currentRotation2 = 0;
AeroKev 0:d38eb4622914 57 float desiredRotation1 = 0, desiredRotation2 = 0;
AeroKev 0:d38eb4622914 58 double error1 = 0, error2 = 0;
AeroKev 0:d38eb4622914 59
AeroKev 0:d38eb4622914 60 double m1_error_integral = 0, m2_error_integral = 0;
AeroKev 0:d38eb4622914 61 double m1_error_derivative = 0, m2_error_derivative = 0;
AeroKev 0:d38eb4622914 62 biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
AeroKev 0:d38eb4622914 63 biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);
AeroKev 0:d38eb4622914 64
AeroKev 0:d38eb4622914 65 bool shutup = true;
AeroKev 0:d38eb4622914 66 bool go_pot = false;
AeroKev 0:d38eb4622914 67 bool go_motor = false;
AeroKev 0:d38eb4622914 68 bool go_graph = false;
AeroKev 0:d38eb4622914 69
AeroKev 0:d38eb4622914 70 float getPotRad(AnalogIn pot)
AeroKev 0:d38eb4622914 71 {
AeroKev 0:d38eb4622914 72 return pot.read() * 4.0f - 2.0f;
AeroKev 0:d38eb4622914 73 }
AeroKev 0:d38eb4622914 74
AeroKev 0:d38eb4622914 75 float toRadians(int pulses)
AeroKev 0:d38eb4622914 76 {
AeroKev 0:d38eb4622914 77 int remaining = pulses;// % sigPerRev;
AeroKev 0:d38eb4622914 78 float percent = (float) remaining / (float) sigPerRev;
AeroKev 0:d38eb4622914 79 return percent * 2.0f;
AeroKev 0:d38eb4622914 80 }
AeroKev 0:d38eb4622914 81
AeroKev 0:d38eb4622914 82 void readPot()
AeroKev 0:d38eb4622914 83 {
AeroKev 0:d38eb4622914 84 go_pot = true;
AeroKev 0:d38eb4622914 85 }
AeroKev 0:d38eb4622914 86
AeroKev 0:d38eb4622914 87 void getMotorRotation()
AeroKev 0:d38eb4622914 88 {
AeroKev 0:d38eb4622914 89 go_motor = true;
AeroKev 0:d38eb4622914 90 }
AeroKev 0:d38eb4622914 91
AeroKev 0:d38eb4622914 92 void sendGraph()
AeroKev 0:d38eb4622914 93 {
AeroKev 0:d38eb4622914 94 go_graph = true;
AeroKev 0:d38eb4622914 95 }
AeroKev 0:d38eb4622914 96
AeroKev 0:d38eb4622914 97 double p_control(double error, double kp)
AeroKev 0:d38eb4622914 98 {
AeroKev 0:d38eb4622914 99 return kp * error;
AeroKev 0:d38eb4622914 100 }
AeroKev 0:d38eb4622914 101
AeroKev 0:d38eb4622914 102 double pi_control(double error, double kp, double ki, double ts, double &error_integral)
AeroKev 0:d38eb4622914 103 {
AeroKev 0:d38eb4622914 104 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 105 double result = kp * error + ki * error_integral;
AeroKev 0:d38eb4622914 106 return result;
AeroKev 0:d38eb4622914 107 }
AeroKev 0:d38eb4622914 108
AeroKev 7:0fb420b3434f 109 double pid_control(double error, double kp, double ki, double ts, double &error_integral,
AeroKev 7:0fb420b3434f 110 double kd, double previous_error, double &error_derivative, biquadFilter filter)
AeroKev 0:d38eb4622914 111 {
AeroKev 0:d38eb4622914 112 error_integral = error_integral + ts * error;
AeroKev 0:d38eb4622914 113 error_derivative = (error - previous_error) / ts;
AeroKev 0:d38eb4622914 114 // error_derivative = filter.step(error_derivative);
AeroKev 7:0fb420b3434f 115
AeroKev 0:d38eb4622914 116 double result = kp * error + ki * error_integral + kd * error_derivative;
AeroKev 0:d38eb4622914 117 return result;
AeroKev 0:d38eb4622914 118 }
AeroKev 0:d38eb4622914 119
AeroKev 1:fe23126b0389 120 int getPDirection(double control, int motor)
AeroKev 0:d38eb4622914 121 {
AeroKev 0:d38eb4622914 122 if (control >= 0)
AeroKev 1:fe23126b0389 123 return (motor == 1)?1:0;
AeroKev 0:d38eb4622914 124 else
AeroKev 1:fe23126b0389 125 return (motor == 1)?0:1;
AeroKev 0:d38eb4622914 126 }
AeroKev 0:d38eb4622914 127
AeroKev 8:ef6b758b73da 128 void PID_init()
AeroKev 0:d38eb4622914 129 {
AeroKev 0:d38eb4622914 130 pc.printf("Initializing...\r\n");
AeroKev 0:d38eb4622914 131
AeroKev 0:d38eb4622914 132 // Set proper baudrate
AeroKev 0:d38eb4622914 133 // pc.baud(115200);
AeroKev 0:d38eb4622914 134
AeroKev 0:d38eb4622914 135 // Reset encoders
AeroKev 0:d38eb4622914 136 enc1.reset();
AeroKev 1:fe23126b0389 137 enc2.reset();
AeroKev 0:d38eb4622914 138 pc.printf("Encoders reset\r\n");
AeroKev 0:d38eb4622914 139
AeroKev 7:0fb420b3434f 140 go_motor = true;
AeroKev 7:0fb420b3434f 141
AeroKev 0:d38eb4622914 142 // Start tickers
AeroKev 0:d38eb4622914 143 potTicker.attach(&readPot, tickRate);
AeroKev 0:d38eb4622914 144 motorTicker.attach(&getMotorRotation, tickRate);
AeroKev 0:d38eb4622914 145 graphTicker.attach(&sendGraph, graphTickRate);
AeroKev 0:d38eb4622914 146 pc.printf("Tickers attached\r\n");
AeroKev 0:d38eb4622914 147
AeroKev 0:d38eb4622914 148 pc.printf("Initialized\r\n");
AeroKev 0:d38eb4622914 149 }
AeroKev 0:d38eb4622914 150
AeroKev 7:0fb420b3434f 151 void getCurrent(double& a, double& b)
AeroKev 7:0fb420b3434f 152 {
AeroKev 6:48bb8aa4888b 153 a = toRadians(enc1.getPulses());
AeroKev 7:0fb420b3434f 154 b = toRadians(enc2.getPulses());
AeroKev 6:48bb8aa4888b 155 }
AeroKev 6:48bb8aa4888b 156
AeroKev 7:0fb420b3434f 157 int move(double a, double b)
AeroKev 7:0fb420b3434f 158 {
AeroKev 7:0fb420b3434f 159 if (shutup) {
AeroKev 7:0fb420b3434f 160 pwm1 = 0;
AeroKev 7:0fb420b3434f 161 pwm2 = 0;
AeroKev 7:0fb420b3434f 162 } else {
AeroKev 7:0fb420b3434f 163 desiredRotation1 = a;
AeroKev 7:0fb420b3434f 164 desiredRotation2 = b;
AeroKev 0:d38eb4622914 165
AeroKev 7:0fb420b3434f 166 currentRotation1 = toRadians(enc1.getPulses());
AeroKev 7:0fb420b3434f 167 currentRotation2 = toRadians(enc2.getPulses());
AeroKev 0:d38eb4622914 168
AeroKev 7:0fb420b3434f 169 double previous_error1 = error1;
AeroKev 7:0fb420b3434f 170 double previous_error2 = error2;
AeroKev 0:d38eb4622914 171
AeroKev 7:0fb420b3434f 172 error1 = desiredRotation1 - currentRotation1;
AeroKev 7:0fb420b3434f 173 error2 = desiredRotation2 - currentRotation2;
AeroKev 7:0fb420b3434f 174
AeroKev 7:0fb420b3434f 175 // PID control
AeroKev 7:0fb420b3434f 176 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 177 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 178
AeroKev 7:0fb420b3434f 179 int d1 = getPDirection(control1,1);
AeroKev 7:0fb420b3434f 180 int d2 = getPDirection(control2,2);
AeroKev 7:0fb420b3434f 181 float speed1 = fabs(control1);
AeroKev 7:0fb420b3434f 182 float speed2 = fabs(control2);
AeroKev 0:d38eb4622914 183
AeroKev 7:0fb420b3434f 184 if (speed1 < 0.1f) speed1 = 0.0f;
AeroKev 7:0fb420b3434f 185 if (speed2 < 0.1f) speed2 = 0.0f;
AeroKev 7:0fb420b3434f 186 dir1 = d1;
AeroKev 7:0fb420b3434f 187 dir2 = d2;
AeroKev 7:0fb420b3434f 188 pwm1 = speed1;
AeroKev 7:0fb420b3434f 189 pwm2 = speed2;
AeroKev 5:937b2f34a1ca 190 }
AeroKev 5:937b2f34a1ca 191 }