Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 13:ec227b229f3d
- Parent:
- 12:12b72bd60fd1
- Child:
- 14:95acac6a07c7
- Child:
- 22:68e3dc374488
diff -r 12b72bd60fd1 -r ec227b229f3d main.cpp --- a/main.cpp Wed Nov 01 06:46:29 2017 +0000 +++ b/main.cpp Wed Nov 01 11:25:22 2017 +0000 @@ -5,22 +5,22 @@ #include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" - + const double pi = 3.1415926535897; // Definition of pi - + // SERIAL COMMUNICATION WITH PC //////////////////////////////////////////////// MODSERIAL pc(USBTX, USBRX); HIDScope scope(4); - + // 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 ///////////////////////////////////////////////////////////////////// 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 //////////////////////////////////////////////////////////////////////// DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW PwmOut motor1MagnitudePin(D5); @@ -33,56 +33,58 @@ //AnalogIn emg(A0); AnalogIn potmeter1(A0); AnalogIn potmeter2(A1); - + // MOTOR CONTROL /////////////////////////////////////////////////////////////// Ticker controllerTicker; const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz const double motorRatio = 131.25; // Ratio of the gearbox in the motors [] const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse] double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start -double velocity = 0.07; // X and Y velocity of the end effector when desired - +double velocity = 0.05; // X and Y velocity of the end effector when desired + // MOTOR 1 volatile double position1; // Position of motor 1 [rad] volatile double reference1 = 0; // Desired rotation of motor 1 [rad] double motor1Max = 0; // Maximum rotation of motor 1 [rad] double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad] // Controller gains -const double motor1_KP = 7; // Position gain [] -const double motor1_KI = 9; // Integral gain [] -const double motor1_KD = 1; // Derivative gain [] +const double motor1_KP = 13; // Position gain [] +const double motor1_KI = 7; // Integral gain [] +const double motor1_KD = 0.3; // Derivative gain [] double motor1_err_int = 0, motor1_prev_err = 0; // Derivative filter coefficients const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients [] const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients [] // Filter variables double motor1_f_v1 = 0, motor1_f_v2 = 0; - + // MOTOR 2 volatile double position2; // Position of motor 2 [rad] volatile double reference2 = 0; // Desired rotation of motor 2 [rad] -double motor2Max = 2*pi*30/360; // Maximum rotation of motor 2 [rad] -double motor2Min = 2*pi*-30/360; // Minimum rotation of motor 2 [rad] +double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad] +double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad] // Controller gains -const double motor2_KP = potmeter1*10; // Position gain [] -const double motor2_KI = potmeter2*20; // Integral gain [] -const double motor2_KD = 1; // Derivative gain [] +//const double motor2_KP = potmeter1*10; // Position gain [] +//const double motor2_KI = potmeter2*20; // Integral gain [] +const double motor2_KP = 13; // Position gain [] +const double motor2_KI = 5; // Integral gain [] +const double motor2_KD = 0.1; // Derivative gain [] double motor2_err_int = 0, motor2_prev_err = 0; // Derivative filter coefficients const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients [] const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients [] // Filter variables double motor2_f_v1 = 0, motor2_f_v2 = 0; - + // EMG FILTER ////////////////////////////////////////////////////////////////// //BiQuadChain bqc; //BiQuad bq1(3.6, 5.0, 2.0, 0.5, 30.0); // EMG filter coefficients [] //BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients [] - + Ticker sampleTicker; const double tickerTs = 0.1; // Time step for sampling [s] - - + + // FUNCTIONS /////////////////////////////////////////////////////////////////// // BIQUAD FILTER FOR DERIVATIVE OF ERROR /////////////////////////////////////// double biquad(double u, double &v1, double &v2, const double a1, @@ -93,8 +95,8 @@ v2 = v1; v1 = v; return y; } - - + + // 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, @@ -112,7 +114,7 @@ //pc.printf("%f --> %f \r\n", e_der, e_derf); return Kp*e + Ki*e_int + Kd*e_der; } - + // MOTOR 1 ///////////////////////////////////////////////////////////////////// void RotateMotor1(double motor1Value) { @@ -126,7 +128,7 @@ } else motor1MagnitudePin = 0; } - + // MOTOR 2 ///////////////////////////////////////////////////////////////////// void RotateMotor2(double motor2Value) { @@ -140,7 +142,7 @@ } else motor2MagnitudePin = 0; } - + // MOTOR 1 PID-CONTROLLER ////////////////////////////////////////////////////// void Motor1Controller() { @@ -159,7 +161,7 @@ motor2_f_b1, motor2_f_b2); RotateMotor2(motor2Value); } - + // MOTOR 2 PID-CONTROLLER ////////////////////////////////////////////////////// /*void Motor2Controller() { @@ -177,7 +179,7 @@ motor1MagnitudePin = 0; motor2MagnitudePin = 0; } - + // DETERMINE JOINT VELOCITIES ////////////////////////////////////////////////// void jointVelocities() { @@ -187,7 +189,7 @@ if(!button1 && reference1 > motor1Min && reference2 < motor2Max) xVelocity = velocity; else if(!button2 && reference1 < motor1Max && reference2 > motor2Min) xVelocity = -velocity; else xVelocity = 0; - + if(!button3 && reference2 < motor2Max && reference1 > motor2Min) yVelocity = velocity; else if(!button4 && reference2 > motor2Min && reference1 > motor2Min) yVelocity = -velocity; else yVelocity = 0; @@ -274,7 +276,7 @@ pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360); //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts); } - + // EMG ///////////////////////////////////////////////////////////////////////// void EmgSample() { @@ -290,7 +292,7 @@ jointVelocities(); } } - + // STATES ////////////////////////////////////////////////////////////////////// void ProcessStateMachine() { @@ -360,7 +362,7 @@ } } } - + // MAIN FUNCTION /////////////////////////////////////////////////////////////// int main() {