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@24:d0af4b2be295, 2015-10-03 (annotated)

Committer:
Rvs94
Date:
Sat Oct 03 15:59:25 2015 +0000
Revision:
24:d0af4b2be295
Parent:
20:f5091e29cd26
Child:
25:ae908de29943
Tweede motor toegevoegd. Motor 2 is werkend, motor 1 reageert niet naar behoren. Snap nog niet waar de fout zit. Verder geeft 2 motoren tegelijkertijd aangesloten ook problemen.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Margreeth95 0:284ed397e046 1 #include "mbed.h"
Margreeth95 0:284ed397e046 2 #include "MODSERIAL.h"
Margreeth95 0:284ed397e046 3 #include "HIDScope.h"
Margreeth95 0:284ed397e046 4 #include "QEI.h"
Rvs94 12:69ab81cf5b7d 5 #include "biquadFilter.h"
Margreeth95 0:284ed397e046 6
Margreeth95 0:284ed397e046 7 Serial pc(USBTX, USBRX); // tx, rx
Rvs94 24:d0af4b2be295 8 QEI Encoder2(D3, D2, NC, 32);
Rvs94 24:d0af4b2be295 9 QEI Encoder1(D13,D12,NC, 32);
Rvs94 24:d0af4b2be295 10 HIDScope scope(4);
Rvs94 20:f5091e29cd26 11
Rvs94 20:f5091e29cd26 12 //Ledjes
Margreeth95 19:9417d2011e8b 13 DigitalOut LedR(LED_RED);
Margreeth95 19:9417d2011e8b 14 DigitalOut LedG(LED_GREEN);
Margreeth95 19:9417d2011e8b 15 DigitalOut LedB(LED_BLUE);
Rvs94 20:f5091e29cd26 16
Rvs94 20:f5091e29cd26 17 //Motor
Rvs94 24:d0af4b2be295 18 DigitalOut motor1direction(D6);
Rvs94 24:d0af4b2be295 19 PwmOut motor1speed(D7);
Rvs94 24:d0af4b2be295 20 DigitalOut motor2direction(D4);
Margreeth95 0:284ed397e046 21 PwmOut motor2speed(D5);
Rvs94 12:69ab81cf5b7d 22
Rvs94 20:f5091e29cd26 23 //Tickers
Margreeth95 0:284ed397e046 24 Ticker ScopeTime;
Rvs94 24:d0af4b2be295 25 Ticker myControllerTicker2;
Rvs94 7:67b50d4fb03c 26
Rvs94 20:f5091e29cd26 27 //Startwaarden
Rvs94 24:d0af4b2be295 28 double reference2, reference1;
Rvs94 24:d0af4b2be295 29 double position2 = 0, position1 = 0;
Rvs94 24:d0af4b2be295 30 double m2_ref = 0, m1_ref = 0;
Margreeth95 19:9417d2011e8b 31 int count = 0;
Rvs94 24:d0af4b2be295 32 double Grens2 = 90, Grens1 = 90;
Rvs94 24:d0af4b2be295 33 double Stapgrootte = 5;
Rvs94 2:099da0fc31b6 34
Rvs94 20:f5091e29cd26 35 //Sample time (motor2-step)
Rvs94 20:f5091e29cd26 36 const double m2_Ts = 0.01;
Rvs94 24:d0af4b2be295 37 const double m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 38
Rvs94 24:d0af4b2be295 39 //Controller gain Motor
Rvs94 24:d0af4b2be295 40 const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
Rvs94 24:d0af4b2be295 41 const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
Rvs94 20:f5091e29cd26 42 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 24:d0af4b2be295 43 double m1_err_int = 0, m1_prev_err = 0;
Rvs94 20:f5091e29cd26 44
Rvs94 20:f5091e29cd26 45 //Derivative filter coeffs Motor 2
Rvs94 20:f5091e29cd26 46 const double BiGain = 0.016955;
Rvs94 20:f5091e29cd26 47 const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
Rvs94 20:f5091e29cd26 48
Rvs94 20:f5091e29cd26 49 // Filter variables
Rvs94 20:f5091e29cd26 50 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 20:f5091e29cd26 51
Rvs94 20:f5091e29cd26 52
Rvs94 20:f5091e29cd26 53 //HIDScope
Margreeth95 0:284ed397e046 54 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 55 {
Rvs94 24:d0af4b2be295 56 scope.set(0, reference2 - position2);
Rvs94 24:d0af4b2be295 57 scope.set(1, position2);
Rvs94 24:d0af4b2be295 58 scope.set(2, reference1 - position1);
Rvs94 24:d0af4b2be295 59 scope.set(3, position1);
Margreeth95 0:284ed397e046 60 scope.send();
Rvs94 1:48aba8d5610a 61
Margreeth95 0:284ed397e046 62 }
Rvs94 12:69ab81cf5b7d 63
Rvs94 12:69ab81cf5b7d 64 // Biquad filter
Rvs94 12:69ab81cf5b7d 65 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
Rvs94 9:774fc3c6a39e 66 {
Rvs94 13:a6770307a5d2 67 double v = u - a1*v1 - a2*v2;
Rvs94 13:a6770307a5d2 68 double y = b0*v + b1*v1 + b2*v2;
Rvs94 12:69ab81cf5b7d 69 v2 = v1; v1 = v;
Rvs94 12:69ab81cf5b7d 70 return y;
Rvs94 12:69ab81cf5b7d 71 }
Rvs94 12:69ab81cf5b7d 72
Rvs94 12:69ab81cf5b7d 73
Rvs94 12:69ab81cf5b7d 74 // Reusable PID controller
Rvs94 12:69ab81cf5b7d 75 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 13:a6770307a5d2 76 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 12:69ab81cf5b7d 77 {
Rvs94 12:69ab81cf5b7d 78 // Derivative
Rvs94 12:69ab81cf5b7d 79 double e_der = (e-e_prev)/Ts;
Rvs94 12:69ab81cf5b7d 80 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 13:a6770307a5d2 81 e_prev = e;
Rvs94 12:69ab81cf5b7d 82 // Integral
Rvs94 12:69ab81cf5b7d 83 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 84 // PID
Rvs94 12:69ab81cf5b7d 85 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 9:774fc3c6a39e 86 }
Margreeth95 0:284ed397e046 87
Margreeth95 18:6f71bb91b8bd 88 // Motor2 control
Rvs94 11:0793a78109a2 89 void motor2_Controller()
Rvs94 9:774fc3c6a39e 90 {
Rvs94 24:d0af4b2be295 91 reference2 = m2_ref; // Setpoint motor 2
Rvs94 24:d0af4b2be295 92 position2 = Encoder2.getPulses()*360/(32*131); // Aantal Degs motor 2
Rvs94 24:d0af4b2be295 93 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 12:69ab81cf5b7d 94 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 24:d0af4b2be295 95 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);
Rvs94 24:d0af4b2be295 96 motor2speed = abs(m2_P2); // Speed control
Rvs94 24:d0af4b2be295 97 if(m2_P2 > 0) // Direction control
Rvs94 9:774fc3c6a39e 98 {
Rvs94 9:774fc3c6a39e 99 motor2direction = 0;
Rvs94 9:774fc3c6a39e 100 }
Rvs94 9:774fc3c6a39e 101 else
Rvs94 9:774fc3c6a39e 102 {
Rvs94 9:774fc3c6a39e 103 motor2direction = 1;
Rvs94 9:774fc3c6a39e 104 }
Rvs94 24:d0af4b2be295 105 reference1 = m1_ref; // Setpoint
Rvs94 24:d0af4b2be295 106 position1 = Encoder1.getPulses()*360/(32*131); // Aantal Degs
Rvs94 24:d0af4b2be295 107 double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,
Rvs94 24:d0af4b2be295 108 m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilerd met dezelfde coeffs als motor 2
Rvs94 24:d0af4b2be295 109 double m1_P2 = biquad(m1_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilterd met dezelfde coeffs als motor 2
Rvs94 24:d0af4b2be295 110 motor2speed = abs(m1_P2); // Speed control
Rvs94 24:d0af4b2be295 111 if(m1_P2 > 0) // Direction control
Rvs94 24:d0af4b2be295 112 {
Rvs94 24:d0af4b2be295 113 motor1direction = 0;
Rvs94 24:d0af4b2be295 114 }
Rvs94 24:d0af4b2be295 115 else
Rvs94 24:d0af4b2be295 116 {
Rvs94 24:d0af4b2be295 117 motor1direction = 1;
Rvs94 24:d0af4b2be295 118 }
Rvs94 9:774fc3c6a39e 119 }
Rvs94 3:687729d7996e 120
Rvs94 24:d0af4b2be295 121
Rvs94 24:d0af4b2be295 122
Rvs94 24:d0af4b2be295 123
Margreeth95 0:284ed397e046 124 int main()
Rvs94 9:774fc3c6a39e 125 {
Rvs94 20:f5091e29cd26 126 LedR.write(1);
Rvs94 20:f5091e29cd26 127 LedB.write(1);
Rvs94 20:f5091e29cd26 128 LedG.write(1);
Margreeth95 0:284ed397e046 129 pc.baud(115200);
Rvs94 3:687729d7996e 130 pc.printf("Tot aan loop werkt\n");
Rvs94 9:774fc3c6a39e 131
Margreeth95 0:284ed397e046 132 ScopeTime.attach_us(&ScopeSend, 10e4);
Rvs94 24:d0af4b2be295 133 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz
Rvs94 24:d0af4b2be295 134
Rvs94 9:774fc3c6a39e 135 while(true)
Rvs94 20:f5091e29cd26 136 {
Rvs94 20:f5091e29cd26 137 char c = pc.getc();
Rvs94 20:f5091e29cd26 138 if(c == 'e') //Ga 1 programma omhoog
Rvs94 20:f5091e29cd26 139 {
Rvs94 20:f5091e29cd26 140 count = count + 1;
Rvs94 20:f5091e29cd26 141 if(count > 2)
Rvs94 20:f5091e29cd26 142 {
Rvs94 20:f5091e29cd26 143 count = 2;
Rvs94 20:f5091e29cd26 144 }
Rvs94 20:f5091e29cd26 145
Rvs94 20:f5091e29cd26 146 }
Rvs94 20:f5091e29cd26 147 if(c == 'd') //Ga 1 programma omlaag
Rvs94 20:f5091e29cd26 148 {
Rvs94 20:f5091e29cd26 149 count = count - 1;
Rvs94 20:f5091e29cd26 150 if(count < 0)
Rvs94 20:f5091e29cd26 151 {
Rvs94 20:f5091e29cd26 152 count = 0;
Rvs94 20:f5091e29cd26 153 }
Rvs94 20:f5091e29cd26 154 }
Rvs94 24:d0af4b2be295 155 if(count == 0) //Motor 2 control, Groene LED
Rvs94 20:f5091e29cd26 156 {
Rvs94 20:f5091e29cd26 157
Rvs94 20:f5091e29cd26 158 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 159 LedG = 0;
Margreeth95 19:9417d2011e8b 160 if(c == 'r')
Margreeth95 19:9417d2011e8b 161 {
Rvs94 24:d0af4b2be295 162 m2_ref = m2_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 163 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 164 {
Rvs94 24:d0af4b2be295 165 m2_ref = Grens2;
Margreeth95 19:9417d2011e8b 166 }
Margreeth95 19:9417d2011e8b 167 }
Margreeth95 19:9417d2011e8b 168 if(c == 'f')
Margreeth95 19:9417d2011e8b 169 {
Rvs94 24:d0af4b2be295 170 m2_ref = m2_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 171 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 172 {
Rvs94 24:d0af4b2be295 173 m2_ref = -1*Grens2;
Margreeth95 19:9417d2011e8b 174 }
Margreeth95 19:9417d2011e8b 175 }
Rvs94 20:f5091e29cd26 176 }
Rvs94 24:d0af4b2be295 177 if(count == 1) //Motor 1 control, Rode LED
Rvs94 20:f5091e29cd26 178 {
Rvs94 20:f5091e29cd26 179 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 180 LedR = 0;
Rvs94 24:d0af4b2be295 181 if(c == 't')
Rvs94 24:d0af4b2be295 182 {
Rvs94 24:d0af4b2be295 183 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 184 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 185 {
Rvs94 24:d0af4b2be295 186 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 187 }
Rvs94 24:d0af4b2be295 188 }
Rvs94 24:d0af4b2be295 189 if(c == 'g')
Rvs94 24:d0af4b2be295 190 {
Rvs94 24:d0af4b2be295 191 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 192 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 193 {
Rvs94 24:d0af4b2be295 194 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 195 }
Rvs94 24:d0af4b2be295 196 }
Rvs94 20:f5091e29cd26 197 }
Rvs94 24:d0af4b2be295 198 if(count == 2) //Vuur mechanisme & Reset, Blauwe LED
Rvs94 20:f5091e29cd26 199 {
Rvs94 20:f5091e29cd26 200
Rvs94 20:f5091e29cd26 201 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 202 LedB = 0;
Rvs94 24:d0af4b2be295 203 //VUUUR!
Rvs94 24:d0af4b2be295 204 wait(1);
Rvs94 24:d0af4b2be295 205 m2_ref = 0;
Rvs94 24:d0af4b2be295 206 m1_ref = 0;
Rvs94 20:f5091e29cd26 207 }
Margreeth95 0:284ed397e046 208 }
Rvs94 9:774fc3c6a39e 209
Margreeth95 0:284ed397e046 210 }