Jorine Oosting
/
Project_Kinematics
Determine variables for angle control from kinematics (RKI)
Diff: main.cpp
- Revision:
- 0:00721d3e1cf7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 29 17:45:31 2018 +0000 @@ -0,0 +1,37 @@ +#include "mbed.h" +#include <math.h> +#include "Matrix.h" +// Define variables +// Lengtes zijn in meters +double L0 = 0.088; // Afstand achterkant robot tot joint 2 +double L1 = 0.288; // Hoogte van tafel tot joint 2 +double L3 = 0.212; // Lengte van de arm +// double r_trans; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation +double q1; // Translational joint, waarde uit encoder +double q2; // Rotational joint, waarde uit encoder --> q2 is 1/5 * waarde vd encoder (omrekeningsfactor) +double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals +double v_y; // Desired velocity end effecftor in y direction --> Determined by EMG signals + +double J2_11 = 1.0; // Berekeningen voor J'' komen uit MATLAB. [J2_11 J2_12; J2_21 J2_22] +double J2_12 = -L3*sin(q2); +double J2_21 = 0.0; +double J2_22 = L3*cos(q2); +double T; // Ticker value + +double q_dot_1 = v_x +(v_y*sin(q2))/cos(q2); // Deze waarde gaat naar de motor +double q_dot_2 = v_y/(L3*cos(q2)); + +double q_t_1 = L0 + q1 + L3*cos(q2); +double q_t_2 = L1 + L3*sin(q2); + +double q_tT_1 = L0 + q1 + T*(v_x + (v_y*sin(q2))/cos(q2)) + L3*cos(q2); +double q_tT_2 = L1 + L3*sin(q2) + (T*v_y)/(L3*cos(q2)); + + + +int main() { + while (true) { + + } +} +