Jorine Oosting / Mbed OS Project_Kinematics

Determine variables for angle control from kinematics (RKI)

Dependencies:   Matrix

TestRKI.cpp@1:11b728aa0077, 2018-10-30 (annotated)

Committer:
JorineOosting
Date:
Tue Oct 30 18:02:23 2018 +0000
Revision:
1:11b728aa0077
Project RKI test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JorineOosting 1:11b728aa0077 1 #include "mbed.h"
JorineOosting 1:11b728aa0077 2 #include <math.h>
JorineOosting 1:11b728aa0077 3
JorineOosting 1:11b728aa0077 4 // Define variables
JorineOosting 1:11b728aa0077 5 // Lengtes zijn in meters
JorineOosting 1:11b728aa0077 6 // Vaste variabelen:
JorineOosting 1:11b728aa0077 7 const double L0 = 0.088; // Afstand achterkant robot tot joint 2
JorineOosting 1:11b728aa0077 8 const double L1 = 0.288; // Hoogte van tafel tot joint 2
JorineOosting 1:11b728aa0077 9 const double L2 = 0.208; // Hoogte van tafel tot joint 1
JorineOosting 1:11b728aa0077 10 const double L3 = 0.212; // Lengte van de arm
JorineOosting 1:11b728aa0077 11 const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
JorineOosting 1:11b728aa0077 12 const double T = 0.002f; // Ticker value
JorineOosting 1:11b728aa0077 13 Ticker ref_ticker; // Ticker aanmaken
JorineOosting 1:11b728aa0077 14
JorineOosting 1:11b728aa0077 15
JorineOosting 1:11b728aa0077 16 // Variërende variabelen:
JorineOosting 1:11b728aa0077 17 double q1 = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
JorineOosting 1:11b728aa0077 18 double q2 = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
JorineOosting 1:11b728aa0077 19 double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
JorineOosting 1:11b728aa0077 20 double v_y; // Desired velocity end effecftor in y direction --> Determined by EMG signals
JorineOosting 1:11b728aa0077 21
JorineOosting 1:11b728aa0077 22 double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
JorineOosting 1:11b728aa0077 23 double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
JorineOosting 1:11b728aa0077 24
JorineOosting 1:11b728aa0077 25 double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
JorineOosting 1:11b728aa0077 26 double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
JorineOosting 1:11b728aa0077 27
JorineOosting 1:11b728aa0077 28 double q1_ii; // Reference signal for motorangle q1
JorineOosting 1:11b728aa0077 29 double q2_ii; // Reference signal for motorangle q2
JorineOosting 1:11b728aa0077 30
JorineOosting 1:11b728aa0077 31
JorineOosting 1:11b728aa0077 32 int main() {
JorineOosting 1:11b728aa0077 33 while (true) {
JorineOosting 1:11b728aa0077 34
JorineOosting 1:11b728aa0077 35 ticker.attach(ref_ticker);
JorineOosting 1:11b728aa0077 36
JorineOosting 1:11b728aa0077 37 Lq1 = q1*r_trans;
JorineOosting 1:11b728aa0077 38 Cq2 = q2/5.0;
JorineOosting 1:11b728aa0077 39
JorineOosting 1:11b728aa0077 40 q1_dot = (v_x + (v_y*(L2 + L3*sin(q2/5.0)))/(L0 + q1*r_trans + L3*cos(q2/5.0)))/r_trans;
JorineOosting 1:11b728aa0077 41 q2_dot = (5.0*v_y)/(L0 + q1*r_trans + L3*cos(q2/5.0));
JorineOosting 1:11b728aa0077 42
JorineOosting 1:11b728aa0077 43 q1_ii = q1 + q1_dot*T;
JorineOosting 1:11b728aa0077 44 q2_ii = q2 + q2_dot*T;
JorineOosting 1:11b728aa0077 45
JorineOosting 1:11b728aa0077 46 q1 = q1_ii;
JorineOosting 1:11b728aa0077 47 q2 = q2_ii;
JorineOosting 1:11b728aa0077 48
JorineOosting 1:11b728aa0077 49 }
JorineOosting 1:11b728aa0077 50 }
JorineOosting 1:11b728aa0077 51