Jorine Oosting
/
Project_Kinematics
Determine variables for angle control from kinematics (RKI)
main.cpp@0:00721d3e1cf7, 2018-10-29 (annotated)
- Committer:
- JorineOosting
- Date:
- Mon Oct 29 17:45:31 2018 +0000
- Revision:
- 0:00721d3e1cf7
First version Kinematics variables symbolic;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JorineOosting | 0:00721d3e1cf7 | 1 | #include "mbed.h" |
JorineOosting | 0:00721d3e1cf7 | 2 | #include <math.h> |
JorineOosting | 0:00721d3e1cf7 | 3 | #include "Matrix.h" |
JorineOosting | 0:00721d3e1cf7 | 4 | // Define variables |
JorineOosting | 0:00721d3e1cf7 | 5 | // Lengtes zijn in meters |
JorineOosting | 0:00721d3e1cf7 | 6 | double L0 = 0.088; // Afstand achterkant robot tot joint 2 |
JorineOosting | 0:00721d3e1cf7 | 7 | double L1 = 0.288; // Hoogte van tafel tot joint 2 |
JorineOosting | 0:00721d3e1cf7 | 8 | double L3 = 0.212; // Lengte van de arm |
JorineOosting | 0:00721d3e1cf7 | 9 | // double r_trans; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation |
JorineOosting | 0:00721d3e1cf7 | 10 | double q1; // Translational joint, waarde uit encoder |
JorineOosting | 0:00721d3e1cf7 | 11 | double q2; // Rotational joint, waarde uit encoder --> q2 is 1/5 * waarde vd encoder (omrekeningsfactor) |
JorineOosting | 0:00721d3e1cf7 | 12 | double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals |
JorineOosting | 0:00721d3e1cf7 | 13 | double v_y; // Desired velocity end effecftor in y direction --> Determined by EMG signals |
JorineOosting | 0:00721d3e1cf7 | 14 | |
JorineOosting | 0:00721d3e1cf7 | 15 | double J2_11 = 1.0; // Berekeningen voor J'' komen uit MATLAB. [J2_11 J2_12; J2_21 J2_22] |
JorineOosting | 0:00721d3e1cf7 | 16 | double J2_12 = -L3*sin(q2); |
JorineOosting | 0:00721d3e1cf7 | 17 | double J2_21 = 0.0; |
JorineOosting | 0:00721d3e1cf7 | 18 | double J2_22 = L3*cos(q2); |
JorineOosting | 0:00721d3e1cf7 | 19 | double T; // Ticker value |
JorineOosting | 0:00721d3e1cf7 | 20 | |
JorineOosting | 0:00721d3e1cf7 | 21 | double q_dot_1 = v_x +(v_y*sin(q2))/cos(q2); // Deze waarde gaat naar de motor |
JorineOosting | 0:00721d3e1cf7 | 22 | double q_dot_2 = v_y/(L3*cos(q2)); |
JorineOosting | 0:00721d3e1cf7 | 23 | |
JorineOosting | 0:00721d3e1cf7 | 24 | double q_t_1 = L0 + q1 + L3*cos(q2); |
JorineOosting | 0:00721d3e1cf7 | 25 | double q_t_2 = L1 + L3*sin(q2); |
JorineOosting | 0:00721d3e1cf7 | 26 | |
JorineOosting | 0:00721d3e1cf7 | 27 | double q_tT_1 = L0 + q1 + T*(v_x + (v_y*sin(q2))/cos(q2)) + L3*cos(q2); |
JorineOosting | 0:00721d3e1cf7 | 28 | double q_tT_2 = L1 + L3*sin(q2) + (T*v_y)/(L3*cos(q2)); |
JorineOosting | 0:00721d3e1cf7 | 29 | |
JorineOosting | 0:00721d3e1cf7 | 30 | |
JorineOosting | 0:00721d3e1cf7 | 31 | |
JorineOosting | 0:00721d3e1cf7 | 32 | int main() { |
JorineOosting | 0:00721d3e1cf7 | 33 | while (true) { |
JorineOosting | 0:00721d3e1cf7 | 34 | |
JorineOosting | 0:00721d3e1cf7 | 35 | } |
JorineOosting | 0:00721d3e1cf7 | 36 | } |
JorineOosting | 0:00721d3e1cf7 | 37 |