Silvie van den Bogaard / Mbed 2 deprecated Inverse_kinematics

x and y should be changed to represent the valid inputs. Takes a setpoint x,y and transfers it to required motor angles

Dependencies:   MODSERIAL mbed

main.cpp@1:edac9d449caa, 2018-10-29 (annotated)

Committer:
Duif
Date:
Mon Oct 29 13:59:50 2018 +0000
Revision:
1:edac9d449caa
Parent:
0:30f920202a5c 
Inverse Kinematics. Change x and y to what should be the input

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Duif 0:30f920202a5c 1 #include "mbed.h"
Duif 0:30f920202a5c 2 #include "MODSERIAL.h"
Duif 0:30f920202a5c 3 #include "math.h"
Duif 0:30f920202a5c 4
Duif 1:edac9d449caa 5 double q1_diff;
Duif 1:edac9d449caa 6 double q2_diff;
Duif 0:30f920202a5c 7 const double pi = 3.14159265358979323846; //definition of pi
Duif 1:edac9d449caa 8 double sq = 2.0; //to square numbers
Duif 0:30f920202a5c 9 const double x0 = 80.0; //zero x position after homing
Duif 0:30f920202a5c 10 const double y0 = 141.0; //zero y position after homing
Duif 0:30f920202a5c 11 const double L1 = 250.0; //length of the first link
Duif 0:30f920202a5c 12 const double L3 = 350.0; //length of the second link
Duif 0:30f920202a5c 13
Duif 0:30f920202a5c 14 double x = 500.0; //x variable that will be set; input
Duif 0:30f920202a5c 15 double y = 141.0; //y variable that will be set; input
Duif 0:30f920202a5c 16
Duif 0:30f920202a5c 17 //reference angles of the starting position
Duif 1:edac9d449caa 18 double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
Duif 1:edac9d449caa 19 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
Duif 1:edac9d449caa 20 double q2_0_enc = q2_0 + q1_0;
Duif 0:30f920202a5c 21
Duif 0:30f920202a5c 22 //function to calculate the angle from
Duif 1:edac9d449caa 23 double makeAngleq1(double x, double y){
Duif 1:edac9d449caa 24
Duif 1:edac9d449caa 25 //double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
Duif 1:edac9d449caa 26
Duif 1:edac9d449caa 27 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
Duif 1:edac9d449caa 28
Duif 1:edac9d449caa 29 //double q2_motor = pi - q2; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
Duif 0:30f920202a5c 30
Duif 1:edac9d449caa 31 q1_diff = -2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Duif 1:edac9d449caa 32 //q2_diff = 2.0*(q2_motor - q2_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Duif 0:30f920202a5c 33
Duif 1:edac9d449caa 34 //double q_diff[2] = {q1_diff,q2_diff}; //make a vector so the answer can be returned
Duif 1:edac9d449caa 35 //q_diff[0] = q1_diff and q_diff[1] = q2_diff
Duif 1:edac9d449caa 36
Duif 1:edac9d449caa 37
Duif 1:edac9d449caa 38 return q1_diff;
Duif 1:edac9d449caa 39 }
Duif 0:30f920202a5c 40
Duif 1:edac9d449caa 41
Duif 1:edac9d449caa 42 //function to calculate the angle from
Duif 1:edac9d449caa 43 double makeAngleq2(double x, double y){
Duif 1:edac9d449caa 44
Duif 1:edac9d449caa 45 double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
Duif 1:edac9d449caa 46
Duif 1:edac9d449caa 47 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
Duif 0:30f920202a5c 48
Duif 1:edac9d449caa 49 double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
Duif 1:edac9d449caa 50
Duif 1:edac9d449caa 51 //q1_diff = 2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Duif 1:edac9d449caa 52 q2_diff = -2.0*(q2_motor - q2_0_enc); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Duif 0:30f920202a5c 53
Duif 1:edac9d449caa 54 //double q_diff[2] = {q1_diff,q2_diff}; //make a vector so the answer can be returned
Duif 1:edac9d449caa 55 //q_diff[0] = q1_diff and q_diff[1] = q2_diff
Duif 0:30f920202a5c 56
Duif 1:edac9d449caa 57
Duif 1:edac9d449caa 58 return q2_diff;
Duif 0:30f920202a5c 59 }
Duif 0:30f920202a5c 60
Duif 0:30f920202a5c 61 int main()
Duif 0:30f920202a5c 62 {
Duif 0:30f920202a5c 63 while (true) {
Duif 0:30f920202a5c 64
Duif 0:30f920202a5c 65 }
Duif 0:30f920202a5c 66 }