Mathieu van der Donck / Mbed 2 deprecated Inverse_kinematics

inverse kinematics

Dependencies:   QEI mbed

main.cpp@1:26bd86929178, 2018-10-30 (annotated)

Committer:
s1737619
Date:
Tue Oct 30 15:21:03 2018 +0000
Revision:
1:26bd86929178
Parent:
0:d1956779c04c 
inverse kinematics

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1737619 0:d1956779c04c 1 #include "mbed.h"
s1737619 0:d1956779c04c 2 #include "QEI.h"
s1737619 0:d1956779c04c 3
s1737619 0:d1956779c04c 4
s1737619 0:d1956779c04c 5 QEI encoder1(D10, D11, NC, 32);
s1737619 0:d1956779c04c 6 QEI encoder2(D12, D13, NC, 32);
s1737619 0:d1956779c04c 7
s1737619 0:d1956779c04c 8 double L1 = 40.0; //cm
s1737619 0:d1956779c04c 9 double L2 = 40.0; //cm
s1737619 0:d1956779c04c 10
s1737619 0:d1956779c04c 11 /*void forward(double M1,double M2,double & X1,double &Y1) //Forward kinematics
s1737619 0:d1956779c04c 12 {
s1737619 0:d1956779c04c 13 THETA1 = M1/9.0; //Angle of motor 1 after gears
s1737619 0:d1956779c04c 14 THETA2 = M2/4.0; //Angle of motor 2 after gears
s1737619 0:d1956779c04c 15 X1 = L1 * cos(THETA1) + L2 * cos(THETA1 + THETA2); // gives x coordinates
s1737619 0:d1956779c04c 16 Y1 = L1 * sin(THETA1) + L2 * sin(THETA1 + THETA2); // gives y coordinates
s1737619 0:d1956779c04c 17 }*/
s1737619 1:26bd86929178 18 /*
s1737619 0:d1956779c04c 19 void inverse(double X1, double Y1, double & THETA1, double & THETA2) //inverse kinematics
s1737619 0:d1956779c04c 20 {
s1737619 0:d1956779c04c 21 THETA1 = (acos((pow(-L1,2.0)-pow(X1,2.0)-pow(Y1,2.0)+pow(L2,2.0))/(-2.0*L1*sqrt(pow(X1,2.0)+pow(Y1,2.0))))+atan(Y1/X1))*9.0; //gives theta M1 in radians
s1737619 1:26bd86929178 22 //THETA2 = (acos((pow(X1,2.0)+ pow(Y1,2.0)-pow(L1,2.0)-pow(L2,2.0))/(-2.0*L1*L2))-3.1416)*4.0; //gives theta M2 in radians
s1737619 1:26bd86929178 23 THETA2= (acos(X1-L1*cos(THETA1))/(L2))*4.0; //gives theta M2 in radians
s1737619 0:d1956779c04c 24 }
s1737619 1:26bd86929178 25 */
s1737619 1:26bd86929178 26
s1737619 1:26bd86929178 27 void inverse(double X1, double Y1, double & thetaM1, double & thetaM2)
s1737619 1:26bd86929178 28 {
s1737619 1:26bd86929178 29 double L1 = 40.0; // %define length of arm 1 attached to gear
s1737619 1:26bd86929178 30 double L3 = sqrt(pow(X1,2.0)+pow(Y1,2.0));
s1737619 1:26bd86929178 31 double q3 = atan(Y1/X1);
s1737619 1:26bd86929178 32 double q4 = 2.0*asin(0.5*L3/L1);
s1737619 1:26bd86929178 33 thetaM1 = (0.5*3.1416-0.5*q4+q3)*9.0;
s1737619 1:26bd86929178 34 thetaM2 = (3.1416-thetaM1/9.0-q4)*4.0;
s1737619 1:26bd86929178 35 }
s1737619 0:d1956779c04c 36
s1737619 0:d1956779c04c 37
s1737619 0:d1956779c04c 38 double meas_pos1 () //Encoder functie
s1737619 0:d1956779c04c 39 {
s1737619 0:d1956779c04c 40 double encoderpulses = encoder1.getPulses(); //number of pulses
s1737619 0:d1956779c04c 41 double motorangle = encoderpulses/32.0/131.25*2.0*3.1416; //motor angle in radians
s1737619 0:d1956779c04c 42 return motorangle;
s1737619 0:d1956779c04c 43 }
s1737619 0:d1956779c04c 44
s1737619 0:d1956779c04c 45 double meas_pos2 () //Encoder functie
s1737619 0:d1956779c04c 46 {
s1737619 0:d1956779c04c 47 double encoderpulses = encoder2.getPulses(); //number of pulses
s1737619 0:d1956779c04c 48 double motorangle = encoderpulses/32.0/131.25*2.0*3.1416; //motor angle in radians
s1737619 0:d1956779c04c 49 return motorangle;
s1737619 0:d1956779c04c 50 }
s1737619 0:d1956779c04c 51
s1737619 0:d1956779c04c 52 void kinematics (double unit_vX, double unit_vY, double & THETA1, double & THETA2)// Kinematics function, takes imput between 1 and -1
s1737619 0:d1956779c04c 53 {
s1737619 0:d1956779c04c 54 double Ts = 0.0001; //timestep(see whatever ticker we attach to this)
s1737619 0:d1956779c04c 55 double V_max = 30.0; //Maximum velocity in either direction
s1737619 0:d1956779c04c 56
s1737619 0:d1956779c04c 57 double deltaX = Ts*V_max*unit_vX; // imput to delta
s1737619 0:d1956779c04c 58 double deltaY = Ts*V_max*unit_vY;
s1737619 0:d1956779c04c 59
s1737619 0:d1956779c04c 60 static double X1;
s1737619 0:d1956779c04c 61 static double Y1;
s1737619 0:d1956779c04c 62 X1 += deltaX;
s1737619 0:d1956779c04c 63 Y1 += deltaY;
s1737619 0:d1956779c04c 64 inverse(X1, Y1, THETA1, THETA2);
s1737619 0:d1956779c04c 65 }
s1737619 0:d1956779c04c 66
s1737619 0:d1956779c04c 67
s1737619 0:d1956779c04c 68
s1737619 0:d1956779c04c 69
s1737619 0:d1956779c04c 70 int main()
s1737619 0:d1956779c04c 71 {
s1737619 0:d1956779c04c 72 while (true) {
s1737619 0:d1956779c04c 73 }
s1737619 0:d1956779c04c 74 }