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
- Committer:
- Duif
- Date:
- 2018-10-29
- Revision:
- 1:edac9d449caa
- Parent:
- 0:30f920202a5c
File content as of revision 1:edac9d449caa:
#include "mbed.h" #include "MODSERIAL.h" #include "math.h" double q1_diff; double q2_diff; const double pi = 3.14159265358979323846; //definition of pi double sq = 2.0; //to square numbers const double x0 = 80.0; //zero x position after homing const double y0 = 141.0; //zero y position after homing const double L1 = 250.0; //length of the first link const double L3 = 350.0; //length of the second link double x = 500.0; //x variable that will be set; input double y = 141.0; //y variable that will be set; input //reference angles of the starting position double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); 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)))); double q2_0_enc = q2_0 + q1_0; //function to calculate the angle from double makeAngleq1(double x, double y){ //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 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 //double q2_motor = pi - q2; //because q2 represents the angle at joint two and not at the motor a calculation has to be done q1_diff = -2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint //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 //double q_diff[2] = {q1_diff,q2_diff}; //make a vector so the answer can be returned //q_diff[0] = q1_diff and q_diff[1] = q2_diff return q1_diff; } //function to calculate the angle from double makeAngleq2(double x, double y){ 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 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 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 //q1_diff = 2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint 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 //double q_diff[2] = {q1_diff,q2_diff}; //make a vector so the answer can be returned //q_diff[0] = q1_diff and q_diff[1] = q2_diff return q2_diff; } int main() { while (true) { } }