Marleen van Hoorn
inverse kinematics toegevoegd en tickers samengevoegd tot 1 ticker
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Fork of
Project_script
by 
Marijke Zondag
Revision 25:bbef09ff226b, committed 2018-10-31
- Comitter:
- Marle
- Date:
- Wed Oct 31 09:28:45 2018 +0000
- Parent:
- 24:6d63ad38fef7
- Commit message:
- inverse kinematics toegevoegd en tickers samengevoegd to 1 ticker
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 6d63ad38fef7 -r bbef09ff226b main.cpp
--- a/main.cpp Tue Oct 30 13:09:58 2018 +0000
+++ b/main.cpp Wed Oct 31 09:28:45 2018 +0000
@@ -32,10 +32,7 @@
//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
//Tickers
-Ticker HIDScope_tick; //Ticker for HIDScope
-Ticker filter_tick; //Ticker for EMG filter
-Ticker MovAg_tick; //Ticker to calculate Moving Average
-Ticker Motor_tick; //Ticker motor aansturen
+Ticker ticker;
//Global variables
const float T = 0.002f; //Ticker period
@@ -76,6 +73,31 @@
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
+// Inverse Kinematica variabelen
+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 T = 0.002f; // Ticker value
+
+// Variërende variabelen inverse kinematics:
+double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q2ref = 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 Lq1; // Translatieafstand als gevolg van motor rotation joint 1
+double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
+
+double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
+double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
+
+double q1_ii; // Reference signal for motorangle q1ref
+double q2_ii; // Reference signal for motorangle q2ref
+
+
//Functions
void switch_to_calibrate()
@@ -246,53 +268,60 @@
EMGFilter2();
}
-void motor_control()
+void inverse_kinematics()
+{
+ Lq1 = q1ref*r_trans;
+ Cq2 = q2ref/5.0;
+
+ q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));
+ q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));
+
+ q1_ii = q1ref + q1_dot*T;
+ q2_ii = q2ref + q2_dot*T;
+
+ q1ref = q1_ii;
+ q2ref = q2_ii;
+}
+
+void v_des_calculate_qref()
{
while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
{
-
- if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
+ if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
{
- pwmpin1 = 1;
- directionpin1.write(1); //translatie vooruit
-
- ledr = 1; //Blue
- ledb = 0;
- ledg = 1;
-
- }
- else if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction
- {
- pwmpin2 = 1;
- directionpin2.write(1); //rotatie omhoog
- ledr = 0; //red
- ledb = 1;
- ledg = 1;
+ v_x = 1.0; //beweging in +x direction
+ ledr = 0; //red
+ ledb = 1;
+ ledg = 1;
}
else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
{
-
- pwmpin1 = 1;
- pwmpin2 = 1;
- directionpin1.write(0); //translatie achteruit
- directionpin2.write(0); //rotatie omlaag
-
- ledr = 1; //green
- ledb = 1;
- ledg = 0;
+ v_y = 1.0; //beweging in +y direction
+ ledr = 1; //green
+ ledb = 1;
+ ledg = 0;
}
- else //If not higher than the threshold, motors will not turn at all
+
+ else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
{
- pwmpin1 = 0; //Motoren doen niets
- pwmpin2 = 0;
-
+ v_x = -v_x;
+ v_y = -v_y;
+ ledr = 1; //Blue
+ ledb = 0;
+ ledg = 1;
+ }
+ else //If not higher than the threshold, motors will not turn at all
+ {
+ v_x = 0;
+ v_y = 0;
ledr = 0; //white
ledb = 0;
ledg = 0;
}
-
+
break;
}
+ inverse_kinematics();
}
@@ -308,10 +337,10 @@
while(true)
{
- filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
- MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
- Motor_tick.attach(&motor_control,T); //Test motor control
-
+ ticker.attach(&emg_filtered,T); //EMG signals filtered every T sec.
+ ticker.attach(&MovAg,T); //Moving average calculation every T sec.
+ ticker.attach(&v_des_calculate_qref,T); //v_des determined every T
+
// HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)