Jordi Luong
Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 10:a9e344e440b8
- Parent:
- 8:78d8ccf84a38
- Child:
- 11:5c06c97c3673
diff -r 78d8ccf84a38 -r a9e344e440b8 main.cpp
--- a/main.cpp Mon Oct 23 12:25:07 2017 +0000
+++ b/main.cpp Fri Oct 27 08:43:34 2017 +0000
@@ -27,20 +27,26 @@
PwmOut motor2MagnitudePin(D6);
InterruptIn button1(D2); // CONNECT BUT1 TO D2
InterruptIn button2(D3); // CONNECT BUT2 TO D3
+InterruptIn button3(SW2);
+InterruptIn button4(SW3);
AnalogIn emg(A0);
// MOTOR CONTROL ///////////////////////////////////////////////////////////////
Ticker controllerTicker;
-const double controller_Ts = 1/5000; // Time step for controllerTicker; Should be between 5kHz and 10kHz [s]
+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
// MOTOR 1
-double reference1 = 0.0; // Desired rotation of motor 1 [rad]
+double position1; // Position of motor 1 [rad]
+volatile 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 = 2.5; // Position gain []
-const double motor1_KI = 1.0; // Integral gain []
-const double motor1_KD = 0.5; // Derivative gain []
+const double motor1_KP = 5; // Position 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 []
@@ -49,26 +55,30 @@
double motor1_f_v1 = 0, motor1_f_v2 = 0;
// MOTOR 2
+double position2; // Position of motor 2 [rad]
+volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
+double motor2Max = 2*pi*45/360; // Maximum rotation of motor 2 [rad]
+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 []
+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 []
+const double motor2_f_b0 = 1.0, motor2_f_b1 = 3.0, motor2_f_b2 = 4.0; // Derivative filter coefficients []
// Filter variables
-
+double motor2_f_v1 = 0, motor2_f_v2 = 0;
// 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
+double emg_Ts = 0.1; // Time step for EMG sampling [s]
// FUNCTIONS ///////////////////////////////////////////////////////////////////
-// EMG /////////////////////////////////////////////////////////////////////////
-void EmgSample()
-{
- double emgFiltered = bqc.step(emg.read()); // Filter 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)
@@ -93,21 +103,23 @@
{
// Derivative
double e_der = (e - e_prev)/Ts; // Calculate 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
+ //pc.printf("der 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
- return Kp*e + Ki*e_int + Kd*e_der;
+ //pc.printf("NAAA der error %f \r\n", e_der);
+ return -(Kp*e + Ki*e_int + Kd*e_der);
}
// MOTOR 1 /////////////////////////////////////////////////////////////////////
void RotateMotor1(double motor1Value)
{
- if(currentState == MOVING) // Check if state is MOVING
+ if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
{
- if(motor1Value >= 0) motor1DirectionPin = 1; // Rotate motor 1 CW
- else motor1DirectionPin = 0; // Rotate motor 1 CCW
+ if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
+ else motor1DirectionPin = 1; // Rotate motor 1 CCW
if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
else motor1MagnitudePin = fabs(motor1Value);
@@ -115,10 +127,25 @@
else motor1MagnitudePin = 0;
}
+// MOTOR 2 /////////////////////////////////////////////////////////////////////
+void RotateMotor2(double motor2Value)
+{
+ if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
+ {
+ if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW
+ else motor2DirectionPin = 0; // Rotate motor 1 CCW
+
+ if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
+ else motor2MagnitudePin = fabs(motor2Value);
+ }
+ else motor2MagnitudePin = 0;
+}
+
// MOTOR 1 P-CONTROLLER ////////////////////////////////////////////////////////
void Motor1PController()
{
- double position1 = radPerPulse * Encoder1.getPulses(); // Calculate the rotation of motor 1
+ position1 = radPerPulse * Encoder1.getPulses(); // Calculate the rotation of motor 1
+ //pc.printf("%f", position1);
double motor1Value = P_Controller(reference1 - position1, motor1_KP);
RotateMotor1(motor1Value);
}
@@ -126,14 +153,29 @@
// MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
void Motor1Controller()
{
- double position1 = radPerPulse * Encoder1.getPulses();
+ position1 = radPerPulse * Encoder1.getPulses();
+ pc.printf("error %f \r\n", reference1 - position1);
double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
motor1_f_b1, motor1_f_b2);
+ //pc.printf("motor value %f \r\n",motor1Value);
RotateMotor1(motor1Value);
}
+// MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
+void Motor2Controller()
+{
+ position2 = radPerPulse * Encoder2.getPulses();
+ //pc.printf("%f", position2);
+ double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
+ motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err,
+ motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
+ motor2_f_b1, motor2_f_b2);
+ pc.printf("motor value %f \r\n",motor2Value);
+ RotateMotor2(motor2Value);
+}
+
// TURN OFF MOTORS /////////////////////////////////////////////////////////////
void TurnMotorsOff()
{
@@ -141,6 +183,114 @@
motor2MagnitudePin = 0;
}
+// DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
+void jointVelocities()
+{
+ 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
+ else xVelocity = 0;
+
+ if(!button3 ) yVelocity = 0.3;//&& position1 < motor1Max && position2 < motor2Max
+ else if(!button4 ) yVelocity = -0.3;//&& position1 < motor1Max && position2 > motor2Min
+ else yVelocity = 0;
+
+ //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
+
+ // Construct Jacobian
+ double q[4];
+ q[0] = position1, q[1] = -position1;
+ q[2] = position2, q[3] = -position2;
+
+ double T2[3]; // Second column of the jacobian
+ double T3[3]; // Third column of the jacobian
+ double T4[3]; // Fourth column of the jacobian
+ double T1[6];
+ static const signed char b_T1[3] = { 1, 0, 0 };
+ double J_data[6];
+
+ T2[0] = 1.0;
+ T2[1] = 0.365 * cos(q[0]);
+ T2[2] = 0.365 * sin(q[0]);
+ T3[0] = 1.0;
+ T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
+ q[0]) + q[1]);
+ T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
+ q[0]) + q[1]);
+ T4[0] = 1.0;
+ T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
+ q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]);
+ T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
+ q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]);
+
+ for (int i = 0; i < 3; i++)
+ {
+ T1[i] = (double)b_T1[i] - T2[i];
+ T1[3 + i] = T3[i] - T4[i];
+ }
+
+ for (int i = 0; i < 2; i++)
+ {
+ for (int j = 0; j < 3; j++)
+ {
+ J_data[j + 3 * i] = T1[j + 3 * i];
+ }
+ }
+
+ // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
+ // Note: the matrices from now on will we constructed rowwise
+ double Jvelocity[4];
+ Jvelocity[0] = J_data[1];
+ Jvelocity[1] = J_data[4];
+ Jvelocity[2] = J_data[2];
+ Jvelocity[3] = J_data[5];
+
+ // Creating the inverse Jacobian
+ double Jvelocity_inv[4]; // The inverse matrix of the jacobian
+ double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
+ Jvelocity_inv[0] = Jvelocity[3]/determ;
+ Jvelocity_inv[1] = -Jvelocity[1]/determ;
+ Jvelocity_inv[2] = -Jvelocity[2]/determ;
+ Jvelocity_inv[3] = Jvelocity[0]/determ;
+
+ // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
+ double msh[2]; // This is the velocity the joints have to have
+ msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
+ msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
+
+ if(position1 + msh[0]*emg_Ts > motor1Max) reference1 = motor1Max;
+ else if(position1 + msh[0]*emg_Ts < motor1Min) reference1 = motor1Min;
+ else reference1 = position1 + msh[0]*emg_Ts;
+
+ if(position2 + msh[1]*emg_Ts > motor2Max) reference2 = motor2Max;
+ else if(position2 + msh[1]*emg_Ts < motor2Min) reference2 = motor2Min;
+ else reference2 = position2 + msh[1]*emg_Ts;
+
+ Motor1Controller(), Motor2Controller();
+
+ pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
+ 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()
+{
+ if(currentState == MOVING)
+ {
+ //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 xVelocity / yVelocity = 0
+ */
+ jointVelocities();
+ }
+}
+
// STATES //////////////////////////////////////////////////////////////////////
void ProcessStateMachine()
{
@@ -151,7 +301,8 @@
// State initialization
if(stateChanged)
{
- pc.printf("Entering MOTORS_OFF \r\n Press button 1 to enter MOVING \r\n");
+ pc.printf("Entering MOTORS_OFF \r\n"
+ "Press button 1 to enter MOVING \r\n");
TurnMotorsOff(); // Turn motors off
stateChanged = false;
}
@@ -171,18 +322,19 @@
// State initialization
if(stateChanged)
{
- pc.printf("Entering MOVING \r\n Press button 2 to enter HITTING \r\n");
+ pc.printf("Entering MOVING \r\n"
+ "Press button 2 to enter HITTING \r\n");
stateChanged = false;
}
// Hit command
- if(!button2)
+ /*if(!button2)
{
currentState = HITTING;
stateChanged = true;
break;
}
-
+ */
break;
}
@@ -219,8 +371,8 @@
bqc.add(&bq1).add(&bq2); // Make BiQuad Chain
- controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1
- //controllerTicker.attach(&Motor1Controller, controller_Ts);
+ //controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1 (P)
+ //controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
emgSampleTicker.attach(&EmgSample, emg_Ts); // Ticker to sample EMG
while(true)