Jorine Oosting / Mbed OS Project_Kinematics

Determine variables for angle control from kinematics (RKI)

Dependencies:   Matrix

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?

UserRevisionLine numberNew 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