Marijke Zondag
Alles samengevoegd geen errors, nog niet getest
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of
Project_script
by 
Marleen van Hoorn
Revision 32:56a8bd82e971, committed 2018-11-01
- Comitter:
- MarijkeZondag
- Date:
- Thu Nov 01 18:58:28 2018 +0000
- Parent:
- 31:0418ce58af56
- Commit message:
- RKI q1ref motor --> q2ref ; Trage translatie snelle rotatie?
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Nov 01 18:04:50 2018 +0000
+++ b/main.cpp Thu Nov 01 18:58:28 2018 +0000
@@ -82,28 +82,6 @@
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 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
-
-// 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 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=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_ii=0.0; // Reference signal for motorangle q1ref
-double q2_ii=0.0; // Reference signal for motorangle q2ref
//Variables PID controller
double PI = 3.14159;
@@ -124,6 +102,31 @@
double u1, u2;
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
+
+// 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 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=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_ii=0.0; // Reference signal for motorangle q1ref
+double q2_ii=0.0; // Reference signal for motorangle q2ref
+
+double q1_motor;
+double q2_motor;
//--------------Functions----------------------------------------------------------------------------------------------------------------------------//
@@ -182,7 +185,7 @@
wait(0.001f); //Does there need to be a wait?
}
Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
- Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
+ Threshold0 = Mean0*0.8; //Threshold calculation calve = 0.8*mean
break; //Stop. Threshold is calculated, we will use this further in the code
}
case 1: //If calibration state 1:
@@ -393,21 +396,23 @@
//------------------ Inversed Kinematics --------------------------------//
void inverse_kinematics()
-{
-
+{
//pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
- Lq1 = q1ref*r_trans;
- Cq2 = q2ref/5.0;
+ //Lq1 = q1ref*r_trans;
+ //Cq2 = q2ref/5.0;
- q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); //RKI systeem
- q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); //
+ q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //RKI systeem
+ q2_dot = v_y/(L3*cos(q2ref)); // Misschien gain toevoegen om te kijken of het dan werkt. Translatie gaat veel trager en moeizamer dan rotatie
- q1_ii = q1ref + (q1_dot/r_trans)*T; //Omgezet naar motorhoeken
- q2_ii = q2ref + (q2_dot*5.0)*T;
+ q1_ii = q1ref + q1_dot*T; //Omgezet naar motorhoeken
+ q2_ii = q2ref + q2_dot*T;
q1ref = q1_ii;
- q2ref = q2_ii;
+ q2ref = q2_ii;
+
+ q1_motor = q1ref*5.0;
+ q2_motor = q2ref/r_trans;
//pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);