Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 11:5c06c97c3673
- Parent:
- 10:a9e344e440b8
- Child:
- 12:12b72bd60fd1
--- a/main.cpp Fri Oct 27 08:43:34 2017 +0000 +++ b/main.cpp Mon Oct 30 12:11:42 2017 +0000 @@ -36,17 +36,18 @@ const double controller_Ts = 0.01; // 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 +double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start +double velocity = 0.3; // X and Y velocity of the end effector when desired // MOTOR 1 double position1; // Position of motor 1 [rad] -volatile double reference1 = 0; // Desired rotation of motor 1 [rad] +double reference1 = 0; // Desired rotation of motor 1 [rad] double motor1Max = 2*pi*45/360; // Maximum rotation of motor 1 [rad] double motor1Min = 0; // Minimum rotation of motor 1 [rad] // Controller gains const double motor1_KP = 5; // Position gain [] -const double motor1_KI = 5; // Integral gain [] -const double motor1_KD = 0.0; // Derivative gain [] +const double motor1_KI = 5; // Integral gain [] +const double motor1_KD = 0.0; // 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; // Derivative filter coefficients [] @@ -61,8 +62,8 @@ double motor2Min = 2*pi*-45/360; // Minimum rotation of motor 2 [rad] // Controller gains const double motor2_KP = 3; // Position gain [] -const double motor2_KI = 3; // Integral gain [] -const double motor2_KD = 0.0; // Derivative gain [] +const double motor2_KI = 3; // Integral gain [] +const double motor2_KD = 0.0; // Derivative gain [] double motor2_err_int = 0, motor2_prev_err = 0; // Derivative filter coefficients const double motor2_f_a1 = 1.0, motor2_f_a2 = 2.0; // Derivative filter coefficients [] @@ -89,12 +90,6 @@ return y; } -// P-CONTROLLER //////////////////////////////////////////////////////////////// -double P_Controller(double error, const double Kp) -{ - return Kp * error; -} - // 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, @@ -103,14 +98,14 @@ { // Derivative double e_der = (e - e_prev)/Ts; // Calculate the derivative of error - //pc.printf("der error %f \r\n", e_der); + //pc.printf("derivative error %f \r\n", e_der); //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; // Calculate the integral of error // PID - //pc.printf("NAAA der error %f \r\n", e_der); - return -(Kp*e + Ki*e_int + Kd*e_der); + //pc.printf("Na derivative error %f \r\n", e_der); + return Kp*e + Ki*e_int + Kd*e_der; } // MOTOR 1 ///////////////////////////////////////////////////////////////////// @@ -141,15 +136,6 @@ else motor2MagnitudePin = 0; } -// MOTOR 1 P-CONTROLLER //////////////////////////////////////////////////////// -void Motor1PController() -{ - position1 = radPerPulse * Encoder1.getPulses(); // Calculate the rotation of motor 1 - //pc.printf("%f", position1); - double motor1Value = P_Controller(reference1 - position1, motor1_KP); - RotateMotor1(motor1Value); -} - // MOTOR 1 PID-CONTROLLER ////////////////////////////////////////////////////// void Motor1Controller() { @@ -189,13 +175,12 @@ position1 = radPerPulse * Encoder1.getPulses(); position2 = radPerPulse * Encoder2.getPulses(); - - if(!button1 ) xVelocity = 0.3;//&& position1 > motor1Min && position2 > motor2Min - else if(!button2 ) xVelocity = -0.3;//&& position1 < motor1Max && position2 < motor2Max + if(!button1) xVelocity = velocity;//&& position1 > motor1Min && position2 > motor2Min + else if(!button2) xVelocity = -velocity;//&& position1 < motor1Max && position2 < motor2Max else xVelocity = 0; - if(!button3 ) yVelocity = 0.3;//&& position1 < motor1Max && position2 < motor2Max - else if(!button4 ) yVelocity = -0.3;//&& position1 < motor1Max && position2 > motor2Min + if(!button3) yVelocity = velocity;//&& position1 < motor1Max && position2 < motor2Max + else if(!button4) yVelocity = -velocity;//&& position1 < motor1Max && position2 > motor2Min else yVelocity = 0; //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity); @@ -284,8 +269,9 @@ //double emgFiltered = bqc.step(emg.read()); // Filter the EMG signal /* If EMG signal 1 --> xVelocity / yVelocity = velocity - Else if EMG signal 2 --> xVelocity / yVelocity = velocity + Else if EMG signal 2 --> xVelocity / yVelocity = -velocity Else xVelocity / yVelocity = 0 + If both signal 1 and 2 --> Switch */ jointVelocities(); } @@ -371,8 +357,7 @@ bqc.add(&bq1).add(&bq2); // Make BiQuad Chain - //controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1 (P) - //controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID) + //controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID) emgSampleTicker.attach(&EmgSample, emg_Ts); // Ticker to sample EMG while(true)