![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Motor programma
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
Diff: main.cpp
- Revision:
- 24:d0af4b2be295
- Parent:
- 20:f5091e29cd26
- Child:
- 25:ae908de29943
--- a/main.cpp Fri Oct 02 17:22:17 2015 +0000 +++ b/main.cpp Sat Oct 03 15:59:25 2015 +0000 @@ -5,8 +5,9 @@ #include "biquadFilter.h" Serial pc(USBTX, USBRX); // tx, rx -QEI Encoder(D3, D2, NC, 128); -HIDScope scope(3); +QEI Encoder2(D3, D2, NC, 32); +QEI Encoder1(D13,D12,NC, 32); +HIDScope scope(4); //Ledjes DigitalOut LedR(LED_RED); @@ -14,26 +15,32 @@ DigitalOut LedB(LED_BLUE); //Motor -DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield) +DigitalOut motor1direction(D6); +PwmOut motor1speed(D7); +DigitalOut motor2direction(D4); PwmOut motor2speed(D5); //Tickers Ticker ScopeTime; -Ticker myControllerTicker; -Ticker PrintTime; +Ticker myControllerTicker2; //Startwaarden -double reference; -double position; -double m2_ref = 0; +double reference2, reference1; +double position2 = 0, position1 = 0; +double m2_ref = 0, m1_ref = 0; int count = 0; +double Grens2 = 90, Grens1 = 90; +double Stapgrootte = 5; //Sample time (motor2-step) const double m2_Ts = 0.01; +const double m1_Ts = 0.01; -//Controller gain Motor 2 -const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5; +//Controller gain Motor +const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2; +const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2; double m2_err_int = 0, m2_prev_err = 0; +double m1_err_int = 0, m1_prev_err = 0; //Derivative filter coeffs Motor 2 const double BiGain = 0.016955; @@ -46,20 +53,14 @@ //HIDScope void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt { - scope.set(0, motor2direction.read()); - scope.set(1, motor2speed.read()); - scope.set(2, position); - + scope.set(0, reference2 - position2); + scope.set(1, position2); + scope.set(2, reference1 - position1); + scope.set(3, position1); scope.send(); } -// PC Printf -void PCPrintf() -{ - pc.printf("position = %f aantal degs = %f, count = %i\n",reference,position,count); -} - // Biquad filter double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 ) { @@ -87,13 +88,13 @@ // Motor2 control void motor2_Controller() { - reference = m2_ref; // Setpoint - double pulses = Encoder.getPulses(); - position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs - double P2 = PID( reference - position, 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, + reference2 = m2_ref; // Setpoint motor 2 + position2 = Encoder2.getPulses()*360/(32*131); // Aantal Degs motor 2 + 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, m2_f_b0, m2_f_b1, m2_f_b2); - motor2speed = abs(P2); // Speed control - if(P2 > 0) // Direction control + 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); + motor2speed = abs(m2_P2); // Speed control + if(m2_P2 > 0) // Direction control { motor2direction = 0; } @@ -101,9 +102,25 @@ { motor2direction = 1; } - + reference1 = m1_ref; // Setpoint + position1 = Encoder1.getPulses()*360/(32*131); // Aantal Degs + 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, + m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilerd met dezelfde coeffs als motor 2 + 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 + motor2speed = abs(m1_P2); // Speed control + if(m1_P2 > 0) // Direction control + { + motor1direction = 0; + } + else + { + motor1direction = 1; + } } + + + int main() { LedR.write(1); @@ -113,8 +130,8 @@ pc.printf("Tot aan loop werkt\n"); ScopeTime.attach_us(&ScopeSend, 10e4); - myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz - PrintTime.attach(&PCPrintf,0.1f); + myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz + while(true) { char c = pc.getc(); @@ -135,38 +152,58 @@ count = 0; } } - if(count == 0) //Motor 1 control + if(count == 0) //Motor 2 control, Groene LED { LedR = LedB = 1; LedG = 0; if(c == 'r') { - m2_ref = m2_ref + 5; - if (m2_ref > 90) + m2_ref = m2_ref + Stapgrootte; + if (m2_ref > Grens2) { - m2_ref = 90; + m2_ref = Grens2; } } if(c == 'f') { - m2_ref = m2_ref - 5; - if (m2_ref < -90) + m2_ref = m2_ref - Stapgrootte; + if (m2_ref < -1*Grens2) { - m2_ref = -90; + m2_ref = -1*Grens2; } } } - if(count == 1) //Motor 2 control + if(count == 1) //Motor 1 control, Rode LED { LedG = LedB = 1; LedR = 0; + if(c == 't') + { + m1_ref = m1_ref + Stapgrootte; + if (m1_ref > Grens1) + { + m1_ref = Grens1; + } + } + if(c == 'g') + { + m1_ref = m1_ref - Stapgrootte; + if (m1_ref < -1*Grens1) + { + m1_ref = -1*Grens1; + } + } } - if(count == 2) //Vuur mechanisme + if(count == 2) //Vuur mechanisme & Reset, Blauwe LED { LedR = LedG = 1; - LedB = 0; + LedB = 0; + //VUUUR! + wait(1); + m2_ref = 0; + m1_ref = 0; } }