Margreeth de Breij / Mbed 2 deprecated frdm_MotorEMG

Motor programma met EMG

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Margreeth de Breij

main.cpp@25:ae908de29943, 2015-10-03 (annotated)

Committer:
Rvs94
Date:
Sat Oct 03 18:04:16 2015 +0000
Revision:
25:ae908de29943
Parent:
24:d0af4b2be295
Child:
26:cd1db85359aa
2 Motoren, werkende programma switch, en script overzichtelijker gemaakt. Enige nadeel is nog de oscilaties van de motoren, dit moet opgelost worden met filters en goede PID constanten.

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)
Rvs94 25:ae908de29943 45 const double m2_Ts = 0.01, m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 46
Rvs94 25:ae908de29943 47 //Controller gain Motor 2 & 1
Rvs94 25:ae908de29943 48 const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
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;
Rvs94 20:f5091e29cd26 52
Rvs94 25:ae908de29943 53 //Derivative filter coeffs Motor 2 & 1
Rvs94 25:ae908de29943 54 const double BiGain2 = 0.016955, BiGain1 = 0.016955;
Rvs94 25:ae908de29943 55 const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
Rvs94 25:ae908de29943 56 const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
Rvs94 20:f5091e29cd26 57
Rvs94 20:f5091e29cd26 58 // Filter variables
Rvs94 25:ae908de29943 59 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 25:ae908de29943 60 double m1_f_v1 = 0, m1_f_v2 = 0;
Rvs94 20:f5091e29cd26 61
Rvs94 25:ae908de29943 62 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 63 // General Functions
Rvs94 25:ae908de29943 64 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 65
Rvs94 20:f5091e29cd26 66 //HIDScope
Rvs94 25:ae908de29943 67 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 68 {
Rvs94 25:ae908de29943 69 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 70 scope.set(1, position2);
Rvs94 25:ae908de29943 71 scope.set(2, reference1 - position1);
Rvs94 25:ae908de29943 72 scope.set(3, position1);
Rvs94 25:ae908de29943 73 scope.send();
Rvs94 1:48aba8d5610a 74
Rvs94 25:ae908de29943 75 }
Rvs94 12:69ab81cf5b7d 76
Rvs94 12:69ab81cf5b7d 77 // Biquad filter
Rvs94 25:ae908de29943 78 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
Rvs94 25:ae908de29943 79 {
Rvs94 25:ae908de29943 80 double v = u - a1*v1 - a2*v2;
Rvs94 25:ae908de29943 81 double y = b0*v + b1*v1 + b2*v2;
Rvs94 25:ae908de29943 82 v2 = v1; v1 = v;
Rvs94 25:ae908de29943 83 return y;
Rvs94 25:ae908de29943 84 }
Rvs94 12:69ab81cf5b7d 85
Rvs94 12:69ab81cf5b7d 86
Rvs94 12:69ab81cf5b7d 87 // Reusable PID controller
Rvs94 25:ae908de29943 88 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
Rvs94 25:ae908de29943 89 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 25:ae908de29943 90 {
Rvs94 12:69ab81cf5b7d 91 // Derivative
Rvs94 25:ae908de29943 92 double e_der = (e-e_prev)/Ts;
Rvs94 25:ae908de29943 93 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 25:ae908de29943 94 e_prev = e;
Rvs94 12:69ab81cf5b7d 95 // Integral
Rvs94 25:ae908de29943 96 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 97 // PID
Rvs94 25:ae908de29943 98 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 25:ae908de29943 99 }
Rvs94 25:ae908de29943 100 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 101 // Motor control functions
Rvs94 25:ae908de29943 102 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 103
Margreeth95 18:6f71bb91b8bd 104 // Motor2 control
Rvs94 25:ae908de29943 105 void motor2_Controller()
Rvs94 9:774fc3c6a39e 106 {
Rvs94 25:ae908de29943 107 // Setpoint motor 2
Rvs94 25:ae908de29943 108 reference2 = m2_ref; // Reference in degrees
Rvs94 25:ae908de29943 109 position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 110 // Speed control
Rvs94 25:ae908de29943 111 double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,
Rvs94 25:ae908de29943 112 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 25:ae908de29943 113 double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input
Rvs94 25:ae908de29943 114 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 115 // Direction control
Rvs94 25:ae908de29943 116 if(m2_P2 > 0)
Rvs94 25:ae908de29943 117 {
Rvs94 25:ae908de29943 118 motor2direction = 0;
Rvs94 25:ae908de29943 119 }
Rvs94 25:ae908de29943 120 else
Rvs94 25:ae908de29943 121 {
Rvs94 25:ae908de29943 122 motor2direction = 1;
Rvs94 25:ae908de29943 123 }
Rvs94 25:ae908de29943 124 }
Rvs94 25:ae908de29943 125
Rvs94 25:ae908de29943 126 // Motor1 control
Rvs94 25:ae908de29943 127 void motor1_Controller()
Rvs94 25:ae908de29943 128 {
Rvs94 25:ae908de29943 129 // Setpoint Motor 1
Rvs94 25:ae908de29943 130 reference1 = m1_ref; // Reference in degrees
Rvs94 25:ae908de29943 131 position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 132 // Speed control
Rvs94 25:ae908de29943 133 double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2,
Rvs94 25:ae908de29943 134 m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 135 double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 136 motor1speed = abs(m1_P2);
Rvs94 25:ae908de29943 137 // Direction control
Rvs94 25:ae908de29943 138 if(m1_P2 > 0)
Rvs94 25:ae908de29943 139 {
Rvs94 25:ae908de29943 140 motor1direction = 1;
Rvs94 25:ae908de29943 141 }
Rvs94 25:ae908de29943 142 else
Rvs94 25:ae908de29943 143 {
Rvs94 25:ae908de29943 144 motor1direction = 0;
Rvs94 25:ae908de29943 145 }
Rvs94 9:774fc3c6a39e 146 }
Rvs94 3:687729d7996e 147
Rvs94 25:ae908de29943 148 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 149 // Main function
Rvs94 25:ae908de29943 150 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 151 int main()
Rvs94 25:ae908de29943 152 {
Rvs94 25:ae908de29943 153 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 154 // Initalizing
Rvs94 25:ae908de29943 155 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 156 //LEDs OFF
Rvs94 25:ae908de29943 157 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 158
Rvs94 25:ae908de29943 159 //PC connection & check
Rvs94 25:ae908de29943 160 pc.baud(115200);
Rvs94 25:ae908de29943 161 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 162
Rvs94 25:ae908de29943 163 // Tickers
Rvs94 25:ae908de29943 164 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 25:ae908de29943 165 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
Rvs94 25:ae908de29943 166 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Rvs94 25:ae908de29943 167
Rvs94 25:ae908de29943 168 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 169 // Control Program
Rvs94 25:ae908de29943 170 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 171 while(true)
Rvs94 20:f5091e29cd26 172 {
Rvs94 20:f5091e29cd26 173 char c = pc.getc();
Rvs94 25:ae908de29943 174 // 1 Program UP
Rvs94 25:ae908de29943 175 if(c == 'e')
Rvs94 20:f5091e29cd26 176 {
Rvs94 20:f5091e29cd26 177 count = count + 1;
Rvs94 20:f5091e29cd26 178 if(count > 2)
Rvs94 20:f5091e29cd26 179 {
Rvs94 20:f5091e29cd26 180 count = 2;
Rvs94 20:f5091e29cd26 181 }
Rvs94 20:f5091e29cd26 182
Rvs94 20:f5091e29cd26 183 }
Rvs94 25:ae908de29943 184 // 1 Program DOWN
Rvs94 25:ae908de29943 185 if(c == 'd')
Rvs94 20:f5091e29cd26 186 {
Rvs94 20:f5091e29cd26 187 count = count - 1;
Rvs94 20:f5091e29cd26 188 if(count < 0)
Rvs94 20:f5091e29cd26 189 {
Rvs94 20:f5091e29cd26 190 count = 0;
Rvs94 20:f5091e29cd26 191 }
Rvs94 25:ae908de29943 192 }
Rvs94 25:ae908de29943 193 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 25:ae908de29943 194 if(count == 0)
Rvs94 20:f5091e29cd26 195 {
Rvs94 20:f5091e29cd26 196
Rvs94 20:f5091e29cd26 197 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 198 LedG = 0;
Margreeth95 19:9417d2011e8b 199 if(c == 'r')
Margreeth95 19:9417d2011e8b 200 {
Rvs94 24:d0af4b2be295 201 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 202 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 203 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 204 {
Rvs94 24:d0af4b2be295 205 m2_ref = Grens2;
Rvs94 25:ae908de29943 206 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 207 }
Margreeth95 19:9417d2011e8b 208 }
Margreeth95 19:9417d2011e8b 209 if(c == 'f')
Margreeth95 19:9417d2011e8b 210 {
Rvs94 24:d0af4b2be295 211 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 212 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 213 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 214 {
Rvs94 24:d0af4b2be295 215 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 216 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 217 }
Margreeth95 19:9417d2011e8b 218 }
Rvs94 20:f5091e29cd26 219 }
Rvs94 25:ae908de29943 220 // PROGRAM 1: Motor 1 control, Red LED
Rvs94 25:ae908de29943 221 if(count == 1)
Rvs94 20:f5091e29cd26 222 {
Rvs94 20:f5091e29cd26 223 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 224 LedR = 0;
Rvs94 24:d0af4b2be295 225 if(c == 't')
Rvs94 24:d0af4b2be295 226 {
Rvs94 24:d0af4b2be295 227 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 228 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 229 {
Rvs94 24:d0af4b2be295 230 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 231 }
Rvs94 24:d0af4b2be295 232 }
Rvs94 24:d0af4b2be295 233 if(c == 'g')
Rvs94 24:d0af4b2be295 234 {
Rvs94 24:d0af4b2be295 235 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 236 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 237 {
Rvs94 24:d0af4b2be295 238 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 239 }
Rvs94 24:d0af4b2be295 240 }
Rvs94 20:f5091e29cd26 241 }
Rvs94 25:ae908de29943 242 // PROGRAM 2: Firing mechanism & Reset, Blue LED
Rvs94 25:ae908de29943 243 if(count == 2)
Rvs94 20:f5091e29cd26 244 {
Rvs94 20:f5091e29cd26 245
Rvs94 20:f5091e29cd26 246 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 247 LedB = 0;
Rvs94 25:ae908de29943 248 //VUUUUR!! (To Do)
Rvs94 24:d0af4b2be295 249 wait(1);
Rvs94 24:d0af4b2be295 250 m2_ref = 0;
Rvs94 24:d0af4b2be295 251 m1_ref = 0;
Rvs94 20:f5091e29cd26 252 }
Margreeth95 0:284ed397e046 253 }
Rvs94 9:774fc3c6a39e 254
Margreeth95 0:284ed397e046 255 }