BMT M9 groep 7 / Mbed 2 deprecated frdm_Motor_V2_4

werkende PID, int/double probleem met encoder opgelost.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Robert Schulte

main.cpp@26:70e5b6908e0a, 2015-10-21 (annotated)

Committer:
laura94
Date:
Wed Oct 21 13:37:34 2015 +0000
Revision:
26:70e5b6908e0a
Parent:
25:ae908de29943 
Werkende PID controller, fout met integer en doubles is opgelost.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rvs94 25:ae908de29943 1 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 2 // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
Rvs94 25:ae908de29943 3 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 4 // Libraries
Rvs94 25:ae908de29943 5 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 6 #include "mbed.h"
Margreeth95 0:284ed397e046 7 #include "MODSERIAL.h"
Margreeth95 0:284ed397e046 8 #include "HIDScope.h"
Margreeth95 0:284ed397e046 9 #include "QEI.h"
Rvs94 12:69ab81cf5b7d 10 #include "biquadFilter.h"
Rvs94 25:ae908de29943 11
Rvs94 25:ae908de29943 12 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 13 // Constanten/Inputs/Outputs
Rvs94 25:ae908de29943 14 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 15 MODSERIAL pc(USBTX, USBRX); // To/From PC
Rvs94 25:ae908de29943 16 QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2
Rvs94 25:ae908de29943 17 QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1
Rvs94 25:ae908de29943 18 HIDScope scope(4); // Scope, 4 channels
Rvs94 20:f5091e29cd26 19
Rvs94 25:ae908de29943 20 // LEDs
Rvs94 25:ae908de29943 21 DigitalOut LedR(LED_RED);
Rvs94 25:ae908de29943 22 DigitalOut LedG(LED_GREEN);
Rvs94 25:ae908de29943 23 DigitalOut LedB(LED_BLUE);
Rvs94 20:f5091e29cd26 24
Rvs94 25:ae908de29943 25 // Motor
Rvs94 25:ae908de29943 26 DigitalOut motor1direction(D7); // Motor 1, Direction & Speed
Rvs94 25:ae908de29943 27 PwmOut motor1speed(D6);
Rvs94 25:ae908de29943 28 DigitalOut motor2direction(D4); // Motor 2, Direction & Speed
Rvs94 25:ae908de29943 29 PwmOut motor2speed(D5);
Rvs94 7:67b50d4fb03c 30
Rvs94 25:ae908de29943 31 // Tickers
Rvs94 25:ae908de29943 32 Ticker ScopeTime;
Rvs94 25:ae908de29943 33 Ticker myControllerTicker2;
Rvs94 25:ae908de29943 34 Ticker myControllerTicker1;
Rvs94 25:ae908de29943 35
Rvs94 25:ae908de29943 36 // Constants
Rvs94 25:ae908de29943 37 double reference2, reference1;
Rvs94 25:ae908de29943 38 double position2 = 0, position1 = 0;
Rvs94 25:ae908de29943 39 double m2_ref = 0, m1_ref = 0;
Rvs94 25:ae908de29943 40 int count = 0;
Rvs94 25:ae908de29943 41 double Grens2 = 90, Grens1 = 90;
Rvs94 25:ae908de29943 42 double Stapgrootte = 5;
Rvs94 2:099da0fc31b6 43
Rvs94 25:ae908de29943 44 //Sample time (motor-step)
laura94 26:70e5b6908e0a 45 const double m2_Ts = 0.001, m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 46
Rvs94 25:ae908de29943 47 //Controller gain Motor 2 & 1
laura94 26:70e5b6908e0a 48 const double m2_Kp = 2.1/57,m2_Ki = 3.9/57, m2_Kd = 0.1/57;
Rvs94 25:ae908de29943 49 const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
Rvs94 25:ae908de29943 50 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 25:ae908de29943 51 double m1_err_int = 0, m1_prev_err = 0;
laura94 26:70e5b6908e0a 52
laura94 26:70e5b6908e0a 53 const double BiGainEMG_L1= 0.231938;
laura94 26:70e5b6908e0a 54 const double EMGL1_a1 = -0.25537145181, EMGL1_a2 = 0.18312488356, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
laura94 26:70e5b6908e0a 55
laura94 26:70e5b6908e0a 56 biquadFilter m2_lowpass (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);
Rvs94 20:f5091e29cd26 57
laura94 26:70e5b6908e0a 58 double x;
Rvs94 25:ae908de29943 59 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 60 // General Functions
Rvs94 25:ae908de29943 61 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 62
Rvs94 20:f5091e29cd26 63 //HIDScope
Rvs94 25:ae908de29943 64 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 65 {
Rvs94 25:ae908de29943 66 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 67 scope.set(1, position2);
Rvs94 25:ae908de29943 68 scope.set(2, reference1 - position1);
Rvs94 25:ae908de29943 69 scope.set(3, position1);
laura94 26:70e5b6908e0a 70 scope.set(4, x);
Rvs94 25:ae908de29943 71 scope.send();
Rvs94 1:48aba8d5610a 72
laura94 26:70e5b6908e0a 73 }
Rvs94 12:69ab81cf5b7d 74
Rvs94 12:69ab81cf5b7d 75
Rvs94 12:69ab81cf5b7d 76 // Reusable PID controller
laura94 26:70e5b6908e0a 77 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
Rvs94 25:ae908de29943 78 {
Rvs94 12:69ab81cf5b7d 79 // Derivative
Rvs94 25:ae908de29943 80 double e_der = (e-e_prev)/Ts;
laura94 26:70e5b6908e0a 81 e_der = m2_lowpass.step(e_der);
Rvs94 25:ae908de29943 82 e_prev = e;
Rvs94 12:69ab81cf5b7d 83 // Integral
Rvs94 25:ae908de29943 84 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 85 // PID
laura94 26:70e5b6908e0a 86 x = Kp * e + Ki*e_int + Kd*e_der;
laura94 26:70e5b6908e0a 87 return x;
Rvs94 25:ae908de29943 88 }
Rvs94 25:ae908de29943 89 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 90 // Motor control functions
Rvs94 25:ae908de29943 91 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 92
Margreeth95 18:6f71bb91b8bd 93 // Motor2 control
Rvs94 25:ae908de29943 94 void motor2_Controller()
Rvs94 9:774fc3c6a39e 95 {
Rvs94 25:ae908de29943 96 // Setpoint motor 2
Rvs94 25:ae908de29943 97 reference2 = m2_ref; // Reference in degrees
laura94 26:70e5b6908e0a 98 position2 = 1.0*Encoder2.getPulses()*360.0/(32.0*131.0); // Position in degrees
Rvs94 25:ae908de29943 99 // Speed control
laura94 26:70e5b6908e0a 100 double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
laura94 26:70e5b6908e0a 101 double m2_P2 = m2_P1; // Filter of motorspeed input
Rvs94 25:ae908de29943 102 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 103 // Direction control
Rvs94 25:ae908de29943 104 if(m2_P2 > 0)
Rvs94 25:ae908de29943 105 {
Rvs94 25:ae908de29943 106 motor2direction = 0;
Rvs94 25:ae908de29943 107 }
Rvs94 25:ae908de29943 108 else
Rvs94 25:ae908de29943 109 {
Rvs94 25:ae908de29943 110 motor2direction = 1;
Rvs94 25:ae908de29943 111 }
Rvs94 25:ae908de29943 112 }
Rvs94 25:ae908de29943 113
Rvs94 25:ae908de29943 114 // Motor1 control
Rvs94 25:ae908de29943 115 void motor1_Controller()
Rvs94 25:ae908de29943 116 {
Rvs94 25:ae908de29943 117 // Setpoint Motor 1
Rvs94 25:ae908de29943 118 reference1 = m1_ref; // Reference in degrees
Rvs94 25:ae908de29943 119 position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 120 // Speed control
laura94 26:70e5b6908e0a 121 double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err);
laura94 26:70e5b6908e0a 122 double m1_P2 = m1_P1;
Rvs94 25:ae908de29943 123 motor1speed = abs(m1_P2);
Rvs94 25:ae908de29943 124 // Direction control
Rvs94 25:ae908de29943 125 if(m1_P2 > 0)
Rvs94 25:ae908de29943 126 {
Rvs94 25:ae908de29943 127 motor1direction = 1;
Rvs94 25:ae908de29943 128 }
Rvs94 25:ae908de29943 129 else
Rvs94 25:ae908de29943 130 {
Rvs94 25:ae908de29943 131 motor1direction = 0;
Rvs94 25:ae908de29943 132 }
Rvs94 9:774fc3c6a39e 133 }
Rvs94 3:687729d7996e 134
Rvs94 25:ae908de29943 135 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 136 // Main function
Rvs94 25:ae908de29943 137 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 138 int main()
Rvs94 25:ae908de29943 139 {
Rvs94 25:ae908de29943 140 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 141 // Initalizing
Rvs94 25:ae908de29943 142 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 143 //LEDs OFF
Rvs94 25:ae908de29943 144 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 145
Rvs94 25:ae908de29943 146 //PC connection & check
Rvs94 25:ae908de29943 147 pc.baud(115200);
Rvs94 25:ae908de29943 148 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 149
Rvs94 25:ae908de29943 150 // Tickers
Rvs94 25:ae908de29943 151 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
laura94 26:70e5b6908e0a 152 myControllerTicker2.attach(&motor2_Controller, m2_Ts ); // 1000 Hz, Motor 2
Rvs94 25:ae908de29943 153 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Rvs94 25:ae908de29943 154
Rvs94 25:ae908de29943 155 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 156 // Control Program
Rvs94 25:ae908de29943 157 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 158 while(true)
Rvs94 20:f5091e29cd26 159 {
Rvs94 20:f5091e29cd26 160 char c = pc.getc();
Rvs94 25:ae908de29943 161 // 1 Program UP
Rvs94 25:ae908de29943 162 if(c == 'e')
Rvs94 20:f5091e29cd26 163 {
Rvs94 20:f5091e29cd26 164 count = count + 1;
Rvs94 20:f5091e29cd26 165 if(count > 2)
Rvs94 20:f5091e29cd26 166 {
Rvs94 20:f5091e29cd26 167 count = 2;
Rvs94 20:f5091e29cd26 168 }
Rvs94 20:f5091e29cd26 169
Rvs94 20:f5091e29cd26 170 }
Rvs94 25:ae908de29943 171 // 1 Program DOWN
Rvs94 25:ae908de29943 172 if(c == 'd')
Rvs94 20:f5091e29cd26 173 {
Rvs94 20:f5091e29cd26 174 count = count - 1;
Rvs94 20:f5091e29cd26 175 if(count < 0)
Rvs94 20:f5091e29cd26 176 {
Rvs94 20:f5091e29cd26 177 count = 0;
Rvs94 20:f5091e29cd26 178 }
Rvs94 25:ae908de29943 179 }
Rvs94 25:ae908de29943 180 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 25:ae908de29943 181 if(count == 0)
Rvs94 20:f5091e29cd26 182 {
Rvs94 20:f5091e29cd26 183
Rvs94 20:f5091e29cd26 184 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 185 LedG = 0;
Margreeth95 19:9417d2011e8b 186 if(c == 'r')
Margreeth95 19:9417d2011e8b 187 {
Rvs94 24:d0af4b2be295 188 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 189 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 190 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 191 {
Rvs94 24:d0af4b2be295 192 m2_ref = Grens2;
Rvs94 25:ae908de29943 193 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 194 }
Margreeth95 19:9417d2011e8b 195 }
Margreeth95 19:9417d2011e8b 196 if(c == 'f')
Margreeth95 19:9417d2011e8b 197 {
Rvs94 24:d0af4b2be295 198 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 199 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 200 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 201 {
Rvs94 24:d0af4b2be295 202 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 203 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 204 }
Margreeth95 19:9417d2011e8b 205 }
Rvs94 20:f5091e29cd26 206 }
Rvs94 25:ae908de29943 207 // PROGRAM 1: Motor 1 control, Red LED
Rvs94 25:ae908de29943 208 if(count == 1)
Rvs94 20:f5091e29cd26 209 {
Rvs94 20:f5091e29cd26 210 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 211 LedR = 0;
Rvs94 24:d0af4b2be295 212 if(c == 't')
Rvs94 24:d0af4b2be295 213 {
Rvs94 24:d0af4b2be295 214 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 215 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 216 {
Rvs94 24:d0af4b2be295 217 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 218 }
Rvs94 24:d0af4b2be295 219 }
Rvs94 24:d0af4b2be295 220 if(c == 'g')
Rvs94 24:d0af4b2be295 221 {
Rvs94 24:d0af4b2be295 222 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 223 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 224 {
Rvs94 24:d0af4b2be295 225 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 226 }
Rvs94 24:d0af4b2be295 227 }
Rvs94 20:f5091e29cd26 228 }
Rvs94 25:ae908de29943 229 // PROGRAM 2: Firing mechanism & Reset, Blue LED
Rvs94 25:ae908de29943 230 if(count == 2)
Rvs94 20:f5091e29cd26 231 {
Rvs94 20:f5091e29cd26 232
Rvs94 20:f5091e29cd26 233 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 234 LedB = 0;
Rvs94 25:ae908de29943 235 //VUUUUR!! (To Do)
Rvs94 24:d0af4b2be295 236 wait(1);
Rvs94 24:d0af4b2be295 237 m2_ref = 0;
Rvs94 24:d0af4b2be295 238 m1_ref = 0;
Rvs94 20:f5091e29cd26 239 }
Margreeth95 0:284ed397e046 240 }
Rvs94 9:774fc3c6a39e 241
Margreeth95 0:284ed397e046 242 }