Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 7:757e95b4dc46
- Parent:
- 5:0d3e8694726e
- Child:
- 8:78d8ccf84a38
--- a/main.cpp Fri Oct 20 11:04:26 2017 +0000 +++ b/main.cpp Mon Oct 23 09:42:21 2017 +0000 @@ -8,51 +8,68 @@ const double pi = 3.1415926535897; // Definition of pi -// SERIAL COMMUNICATION WITH PC +// SERIAL COMMUNICATION WITH PC //////////////////////////////////////////////// MODSERIAL pc(USBTX, USBRX); -// STATES +// STATES ////////////////////////////////////////////////////////////////////// enum states{MOTORS_OFF, MOVING, HITTING}; states currentState = MOTORS_OFF; // Start with motors off bool stateChanged = true; // Make sure the initialization of first state is executed -// ENCODER +// ENCODER ///////////////////////////////////////////////////////////////////// QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13 -// PINS +// PINS //////////////////////////////////////////////////////////////////////// DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW PwmOut motor1MagnitudePin(D5); DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW? PwmOut motor2MagnitudePin(D6); InterruptIn button1(D2); // CONNECT BUT1 TO D2 InterruptIn button2(D3); // CONNECT BUT2 TO D3 +AnalogIn emg(A0); -// MOTOR CONTROL +// MOTOR CONTROL /////////////////////////////////////////////////////////////// Ticker controllerTicker; const double controller_Ts = 1/5000; // Time step for controllerTicker; Should be between 5kHz and 10kHz [s] const double motorRatio = 131.25; // Ratio of the gearbox in the motors [] -const double radPerPulse = 2*pi/(32*motorRatio); -// Motor 1 +const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse] + +// MOTOR 1 +double reference1 = 0.0; // Desired rotation of motor 1 [rad] // Controller gains const double motor1_KP = 2.5; // Position gain [] const double motor1_KI = 1.0; // Integral gain [] const double motor1_KD = 0.5; // Derivative gain [] double motor1_err_int = 0, motor1_prev_err = 0; // Derivative filter coefficients -const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0; -const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0; +const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0; // Derivative filter coefficients [] +const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0; // Derivative filter coefficients [] // Filter variables double motor1_f_v1 = 0, motor1_f_v2 = 0; -double reference1 = 0.0; // Wanted rotation of motor 1 [rad] -// Motor 2 + +// MOTOR 2 // Controller gains // Derivative filter coefficients // Filter variables -// FUNCTIONS -// BIQUAD FILTER FOR DERIVATIVE OF ERROR +// EMG FILTER ////////////////////////////////////////////////////////////////// +BiQuadChain bqc; +BiQuad bq1(0, 0, 0, 0, 0); // EMG filter coefficients [] +BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients [] +Ticker emgSampleTicker; +double emg_Ts = 0.01; // Time step for EMG sampling + + +// FUNCTIONS /////////////////////////////////////////////////////////////////// +// EMG ///////////////////////////////////////////////////////////////////////// +void EmgSample() +{ + double emgFiltered = bqc.step(emg.read()); // Filters the EMG signal +} + +// BIQUAD FILTER FOR DERIVATIVE OF ERROR /////////////////////////////////////// double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) { @@ -62,29 +79,29 @@ return y; } -// P-CONTROLLER +// P-CONTROLLER //////////////////////////////////////////////////////////////// double P_Controller(double error, const double Kp) { return Kp * error; } -// PID-CONTROLLER WITH FILTER +// PID-CONTROLLER WITH FILTER ////////////////////////////////////////////////// double PID_Controller(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); + double e_der = (e - e_prev)/Ts; // Calculates the derivative of error + e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error e_prev = e; // Integral - e_int = e_int + Ts*e; + e_int = e_int + Ts*e; // Calculates the integral of error // PID return Kp*e + Ki*e_int + Kd*e_der; } -// MOTOR 1 +// MOTOR 1 ///////////////////////////////////////////////////////////////////// void RotateMotor1(double motor1Value) { if(currentState == MOVING) // Checks if state is MOVING @@ -98,7 +115,7 @@ else motor1MagnitudePin = 0; } -// MOTOR 1 P-CONTROLLER +// MOTOR 1 P-CONTROLLER //////////////////////////////////////////////////////// void Motor1PController() { double position1 = radPerPulse * Encoder1.getPulses(); // Calculates the rotation of motor 1 @@ -106,7 +123,7 @@ RotateMotor1(motor1Value); } -// MOTOR 1 PID-CONTROLLER +// MOTOR 1 PID-CONTROLLER ////////////////////////////////////////////////////// void Motor1Controller() { double position1 = radPerPulse * Encoder1.getPulses(); @@ -117,14 +134,14 @@ RotateMotor1(motor1Value); } -// TURN OFF MOTORS +// TURN OFF MOTORS ///////////////////////////////////////////////////////////// void TurnMotorsOff() { motor1MagnitudePin = 0; motor2MagnitudePin = 0; } -// STATES +// STATES ////////////////////////////////////////////////////////////////////// void ProcessStateMachine() { switch(currentState) @@ -134,7 +151,7 @@ // State initialization if(stateChanged) { - pc.printf("Entering MOTORS_OFF \r\n"); + pc.printf("Entering MOTORS_OFF \r\n Press button 1 to enter MOVING \r\n"); TurnMotorsOff(); // Turn motors off stateChanged = false; } @@ -154,12 +171,12 @@ // State initialization if(stateChanged) { - pc.printf("Entering MOVING \r\n"); + pc.printf("Entering MOVING \r\n Press button 2 to enter HITTING \r\n"); stateChanged = false; } // Hit command - if(!button1) + if(!button2) { currentState = HITTING; stateChanged = true; @@ -192,7 +209,7 @@ } } -// MAIN FUNCTION +// MAIN FUNCTION /////////////////////////////////////////////////////////////// int main() { // Serial communication @@ -200,8 +217,11 @@ pc.printf("START \r\n"); + bqc.add(&bq1).add(&bq2); + controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1 //controllerTicker.attach(&Motor1Controller, controller_Ts); + emgSampleTicker.attach(&EmgSample, emg_Ts); while(true) {