Marijke Zondag
/
Project_script_union_final_version
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Project_script_union_final
by 
Jorine Oosting
Revision 35:63c890ac71ff, committed 2018-11-05
- Comitter:
- MarijkeZondag
- Date:
- Mon Nov 05 16:45:17 2018 +0000
- Parent:
- 34:b8b18ba0c336
- Child:
- 36:650a9245bc44
- Commit message:
- Code mooi gemaakt + comments
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 05 15:19:42 2018 +0000
+++ b/main.cpp Mon Nov 05 16:45:17 2018 +0000
@@ -29,11 +29,11 @@
DigitalOut ledg (LED_GREEN);
-MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
+MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING);
QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING);
-//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
+//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
//Tickers
Ticker func_tick;
@@ -43,58 +43,59 @@
//Global variables
-const float T = 0.002f; //Ticker period EMG, engine control
-const float T2 = 0.2f; //Ticker print function
+const float T = 0.002f; //Ticker period EMG, engine control
+const float T2 = 0.2f; //Ticker print function
//EMG filter
double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
double emg0_raw, emg1_raw, emg2_raw;
double emg0_filt_x, emg1_filt_x, emg2_filt_x;
-const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
+const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated
double sum, sum1, sum2, sum3; //variables used to sum elements in array
double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
-double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
+double movAg0, movAg1, movAg2; //Outcome of MovAg
//Calibration variables
int x = -1; //Start switch, colour LED is blue.
-int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!)
-const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000
-double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
-double Mean0,Mean1,Mean2; //average of maximum tightening
+int emg_cal = 0; //If emg_cal is set to 1, motors can begin to work in this code
+const int sizeCal = 1500; //Size of the dataset used for calibration
+double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //Arrays to put the dataset of the calibration in
+double Mean0,Mean1,Mean2; //Average of maximum contraction
double Threshold0, Threshold1, Threshold2;
-//Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
+//Biquad //Variables for the biquad band filters
BiQuadChain emg0filter;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
+BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
BiQuadChain emg1filter;
BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
+BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
BiQuadChain emg2filter;
BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
+BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
//Variables PID controller
-double PI = 3.14159;
-double Kp1 = 20.0; //Motor 1
-double Ki1 = 1.02;
-double Kd1 = 1.0;
-double encoder_radians1=0;
-double err_integral1 = 0;
-double err_prev1, err_prev2;
-double err1, err2;
-BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass
+double PI = 3.14159; //Pi value
-double Kp2 = 20.0; //Motor 2
+double Kp1 = 20.0; //Proportional gain motor 1
+double Ki1 = 1.02; //Integrative term motor 1
+double Kd1 = 1.0; //Differential term motor 1
+double encoder_radians1=0; //Inital encoder value motor 1
+double err_integral1 = 0; //Initial error integral value motor 1
+double err_prev1, err_prev2; //Variables called previous error motor 1 and motor 2
+double err1, err2; //Variables called error motor 1 and motor 2
+BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //Low_pass differential term Sample frequency 500 Hz, cutoff 20Hz low pass
+
+double Kp2 = 20.0; / //Motor 2
double Ki2 = 1.02;
double Kd2 = 1.0;
double encoder_radians2=0;
@@ -103,27 +104,27 @@
BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
// Inverse Kinematica variables
-//const double L1 = 0.208; // Hoogte van tafel tot joint 1
-//const double L2 = 0.288; // Hoogte van tafel tot joint 2
-const double L3 = 0.212; // Lengte van de arm
-//const double L4 = 0.089; // Afstand van achterkant base tot joint 1
-//const double L5 = 0.030; // Afstand van joint 1 naar joint 2
-const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
+//const double L1 = 0.208; // Height table to joint 1
+//const double L2 = 0.288; // Height table to joint 2
+const double L3 = 0.212; // Length arm
+//const double L4 = 0.089; // Distance backside base to joint 1
+//const double L5 = 0.030; // Distance from joint 1 to joint 2
+const double r_trans = 0.035; // Calculate translation to shaft rotation
// Variërende variabelen inverse kinematics:
-double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
-double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
-double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
-double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
+double q1ref = 0.0; // Current motor angle of joint 1 as determined out of reference signal
+double q2ref = 0.0; // Current motor angle of joint 2 as determined out of reference signal
+double v_x; // Desired velocity of end effector in x direction --> Determined by EMG signals
+double v_y; // Desired velocity of end effector in y direction --> Determined by EMG signals
-//double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
-//double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
+//double Lq1; // Translational distance due to motor rotation joint 1
+//double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
-double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
-double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
+double q1_dot=0.0; // Required angular velocity of motor 1 to reach v_des
+double q2_dot=0.0; // Required angular velocity of motor 2 to reach v_des
-double q1_ii=0.0; // Reference signal for motorangle q1ref
-double q2_ii=0.0; // Reference signal for motorangle q2ref
+double q1_ii=0.0; // Reference signal for motorangle q1ref
+double q2_ii=0.0; // Reference signal for motorangle q2ref
double q1_motor;
double q2_motor;
@@ -131,42 +132,45 @@
//--------------Functions----------------------------------------------------------------------------------------------------------------------------//
-//------------------ Filter EMG + Calibration EMG --------------------------------//
+//------------------ Filter EMG + Calibration EMG --------------------------------------------------------------------------------------------------//
void switch_to_calibrate()
{
- x++; //Every time function gets called, x increases. Every button press --> new calibration state.
- //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
+ //Every time function gets called, x increases. Every button press --> new calibration state.
+ //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
+
+ x++;
+
- if(x==0) //If x = 0, led is red
+ if(x==0) //If x = 0, led is red
{
ledr = 0;
ledb = 1;
ledg = 1;
}
- else if (x==1) //If x = 1, led is blue
+ else if (x==1) //If x = 1, led is blue
{
ledr = 1;
ledb = 0;
ledg = 1;
}
- else if (x==2) //If x = 2, led is green
+ else if (x==2) //If x = 2, led is green
{
ledr = 1;
ledb = 1;
ledg = 0;
}
- else //If x = 3 or 4, led is white
+ else //If x = 3 or 4, led is white
{
ledr = 0;
ledb = 0;
ledg = 0;
}
- if(x==4) //Reset back to x = -1
+ if(x==4) //Reset back to x = -1
{
x = -1;
- emg_cal=0; //reset, motors off
+ emg_cal=0; //Reset, motors off
}
}
@@ -175,23 +179,23 @@
{
switch(x)
{
- case 0: //If calibration state 0:
+ case 0: //If calibration state 0:
{
sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
{
StoreCal0[j] = emg0_filt;
sum+=StoreCal0[j];
- wait(0.001f); //Does there need to be a wait?
+ wait(0.001f);
}
- Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
- Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean
- break; //Stop. Threshold is calculated, we will use this further in the code
+ Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set
+ Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean
+ break; //Stop. Threshold is calculated.
}
- case 1: //If calibration state 1:
+ case 1: //If calibration state 1:
{
sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
{
StoreCal1[j] = emg1_filt;
sum+=StoreCal1[j];
@@ -200,11 +204,11 @@
Mean1 = sum/sizeCal;
Threshold1 = Mean1/2;
break;
- }
- case 2: //If calibration state 2:
+ }
+ case 2: //If calibration state 2:
{
sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
{
StoreCal2[j] = emg2_filt;
sum+=StoreCal2[j];
@@ -214,14 +218,14 @@
Threshold2 = Mean2/2;
break;
}
- case 3: //EMG is calibrated, robot can be set to Home position.
+ case 3: //EMG is calibrated, robot can be set to Home position.
{
- emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
+ emg_cal = 1; //This is the setting for which the motors can begin turning in this code
wait(0.001f);
break;
}
- default: //Ensures nothing happens if x is not 0,1 or 2.
+ default: //Ensures nothing happens if x is not 0,1 or 2
{
break;
}
@@ -230,35 +234,35 @@
void EMGFilter0()
{
- emg0_raw = emg0_in.read(); //give name to raw EMG0 data
- emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
- emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
+ emg0_raw = emg0_in.read(); //Give name to raw EMG0 data
+ emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
+ emg0_filt = abs(emg0_filt_x); //Rectifier
}
void EMGFilter1()
{
- emg1_raw = emg1_in.read(); //give name to raw EMG1 data
- emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
- emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
+ emg1_raw = emg1_in.read(); //Give name to raw EMG1 data
+ emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
+ emg1_filt = abs(emg1_filt_x); //Rectifier
}
void EMGFilter2()
{
- emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
- emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
- emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
+ emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
+ emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
+ emg2_filt = abs(emg2_filt_x); //Rectifier
}
-void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
+void MovAg() //Calculate moving average (MovAg)
{
- for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
+ for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
{
- StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
+ StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
StoreArray1[i] = StoreArray1[i-1];
StoreArray2[i] = StoreArray2[i-1];
}
- StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
+ StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
StoreArray1[0] = emg1_filt;
StoreArray2[0] = emg2_filt;
@@ -266,71 +270,47 @@
sum2 = 0.0;
sum3 = 0.0;
- for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
+ for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
{
sum1 += StoreArray0[a];
sum2 += StoreArray1[a];
sum3 += StoreArray2[a];
}
- movAg0 = sum1/windowsize; //calculates an average in the array
+ movAg0 = sum1/windowsize; //calculates an average in the array
movAg1 = sum2/windowsize;
movAg2 = sum3/windowsize;
- //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
}
-void emg_filtered() //Call all filter functions
+void emg_filtered() //Call all filter functions
{
EMGFilter0();
EMGFilter1();
EMGFilter2();
}
-/*
-void HIDScope_sample()
-{
- scope.set(0,emg0_raw);
- scope.set(1,emg0_filt);
- scope.set(1,movAg0); //als moving average werkt
- scope.set(2,emg1_raw);
- scope.set(3,emg1_filt);
- scope.set(3,movAg1); //als moving average werkt
- scope.set(4,emg2_raw);
- scope.set(5,emg2_filt);
- scope.set(5,movAg2); //als moving average werkt
-
- scope.send(); //Send data to HIDScope server
-}
-*/
-
-
-//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------//
+//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------------------------------------------//
void PID_control1()
{
- //pc.printf("ik doe het, PDI \n\r");
-
// Proportional part:
- double u_k1 = Kp1 * err1;
+ double u_k1 = Kp1 * err1; //Proportional gain times calculated error
//Integral part
- err_integral1 = err_integral1 + err1 * T;
- double u_i1 = Ki1 * err_integral1;
+ err_integral1 = err_integral1 + err1 * T; //Adds the error*T
+ double u_i1 = Ki1 * err_integral1; //Integral term times the integral
// Derivative part
- double err_derivative1 = (err1 - err_prev1)/T;
- double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1);
- double u_d1 = Kd1 * filtered_err_derivative1;
- err_prev1 = err1;
+ double err_derivative1 = (err1 - err_prev1)/T; //error - previous error /T
+ double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); //Filter the derivative term for stabilization
+ double u_d1 = Kd1 * filtered_err_derivative1; //Derivative term times the derivative error
+ err_prev1 = err1; //Sets the current error to previous error (remember)
-
- // Sum all parts and return it
- u1 = u_k1 + u_i1 + u_d1;
+ // Sum all parts and return it
+ u1 = u_k1 + u_i1 + u_d1;
}
void PID_control2()
{
- //pc.printf("ik doe het, PDI \n\r");
-
// Proportional part:
double u_k2 = Kp2 * err2;
@@ -344,196 +324,124 @@
double u_d2 = Kd2 * filtered_err_derivative2;
err_prev2 = err2;
-
- // Sum all parts and return it
+ // Sum all parts and return it
u2 = u_k2 + u_i2 + u_d2;
}
-void engine_control1() //Engine 1 is translational engine, connected with left side pins
+void engine_control1() //Engine 1 is translational engine, connected with left side pins
{
encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
- err1 = q1_motor - encoder_radians1;
- PID_control1(); //PID controller function call
-
- //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
- //{
- pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin1.write(u1<0);
- //}
- //else
- // {
- // pwmpin1 = 0;
- // }
+ err1 = q1_motor - encoder_radians1; //Calculate error between desired angle 1 and current angle 1
+ PID_control1(); //PID 1 controller function call
+ pwmpin1 = fabs(u1); //Motor 1 speed set
+ directionpin1.write(u1<0); //Direction motor 1 set
}
-void engine_control2() //Engine 2 is rotational engine, connected with right side wires
+void engine_control2() //Engine 2 is rotational engine, connected with right side wires
{
encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
- //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
- //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
- err2 = q2_motor - encoder_radians2;
- //pc.printf("err2 = %f\n\r",err2);
- PID_control2(); //PID controller function call
- //pc.printf("u2 = %f\n\r",u2);
-
- //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts
- // {
- pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin2.write(u2>0);
- // }
- //else
- // {
- // pwmpin2 = 0;
- // }
+ err2 = q2_motor - encoder_radians2; //Calculate error between desired angle 2 and current angle 2
+ PID_control2(); //PID 2 controller function call
+ pwmpin2 = fabs(u2); //Motor 2 speed set
+ directionpin2.write(u2>0); //Direction motor 2 set
}
-/*void engine_control1() //Engine 1 is translational engine, connected with left side pins
-{
- encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
- err1 = q1_motor - encoder_radians1;
- PID_control1(); //PID controller function call
-
- if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
- {
- pwmpin1 = fabs(u1);
- directionpin1.write(u1<0);
- }
- else
- {
- pwmpin1 = fabs(u1);
- directionpin1.write(u1>0);
- }
-}
-
-void engine_control2() //Engine 2 is rotational engine, connected with right side wires
-{
- encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
- err2 = q2_motor - encoder_radians2;
- PID_control2(); //PID controller function call
-
- if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts
- {
- pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin2.write(u2>0);
- }
- else
- {
- pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin2.write(u2<0);
- }
-}
-*/
-
-//------------------ Inversed Kinematics --------------------------------//
+//------------------ Inversed Kinematics -----------------------------------------------------------------------------------------------------------//
void inverse_kinematics()
{
- q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //RKI systeem
- q2_dot = v_y/(L3*cos(q2ref)); //
-
- q1_ii = q1ref + q1_dot*T; //Omgezet naar motorhoeken
- q2_ii = q2ref + q2_dot*T;
+ q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //Calculate desired angular velocity of motor 1
+ q2_dot = v_y/(L3*cos(q2ref)); //Calculate desired angular velocity of motor 2
+
+ q1_ii = q1ref + q1_dot*T; //Adds the desired angle of motor 1 to the reference angle
+ q2_ii = q2ref + q2_dot*T; //Adds the desired angle of motor 2 to the reference angle
- q1ref = q1_ii;
- q2ref = q2_ii;
+ q1ref = q1_ii; //Makes new qref
+ q2ref = q2_ii; //Makes new qref
- q1_motor = -q1ref/r_trans;
- q2_motor = q2ref*5.0;
+ q1_motor = -q1ref/r_trans; //Sets the angle at which motor 1 needs to go, with ratio rotation/translation
+ q2_motor = q2ref*5.0; //Sets the angle at which motor 2 needs to go, scaled by 5
- engine_control1();
- engine_control2();
+ engine_control1(); //Call engine_control 1 function
+ engine_control2(); //Call engine_control 2 function
}
void v_des_calculate_qref()
{
- while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
+ while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
{
- if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
+ if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
{
- v_x = 0.05; //movement in +x direction
+ v_x = 0.05; //Movement in +x direction
v_y = 0.0;
- ledr = 0; //red
+ ledr = 0; //Led is red
ledb = 1;
ledg = 1;
}
- else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
+ else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
{
- v_y = 0.05; //Movement in +y direction
+ v_y = 0.05; //Movement in +y direction
v_x = 0.0;
- ledr = 1; //Green
+ ledr = 1; //Led is green
ledb = 1;
ledg = 0;
}
- else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
+ else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
{
- v_y = 0.0; //Movement in -x direction
+ v_y = 0.0; //Movement in -x direction
v_x = -0.05;
- ledr = 0; //Purple
+ ledr = 0; //Led is purple
ledb = 0;
ledg = 1;
}
- else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
+ else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
{
- v_y = -0.05; //Movement in -y direction
+ v_y = -0.05; //Movement in -y direction
v_x = 0.0;
- ledr = 1; //Blue
+ ledr = 1; //Led is blue
ledb = 0;
ledg = 1;
}
- else //If not higher than any threshold, motors will not turn at all
+ else //If not higher than any threshold, motors will not turn at all
{
v_x = 0;
v_y = 0;
- ledr = 0; //White
+ ledr = 0; //Led is white
ledb = 0;
ledg = 0;
}
- inverse_kinematics(); //Call inverse kinematics function
+ inverse_kinematics(); //Call inverse kinematics function
break;
}
}
-void printFunction()
-{
- pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
- pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
-}
-
-
-
-//------------------ Start main function --------------------------//
-
+//------------------ Start main function -----------------------------------------------------------------------------------------------------------//
int main()
{
- pc.baud(115200);
- pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
- pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
+ pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
- emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain
+ emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //Attach biquad elements to chain
emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 );
emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 );
- emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
+ emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
movag_tick.attach(&MovAg,T);
- func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T
- print_tick.attach(&printFunction,T2);
+ func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T
- button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
- //wait(0.2f); //Wait to avoid bouncing of button
- button2.rise(calibrate); //Calibrate threshold for 3 muscles
- //wait(0.2f); //Wait to avoid bouncing of button
+ button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
+ button2.rise(calibrate); //Calibrate threshold for 3 muscles
while(true)
{