Jordi Luong
Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 18:2b6f41f39a7f
- Parent:
- 17:f8dd5b8e4b52
- Child:
- 19:6f720e4fcb47
--- a/main.cpp Thu Nov 02 14:03:10 2017 +0000
+++ b/main.cpp Thu Nov 02 16:23:26 2017 +0000
@@ -13,18 +13,18 @@
HIDScope scope(4);
// STATES //////////////////////////////////////////////////////////////////////
-enum states{MOTORS_OFF, CALIBRATING, MOVING, HITTING};
+enum states{MOTORS_OFF, CALIBRATING, MOVING};
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
+QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D12, ENC1B TO D13
+QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D10, ENC2B TO D11
// PINS ////////////////////////////////////////////////////////////////////////
DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
PwmOut motor1MagnitudePin(D5);
-DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW?
+DigitalOut motor2DirectionPin(D7); // Value 0: CW; 1: CCW
PwmOut motor2MagnitudePin(D6);
InterruptIn button1(D2); // CONNECT BUT1 TO D2
InterruptIn button2(D3); // CONNECT BUT2 TO D3
@@ -42,7 +42,7 @@
// MOTOR CONTROL ///////////////////////////////////////////////////////////////
Ticker controllerTicker; // Ticker for the controller
-const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]
+const double controllerTs = 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]
volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
@@ -50,7 +50,7 @@
// MOTOR 1
volatile double position1; // Position of motor 1 [rad]
-volatile double reference1 = -5; // Desired rotation of motor 1 [rad]
+volatile double reference1 = 2*pi*-5/360; // Desired rotation of motor 1 [rad]
const double motor1Max = 0; // Maximum rotation of motor 1 [rad]
const double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad]
// Controller gains
@@ -80,21 +80,21 @@
// Filter variables
double motor2_f_v1 = 0, motor2_f_v2 = 0;
-// EMG //////////////////////////////////////////////////////////////////
-Ticker emgLeft;
-Ticker emgRight;
-const double emgTs = 0.5;
+// EMG /////////////////////////////////////////////////////////////////////////
+Ticker emgLeft; // Ticker for EMG of left arm
+Ticker emgRight; // Ticker for EMG of right arm
+const double emgTs = 0.491; // Time step for EMG sampling
// 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);
+BiQuadChain bqc;
+BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004); // High pass filter
+BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780); // Notch filter
+BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191); // Low pass filter
// 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 bool check_calibration_r = false;
volatile double avg_emg_r;
volatile bool active_r = false;
// Left arm
@@ -102,16 +102,16 @@
volatile double filteredAbs_l;
volatile double emg_value_l;
volatile double onoffsignal_l;
-volatile bool check_calibration_l=0;
+volatile bool check_calibration_l = false;
volatile double avg_emg_l;
volatile bool active_l = false;
-
-Ticker sampleTicker;
-const double sampleTs = 0.05;
-volatile bool xdir = true, ydir = false;
-volatile bool timer = false;
-volatile int count = 0;
+// PROCESS EMG SIGNALS /////////////////////////////////////////////////////////
+Ticker processTicker; // Ticker for processing of EMG
+const double processTs = 0.1; // Time step for processing of EMG
+
+volatile bool xdir = true, ydir = false; // Direction the EMG signal moves the end effector
+volatile int count = 0; // Counter to change direction
// FUNCTIONS ///////////////////////////////////////////////////////////////////
// BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
@@ -133,19 +133,17 @@
// 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
- //e_der = bq1.step(e_der);
e_prev = e;
// Integral
e_int = e_int + Ts*e; // Calculate the integral of error
// PID
- //pc.printf("%f --> %f \r\n", e_der, e_derf);
- return Kp*e + Ki*e_int + Kd*e_der;
+ return Kp*e + Ki*e_int + Kd*e_der; // Calculate motor value
}
// MOTOR 1 /////////////////////////////////////////////////////////////////////
void RotateMotor1(double motor1Value)
{
- if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
+ if(currentState == MOVING) // Check if state is MOVING
{
if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
else motor1DirectionPin = 1; // Rotate motor 1 CCW
@@ -159,10 +157,10 @@
// MOTOR 2 /////////////////////////////////////////////////////////////////////
void RotateMotor2(double motor2Value)
{
- if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
+ if(currentState == MOVING) // Check if state is MOVING
{
- if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW
- else motor2DirectionPin = 0; // Rotate motor 1 CCW
+ if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 2 CW
+ else motor2DirectionPin = 0; // Rotate motor 2 CCW
if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
else motor2MagnitudePin = fabs(motor2Value);
@@ -170,43 +168,29 @@
else motor2MagnitudePin = 0;
}
-// MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
-void Motor1Controller()
+// MOTOR PID-CONTROLLER //////////////////////////////////////////////////////
+void MotorController()
{
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,
+ position1 = radPerPulse * Encoder1.getPulses(); // Get current position of motor 1
+ position2 = radPerPulse * Encoder2.getPulses(); // Get current position of motor 2
+
+ double motor1Value = PID_Controller(reference1 - position1, motor1_KP, // Calculate motor value motor 1
+ motor1_KI, motor1_KD, controllerTs, 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,
+ double motor2Value = PID_Controller(reference2 - position2, motor2_KP, // Calculate motor value motor 2
+ motor2_KI, motor2_KD, controllerTs, 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);
+ RotateMotor1(motor1Value); // Rotate motor 1
+ RotateMotor2(motor2Value); // Rotate motor 2
}
}
-
-// MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
-/*void Motor2Controller()
-{
- position2 = radPerPulse * Encoder2.getPulses();
- double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
- motor2_KI, motor2_KD, controller2_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);
-}
-*/
+
// TURN OFF MOTORS /////////////////////////////////////////////////////////////
void TurnMotorsOff()
{
@@ -214,139 +198,140 @@
motor2MagnitudePin = 0;
}
-
// EMG /////////////////////////////////////////////////////////////////////////
// Filter EMG signal of right arm
-
-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
- }
+void filter_r()
+{
+ if(check_calibration_r == 1)
+ {
+ emg_value_r = emg_r.read(); // Get EMG signal
+ emgFiltered_r = bqc.step(emg_value_r); // Filter EMG signal using BiQuad Chain
+ filteredAbs_r = abs(emgFiltered_r); // Takes absolute value
+
+ 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){
+void filter_l()
+{
+ if(check_calibration_l == 1)
+ {
emg_value_l = emg_l.read();
- emgFiltered_l = bqc.step( emg_value_l );
+ 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
- }
+ if (avg_emg_l != 0)
+ {
+ onoffsignal_l = filteredAbs_l/avg_emg_l;
+ }
}
}
// Check threshold right arm
-void check_emg_r(){
-double filteredAbs_temp_r;
+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;
- }
+ 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);
+ }
+ 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;
+ }
}
}
- // Check threshold left arm
-void check_emg_l(){
-double filteredAbs_temp_l;
+
+// 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;
- }
+ 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);
+ }
+ 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
+ {
+ led2.write(1);
+ active_l = false;
+ }
+ else if(filteredAbs_temp_l > 0.3) //if signal does not pass threshold value, blue light goes on
+ {
+ led2.write(0);
+ active_l = true;
+ }
}
-
}
// Calibrate right arm
-int calibration_r(){
- led3.write(0);
+void 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);
+ double signal_collection_r = 0;
+ for(int n =0; n < 5000; n++) //read for 5000 samples as calibration
+ {
+ filter_r();
+ 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);
+ signal_collection_r = signal_collection_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;
+ if (n == 4999)
+ {
+ avg_emg_r = signal_collection_r / n;
+ }
+ }
+ led3.write(1);
+ check_calibration_r = 1;
}
// Calibrate left arm
-int calibration_l(){
- led3.write(0);
+void 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);
+ double signal_collection_l = 0;
+ for(int n = 0; n < 5000; n++) //read for 5000 samples as calibration
+ {
+ filter_l();
+ emg_value_l = emg_l.read();
+ emgFiltered_l = bqc.step(emg_value_l);
+ filteredAbs_l = abs(emgFiltered_l);
+ signal_collection_l = signal_collection_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;
+ if (n == 4999)
+ {
+ avg_emg_l = signal_collection_l / n;
+ }
+ }
+ led3.write(1);
+ wait(1);
+ check_calibration_l = 1;
}
// DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
@@ -354,13 +339,13 @@
{
if(currentState == MOVING)
{
- position1 = radPerPulse * Encoder1.getPulses();
- position2 = radPerPulse * Encoder2.getPulses();
+ //position1 = radPerPulse * Encoder1.getPulses();
+ //position2 = radPerPulse * Encoder2.getPulses();
- if(active_l && active_r)
+ if(active_l && active_r) // If both left and right EMG are active for 1 second the direction of control changes
{
count += 1;
- if(count == 40)
+ if(count == 20)
{
active_l = false;
active_r = false;
@@ -374,45 +359,43 @@
}
else count = 0;
- if(xdir)
+ if(xdir) // Control in x-direction
{
- if(active_r && count == 0 && reference1 > motor1Min && reference2 < motor2Max)
+ if(active_r && count == 0 && // Checks whether EMG is active, changing direction and max rotation of motors
+ reference1 > motor1Min && reference2 < motor2Max)
{
- xVelocity = velocity;
- pc.printf("x positive \r\n");
+ xVelocity = velocity; // Give velocity to end effector
}
- else if(active_l && count == 0 && reference1 < motor1Max && reference2 > motor2Min)
+ else if(active_l && count == 0 &&
+ reference1 < motor1Max && reference2 > motor2Min)
{
xVelocity = -velocity;
- pc.printf("x negative \r\n");
}
else xVelocity = 0;
}
- else if(ydir)
+ else if(ydir) // Control in y-direction
{
- if(active_r && count == 0 && reference2 < motor2Max )//&& reference1 > motor2Min)
+ if(active_r && count == 0 &&
+ reference2 < motor2Max ) //&& reference1 > motor2Min)
{
yVelocity = velocity;
- pc.printf("y positive \r\n");
}
- else if(active_l && count == 0 && reference2 > motor2Min )//&& reference1 > motor2Min)
+ else if(active_l && count == 0
+ && reference2 > motor2Min ) //&& reference1 > motor2Min)
{
yVelocity = -velocity;
- pc.printf("y negative \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 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];
@@ -421,15 +404,17 @@
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]);
+ 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]);
+ 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++)
{
@@ -454,36 +439,36 @@
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
+ 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
+ double msh[2]; // 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;
+ // Determine reference position of motor 1
+ if(reference1 + msh[0]*processTs > motor1Max) reference1 = motor1Max;
+ else if(reference1 + msh[0]*processTs < motor1Min) reference1 = motor1Min;
+ else reference1 = reference1 + msh[0]*processTs;
- if(reference2 + msh[1]*sampleTs > motor2Max) reference2 = motor2Max;
- else if(reference2 + msh[1]*sampleTs < motor2Min) reference2 = motor2Min;
- else reference2 = reference2 + msh[1]*sampleTs;
+ // Determine reference position of motor 2
+ if(reference2 + msh[1]*processTs > motor2Max) reference2 = motor2Max;
+ else if(reference2 + msh[1]*processTs < motor2Min) reference2 = motor2Min;
+ else reference2 = reference2 + msh[1]*processTs;
- scope.set(0,reference1);
+ /*scope.set(0,reference1);
scope.set(1,position1);
scope.set(2,reference2);
scope.set(3,position2);
- scope.send();
+ 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);
-
}
}
@@ -503,7 +488,7 @@
stateChanged = false;
}
- // Home command
+ // Continue button
if(!button1)
{
currentState = CALIBRATING;
@@ -519,22 +504,13 @@
if(stateChanged)
{
pc.printf("Entering CALIBRATING \r\n"
- "Press button 1 to enter MOVING \r\n");
+ "Tighten muscles until green LED is off \r\n");
stateChanged = false;
- calibration_r();
- calibration_l();
+ calibration_r(); // Calibrate right arm
+ calibration_l(); // Calibrate left arm
currentState = MOVING;
stateChanged = true;
}
- /*
- // Home command
- if(!button1)
- {
- currentState = MOVING;
- stateChanged = true;
- break;
- }
- */
break;
}
@@ -544,35 +520,8 @@
if(stateChanged)
{
pc.printf("Entering MOVING \r\n");
- //"Press button 2 to enter HITTING \r\n");
stateChanged = false;
}
-
-
-
- // Hit command
- /*if(!button2)
- {
- currentState = HITTING;
- stateChanged = true;
- break;
- }
- */
- break;
- }
-
- case HITTING:
- {
- // State initialization
- if(stateChanged)
- {
- //pc.printf("Entering HITTING \r\n");
- stateChanged = false;
- //HitBall(); // Hit the ball
- currentState = MOVING;
- stateChanged = true;
- break;
- }
break;
}
@@ -590,6 +539,7 @@
// Serial communication
pc.baud(115200);
+ // Start values of LEDs
led1.write(1);
led2.write(1);
led3.write(1);
@@ -598,16 +548,15 @@
pc.printf("START \r\n");
- bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
+ bqc.add(&bq1_low ).add(&bq2_high ).add(&bq3_notch ); // Make BiQuad Chain
- 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);
+ processTicker.attach(&JointVelocities, processTs); // Ticker to process EMG
+ controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motor 1 (PID)
+ emgRight.attach(&check_emg_r, emgTs); // Ticker to sample EMG of right arm
+ emgLeft.attach(&check_emg_l, emgTs); // Ticker to sample EMG of left arm
- motor1MagnitudePin.period_ms(1);
- motor2MagnitudePin.period_ms(1);
- TurnMotorsOff();
+ motor1MagnitudePin.period_ms(1); // PWM frequency of motor 1 (Should actually be 5 - 10 kHz)
+ motor2MagnitudePin.period_ms(1); // PWM frequency of motor 2 (Should actually be 5 - 10 kHz)
while(true)
{