Jordi Luong
Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 14:95acac6a07c7
- Parent:
- 13:ec227b229f3d
- Child:
- 15:5d24f832bb7b
diff -r ec227b229f3d -r 95acac6a07c7 main.cpp
--- a/main.cpp Wed Nov 01 11:25:22 2017 +0000
+++ b/main.cpp Thu Nov 02 12:19:54 2017 +0000
@@ -7,13 +7,13 @@
#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};
+enum states{MOTORS_OFF, CALIBRATING, MOVING, HITTING};
states currentState = MOTORS_OFF; // Start with motors off
bool stateChanged = true; // Make sure the initialization of first state is executed
@@ -28,62 +28,86 @@
PwmOut motor2MagnitudePin(D6);
InterruptIn button1(D2); // CONNECT BUT1 TO D2
InterruptIn button2(D3); // CONNECT BUT2 TO D3
-InterruptIn button3(SW2);
+InterruptIn button3(SW2);
InterruptIn button4(SW3);
-//AnalogIn emg(A0);
-AnalogIn potmeter1(A0);
-AnalogIn potmeter2(A1);
+AnalogIn potmeter1(A0); // CONNECT POT1 TO A0
+AnalogIn potmeter2(A1); // CONNECT POT2 TO A1
+DigitalOut led1(LED_RED);
+DigitalOut led2(LED_BLUE);
+DigitalOut led3(LED_GREEN);
+AnalogIn emg_r(A2); // CONNECT EMG TO A2
+AnalogIn emg_l(A3); // CONNECT EMG TO A3
// MOTOR CONTROL ///////////////////////////////////////////////////////////////
-Ticker controllerTicker;
-const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
+Ticker controllerTicker; // Ticker for the controller
+const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]
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.05; // X and Y velocity of the end effector when desired
+volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
+double velocity = 0.01; // 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]
+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 = 13; // Position 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 []
+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]
+volatile double position2; // Position of motor 2 [rad]
+volatile double reference2 = 0; // Desired 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_KP = 13; // Position 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 []
+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 []
+// EMG //////////////////////////////////////////////////////////////////
+Ticker emgLeft;
+Ticker emgRight;
+double emgTs = 0.5;
+// Filters
+BiQuadChain bqc;
+BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004);
+BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
+BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191);
+// Right arm
+volatile double emgFiltered_r;
+volatile double filteredAbs_r;
+volatile double emg_value_r;
+volatile double onoffsignal_r;
+volatile bool check_calibration_r=0;
+volatile double avg_emg_r;
+volatile bool active_r = false;
+// Left arm
+volatile double emgFiltered_l;
+volatile double filteredAbs_l;
+volatile double emg_value_l;
+volatile double onoffsignal_l;
+volatile bool check_calibration_l=0;
+volatile double avg_emg_l;
+volatile bool active_l = false;
-Ticker sampleTicker;
-const double tickerTs = 0.1; // Time step for sampling [s]
-
+Ticker sampleTicker;
+const double sampleTs = 0.05;
// FUNCTIONS ///////////////////////////////////////////////////////////////////
// BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
@@ -96,7 +120,6 @@
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,
@@ -146,20 +169,27 @@
// MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
void Motor1Controller()
{
- position1 = radPerPulse * Encoder1.getPulses();
- position2 = radPerPulse * Encoder2.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);
- 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);
- RotateMotor2(motor2Value);
+ if(currentState == MOVING)
+ {
+ position1 = radPerPulse * Encoder1.getPulses();
+ position2 = radPerPulse * Encoder2.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);
+
+ 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("%f, %f \r\n", motor1Value, motor2Value);
+
+ RotateMotor1(motor1Value);
+ RotateMotor2(motor2Value);
+ }
}
// MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
@@ -180,116 +210,257 @@
motor2MagnitudePin = 0;
}
-// DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
-void jointVelocities()
-{
- position1 = radPerPulse * Encoder1.getPulses();
- position2 = radPerPulse * Encoder2.getPulses();
-
- if(!button1 && reference1 > motor1Min && reference2 < motor2Max) xVelocity = velocity;
- else if(!button2 && reference1 < motor1Max && reference2 > motor2Min) xVelocity = -velocity;
- else xVelocity = 0;
+
+// EMG /////////////////////////////////////////////////////////////////////////
+// Filter EMG signal of right arm
- if(!button3 && reference2 < motor2Max && reference1 > motor2Min) yVelocity = velocity;
- else if(!button4 && reference2 > motor2Min && reference1 > motor2Min) yVelocity = -velocity;
- 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];
+void filter_r(){
+ if(check_calibration_r==1){
+ emg_value_r = emg_r.read();
+ emgFiltered_r = bqc.step( emg_value_r );
+ filteredAbs_r = abs( emgFiltered_r );
+ if (avg_emg_r != 0){
+ onoffsignal_r=filteredAbs_r/avg_emg_r; //divide the emg_r signal by the max emg_r to calibrate the signal per person
+ }
+ }
+}
+
+// Filter EMG signal of left arm
+void filter_l(){
+ if(check_calibration_l==1){
+ emg_value_l = emg_l.read();
+ emgFiltered_l = bqc.step( emg_value_l );
+ filteredAbs_l = abs( emgFiltered_l );
+ if (avg_emg_l != 0){
+ onoffsignal_l=filteredAbs_l/avg_emg_l; //divide the emg_r signal by the avg_emg_l wat staat voor avg emg in gespannen toestand
+ }
}
-
- for (int i = 0; i < 2; i++)
- {
- for (int j = 0; j < 3; j++)
- {
- J_data[j + 3 * i] = T1[j + 3 * i];
- }
+}
+
+// Check threshold right arm
+void check_emg_r(){
+double filteredAbs_temp_r;
+
+ if((check_calibration_l==1) &&(check_calibration_r==1)){
+ for( int i = 0; i<250;i++){
+ filter_r();
+ filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
+ wait(0.0004); // 0.0004
+ }
+ filteredAbs_temp_r = filteredAbs_temp_r/250;
+ if(filteredAbs_temp_r<=0.3){ //if signal is lower then 0.5 the blue light goes on
+ led1.write(1); //led 1 is rood en uit
+
+ active_r = false;
+ }
+ else if(filteredAbs_temp_r > 0.3){ //if signal does not pass threshold value, blue light goes on
+ led1.write(0);
+ active_r = true;
+ }
}
-
- // 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(reference1 + msh[0]*tickerTs > motor1Max) reference1 = motor1Max;
- else if(reference1 + msh[0]*tickerTs < motor1Min) reference1 = motor1Min;
- else reference1 = reference1 + msh[0]*tickerTs;
-
- if(reference2 + msh[1]*tickerTs > motor2Max) reference2 = motor2Max;
- else if(reference2 + msh[1]*tickerTs < motor2Min) reference2 = motor2Min;
- else reference2 = reference2 + msh[1]*tickerTs;
-
- //Motor1Controller(), Motor2Controller();
- scope.set(0,reference1);
- scope.set(1,position1);
- scope.set(2,reference2);
- scope.set(3,position2);
- scope.send();
-
- 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);
+}
+ // Check threshold left arm
+void check_emg_l(){
+double filteredAbs_temp_l;
+
+ if((check_calibration_l)==1 &&(check_calibration_r==1) ){
+ for( int i = 0; i<250;i++){
+ filter_l();
+ filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
+ wait(0.0004); // 0.0004
+ }
+ filteredAbs_temp_l = filteredAbs_temp_l/250;
+ if(filteredAbs_temp_l<=0.3){ //if signal is lower then 0.5 the blue light goes on
+ // led1.write(1); //led 1 is rood en uit
+ led2.write(1); //led 2 is blauw en aan
+ active_l = false;
+ }
+ else if(filteredAbs_temp_l > 0.3){ //if signal does not pass threshold value, blue light goes on
+ // led1.write(0);
+ led2.write(0);
+ active_l = true;
+ }
+ }
+
}
-// EMG /////////////////////////////////////////////////////////////////////////
-void EmgSample()
+// Calibrate right arm
+int calibration_r(){
+ led3.write(0);
+
+ double signal_verzameling_r = 0;
+ for(int n =0; n<5000;n++){
+ filter_r();
+ //read for 5000 samples as calibration
+ emg_value_r = emg_r.read();
+ emgFiltered_r = bqc.step( emg_value_r );
+ filteredAbs_r = abs(emgFiltered_r);
+
+
+ // signal_verzameling[n]= filteredAbs_r;
+ signal_verzameling_r = signal_verzameling_r + filteredAbs_r ;
+ wait(0.0004);
+
+ if (n == 4999){
+ avg_emg_r = signal_verzameling_r / n;
+
+ }
+ }
+
+ led3.write(1);
+ //pc.printf("calibratie rechts compleet\n\r");
+
+ check_calibration_r=1;
+ return 0;
+}
+
+// Calibrate left arm
+int calibration_l(){
+ led3.write(0);
+
+ double signal_verzameling_l= 0;
+ for(int n =0; n<5000;n++){
+ filter_l();
+ //read for 5000 samples as calibration
+ emg_value_l = emg_l.read();
+ emgFiltered_l = bqc.step( emg_value_l );
+ filteredAbs_l = abs(emgFiltered_l);
+
+
+ // signal_verzameling[n]= filteredAbs_r;
+ signal_verzameling_l = signal_verzameling_l + filteredAbs_l ;
+ wait(0.0004);
+
+ if (n == 4999){
+ avg_emg_l = signal_verzameling_l / n;
+
+ }
+ }
+ led3.write(1);
+ wait(1);
+ check_calibration_l=1;
+
+ //pc.printf("calibratie links compleet\n\r");
+ // }
+ return 0;
+}
+
+// DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
+void JointVelocities()
{
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
- If both signal 1 and 2 --> Switch
- */
- jointVelocities();
+ position1 = radPerPulse * Encoder1.getPulses();
+ position2 = radPerPulse * Encoder2.getPulses();
+
+ //check_emg_r(), check_emg_l();
+
+ if(active_r && reference1 > motor1Min && reference2 < motor2Max)
+ {
+ xVelocity = velocity;
+ //pc.printf("button1 \r\n");
+ }
+ else if(active_l && reference1 < motor1Max && reference2 > motor2Min)
+ {
+ xVelocity = -velocity;
+ //pc.printf(" button2 \r\n");
+ }
+ else xVelocity = 0;
+
+ if(!button3 && reference2 < motor2Max )//&& reference1 > motor2Min)
+ {
+ yVelocity = velocity;
+ //pc.printf(" button3 \r\n");
+ }
+ else if(!button4 && reference2 > motor2Min )//&& reference1 > motor2Min)
+ {
+ yVelocity = -velocity;
+ //pc.printf(" button4 \r\n");
+ }
+ 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(reference1 + msh[0]*sampleTs > motor1Max) reference1 = motor1Max;
+ else if(reference1 + msh[0]*sampleTs < motor1Min) reference1 = motor1Min;
+ else reference1 = reference1 + msh[0]*sampleTs;
+
+ if(reference2 + msh[1]*sampleTs > motor2Max) reference2 = motor2Max;
+ else if(reference2 + msh[1]*sampleTs < motor2Min) reference2 = motor2Min;
+ else reference2 = reference2 + msh[1]*sampleTs;
+
+
+ scope.set(0,reference1);
+ scope.set(1,position1);
+ scope.set(2,reference2);
+ scope.set(3,position2);
+ scope.send();
+
+ 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);
+
}
}
@@ -304,7 +475,7 @@
if(stateChanged)
{
//pc.printf("Entering MOTORS_OFF \r\n"
- //"Press button 1 to enter MOVING \r\n");
+ //"Press button 1 to enter CALIBRATING \r\n");
TurnMotorsOff(); // Turn motors off
stateChanged = false;
}
@@ -312,10 +483,35 @@
// Home command
if(!button1)
{
+ currentState = CALIBRATING;
+ stateChanged = true;
+ break;
+ }
+ break;
+ }
+
+ case CALIBRATING:
+ {
+ // State initialization
+ if(stateChanged)
+ {
+ //pc.printf("Entering CALIBRATING \r\n"
+ //"Press button 1 to enter MOVING \r\n");
+ stateChanged = false;
+ calibration_r();
+ calibration_l();
+ currentState = MOVING;
+ stateChanged = true;
+ }
+ /*
+ // Home command
+ if(!button1)
+ {
currentState = MOVING;
stateChanged = true;
break;
}
+ */
break;
}
@@ -324,7 +520,7 @@
// 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;
}
@@ -369,12 +565,17 @@
// Serial communication
pc.baud(115200);
+ led1.write(1);
+ led2.write(1);
+ led3.write(1);
pc.printf("START \r\n");
- //bqc.add(&bq1).add(&bq2); // Make BiQuad Chain
+ bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
- sampleTicker.attach(&EmgSample, tickerTs); // Ticker to sample EMG
+ sampleTicker.attach(&JointVelocities, sampleTs); // Ticker to sample EMG
controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
+ emgRight.attach(&check_emg_r, emgTs); //continously execute the motor controller
+ emgLeft.attach(&check_emg_l, emgTs);
motor1MagnitudePin.period_ms(1);
motor2MagnitudePin.period_ms(1);