x and y should be changed to represent the valid inputs. Takes a setpoint x,y and transfers it to required motor angles
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?
User | Revision | Line number | New 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 | } |