Maths/Code for forward kinematics of a 5-bar SCARA January 31, 2016 09:50AM |
Registered: 8 years ago Posts: 5 |
Re: Maths/Code for forward kinematics of a 5-bar SCARA February 02, 2016 07:44AM |
Registered: 12 years ago Posts: 30 |
// length of arms #define L1 35 //both lower arms #define L2 55.1 //left upper arm to pen #define L3 13.2 //pen to left upper arm joint #define L4 45 //reight upper arm // origin points of left and right servo #define O1X 24//22 #define O1Y -25 #define O2X 49//47 #define O2Y -25 double return_angle(double a, double b, double c) { // cosine rule for angle between c and a return acos((a * a + c * c - b * b) / (2 * a * c)); } void set_XY(double Tx, double Ty) // Tx, Ty = Target X,Y { delay(1); double dx, dy, c, a1, a2, Hx, Hy; // Calcutlating the left servo angle // calculate triangle between pen, servoLeft and arm joint // cartesian dx/dy dx = Tx - O1X; dy = Ty - O1Y; // polar lemgth (c) and angle (a1) c = sqrt(dx * dx + dy * dy); // a1 = atan2(dy, dx); // a2 = return_angle(L1, L2, c); servo2.writeMicroseconds(floor(((a2 + a1 - M_PI) * SERVOFAKTORLEFT) + SERVOLEFTNULL)); // Calcutlating the right servo angle // calculate joint arm point for triangle of the right servo arm (joint near the pen) a2 = return_angle(L2, L1, c); Hx = Tx + L3 * cos((a1 - a2 + 0.621) + M_PI); //36,5° Hy = Ty + L3 * sin((a1 - a2 + 0.621) + M_PI); // calculate triangle between pen joint, servoRight and arm joint dx = Hx - O2X; dy = Hy - O2Y; c = sqrt(dx * dx + dy * dy); a1 = atan2(dy, dx); a2 = return_angle(L1, L4, c); servo3.writeMicroseconds(floor(((a1 - a2) * SERVOFAKTORRIGHT) + SERVORIGHTNULL)); }
Re: Maths/Code for forward kinematics of a 5-bar SCARA February 09, 2016 02:21PM |
Registered: 8 years ago Posts: 5 |