werkende PID, int/double probleem met encoder opgelost.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
main.cpp
- Committer:
- laura94
- Date:
- 2015-10-21
- Revision:
- 26:70e5b6908e0a
- Parent:
- 25:ae908de29943
File content as of revision 26:70e5b6908e0a:
//--------------------------------------------------------------------------------------------------------------------------// // 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.001, m1_Ts = 0.01; //Controller gain Motor 2 & 1 const double m2_Kp = 2.1/57,m2_Ki = 3.9/57, m2_Kd = 0.1/57; 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; const double BiGainEMG_L1= 0.231938; 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 biquadFilter m2_lowpass (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); double x; //--------------------------------------------------------------------------------------------------------------------------// // 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.set(4, x); scope.send(); } // Reusable PID controller double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev) { // Derivative double e_der = (e-e_prev)/Ts; e_der = m2_lowpass.step(e_der); e_prev = e; // Integral e_int = e_int + Ts*e; // PID x = Kp * e + Ki*e_int + Kd*e_der; return x; } //--------------------------------------------------------------------------------------------------------------------------// // Motor control functions //--------------------------------------------------------------------------------------------------------------------------// // Motor2 control void motor2_Controller() { // Setpoint motor 2 reference2 = m2_ref; // Reference in degrees position2 = 1.0*Encoder2.getPulses()*360.0/(32.0*131.0); // 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); double m2_P2 = m2_P1; // 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); double m1_P2 = m1_P1; 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, m2_Ts ); // 1000 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; } } }