Werkend aansturingsscript voor 2 motoren, incl werkende program switch. Motoren oscilleren nog iets. Vuur mechanisme ontbreekt nog.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
main.cpp
- Committer:
- Rvs94
- Date:
- 2015-10-03
- Revision:
- 25:ae908de29943
- Parent:
- 24:d0af4b2be295
- Child:
- 26:58c3d24b29d2
- Child:
- 27:4d7ca91e2e64
File content as of revision 25:ae908de29943:
//--------------------------------------------------------------------------------------------------------------------------// // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7 //--------------------------------------------------------------------------------------------------------------------------// // Libraries //--------------------------------------------------------------------------------------------------------------------------// #include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "biquadFilter.h" //--------------------------------------------------------------------------------------------------------------------------// // Constanten/Inputs/Outputs //--------------------------------------------------------------------------------------------------------------------------// MODSERIAL pc(USBTX, USBRX); // To/From PC QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2 QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1 HIDScope scope(4); // Scope, 4 channels // LEDs DigitalOut LedR(LED_RED); DigitalOut LedG(LED_GREEN); DigitalOut LedB(LED_BLUE); // Motor DigitalOut motor1direction(D7); // Motor 1, Direction & Speed PwmOut motor1speed(D6); DigitalOut motor2direction(D4); // Motor 2, Direction & Speed PwmOut motor2speed(D5); // Tickers Ticker ScopeTime; Ticker myControllerTicker2; Ticker myControllerTicker1; // Constants 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 (motor-step) const double m2_Ts = 0.01, m1_Ts = 0.01; //Controller gain Motor 2 & 1 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 & 1 const double BiGain2 = 0.016955, BiGain1 = 0.016955; 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; 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; // Filter variables double m2_f_v1 = 0, m2_f_v2 = 0; double m1_f_v1 = 0, m1_f_v2 = 0; //--------------------------------------------------------------------------------------------------------------------------// // General Functions //--------------------------------------------------------------------------------------------------------------------------// //HIDScope void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt { scope.set(0, reference2 - position2); scope.set(1, position2); scope.set(2, reference1 - position1); scope.set(3, position1); scope.send(); } // 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 ) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } // Reusable PID controller 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, const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2) { // Derivative double e_der = (e-e_prev)/Ts; e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); e_prev = e; // Integral e_int = e_int + Ts*e; // PID return Kp * e + Ki*e_int + Kd*e_der; } //--------------------------------------------------------------------------------------------------------------------------// // Motor control functions //--------------------------------------------------------------------------------------------------------------------------// // Motor2 control void motor2_Controller() { // Setpoint motor 2 reference2 = m2_ref; // Reference in degrees position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees // Speed control 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); 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 motor2speed = abs(m2_P2); // Direction control if(m2_P2 > 0) { motor2direction = 0; } else { motor2direction = 1; } } // Motor1 control void motor1_Controller() { // Setpoint Motor 1 reference1 = m1_ref; // Reference in degrees position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees // Speed control 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, m1_f_b0, m1_f_b1, m1_f_b2); 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); motor1speed = abs(m1_P2); // Direction control if(m1_P2 > 0) { motor1direction = 1; } else { motor1direction = 0; } } //--------------------------------------------------------------------------------------------------------------------------// // Main function //--------------------------------------------------------------------------------------------------------------------------// int main() { //--------------------------------------------------------------------------------------------------------------------------// // Initalizing //--------------------------------------------------------------------------------------------------------------------------// //LEDs OFF LedR = LedB = LedG = 1; //PC connection & check pc.baud(115200); pc.printf("Tot aan loop werkt\n"); // Tickers ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1 //--------------------------------------------------------------------------------------------------------------------------// // Control Program //--------------------------------------------------------------------------------------------------------------------------// while(true) { char c = pc.getc(); // 1 Program UP if(c == 'e') { count = count + 1; if(count > 2) { count = 2; } } // 1 Program DOWN if(c == 'd') { count = count - 1; if(count < 0) { count = 0; } } // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED if(count == 0) { LedR = LedB = 1; LedG = 0; if(c == 'r') { m2_ref = m2_ref + Stapgrootte; m1_ref = m1_ref - Stapgrootte; if (m2_ref > Grens2) { m2_ref = Grens2; m1_ref = -1*Grens1; } } if(c == 'f') { m2_ref = m2_ref - Stapgrootte; m1_ref = m1_ref + Stapgrootte; if (m2_ref < -1*Grens2) { m2_ref = -1*Grens2; m1_ref = Grens1; } } } // PROGRAM 1: Motor 1 control, Red LED if(count == 1) { 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; } } } // PROGRAM 2: Firing mechanism & Reset, Blue LED if(count == 2) { LedR = LedG = 1; LedB = 0; //VUUUUR!! (To Do) wait(1); m2_ref = 0; m1_ref = 0; } } }