ScaraArms.cpp

00001 // SCARA_Arms library
00002 // Code to compute the shoulder angles required to move a Dual-Arm SCARA robot to required x,y position
00003 // More information at http://robdobson.com/scara-arm-calculations
00004 // Copyright (C) Rob Dobson 2013-2015, MIT License
00005 
00006 #include "mbed.h"
00007 #include "math.h"
00008 #include "ScaraArms.h"
00009 
00010 const double PI =  3.14159;
00011 
00012 // Description of a Dual-Arm SCARA robot
00013 // The two arms of the robot are akin to two human arms with the hands permanently clasped together
00014 // The arms are always in the same plane (generally horizontal) and the hands hold a stick (also in the same plane)
00015 // On the end of the stick is the tool (pen/actuayor/nozzle/laser etc)
00016 
00017 // Constructor
00018 // The measurements required to describe a Dual-Arm SCARA robot of the kind described above are:
00019 // 1) Distance between shoulders
00020 // 2) Distance from shoulder to elbow
00021 // 3) Distance from elbow to hand
00022 // 4) Distance from hand to tool (length of stick holding the tool)
00023 
00024 ScaraArms::ScaraArms(double betweenShouldersMM, double shoulderToElbowMM, double elbowToHandMM, double handToToolMM)
00025 {
00026     _betweenShouldersMM = betweenShouldersMM;
00027     _shoulderToElbowMM = shoulderToElbowMM;
00028     _elbowToHandMM = elbowToHandMM;
00029     _handToToolMM = handToToolMM;
00030 }
00031 
00032 // Compute the angles at the shoulders given the required XY position of the tool in radians
00033 // Note that the way the angles are measured is that theta1 (the left arm angle) is measured counter-clockwise from 3 O'Clock
00034 // theta2 (the right arm angle) is measured clockwise from 9 O'Clock
00035 // To calculate both clockwise from 9 O'Clock use PI-theta2
00036 // To convert the angles to degrees multiply by 180/PI
00037 // The XY position has it's origin (0,0) at the left shoulder
00038 void ScaraArms::ConvertXYtoScara(double x, double y, double &theta1, double &theta2)
00039 {
00040     double L1 = _shoulderToElbowMM;
00041     double L2 = _elbowToHandMM;
00042     double L3 = _handToToolMM;
00043     double alpha = PI/4;
00044     double C = _betweenShouldersMM;
00045     double L4 = sqrt(L2*L2+L3*L3-2*L2*L3*cos(PI-alpha));
00046     double eta = acos((L3*L3+L4*L4-L2*L2)/(2*L3*L4));    
00047 
00048 //    printf("L1 %f, L2 %f, L3 %f, L4 %f, eta %f, alpha %f\r\n", L1, L2, L3, L4, eta*180/PI, alpha*180/PI);
00049 
00050     double d3 = sqrt((C-x)*(C-x)+y*y);
00051     double theta2c = atan2(y, (C-x));
00052     double theta2d = acos((d3*d3+L1*L1-L4*L4) / (2*L1*d3));
00053     theta2 = theta2c+theta2d;
00054     
00055     double x4 = C+L1*cos(PI-theta2);
00056     double y4 = L1*sin(PI-theta2);
00057     
00058     double delta = atan2(x4-x,y-y4);
00059     double x1 = x + L3*sin(delta - eta);
00060     double y1 = y - L3*cos(delta - eta);
00061 
00062     double d1 = sqrt(x1*x1+y1*y1);
00063     double theta1a = atan2(y1, x1);
00064     double theta1b = acos((d1*d1+L1*L1-L2*L2) / (2*L1*d1));
00065     
00066     theta1 = theta1a+theta1b;
00067 }
00068 
00069 // Compute the XY position of the tool given the angles at the shoulders
00070 // See the comments above for the reverse calculation concerning the way the angles are measured and the origin 
00071 // of the XY coordinates
00072 // These calculations use a different mathematical model as the conversion in this direction is simpler and
00073 // it provides a useful cross check
00074 void ScaraArms::ConvertScaratoXY(double theta1, double theta2, double &x, double &y)
00075 {
00076     double L1 = _shoulderToElbowMM;
00077     double L2 = _elbowToHandMM;
00078     double L3 = _handToToolMM;
00079     double alpha = PI/4;
00080     double C = _betweenShouldersMM;
00081 
00082     // Uses different maths as a cross check
00083     double theta1rev = theta1;
00084     double theta2rev = PI - theta2;
00085     double xh1 = L1 * cos(theta1rev);
00086     double yh1 = sin(theta1rev) * L1;
00087     double xh2 = C + L1 * cos(theta2rev);
00088     double yh2 = sin(theta2rev) * L1;
00089     double gamma = atan2((yh1-yh2), (xh1-xh2));
00090     double l = sqrt(pow(xh1-xh2,2)+pow(yh1-yh2,2));
00091     double h = sqrt(pow(L2,2)-pow(l/2,2));
00092     double xi = (xh1+xh2)/2 + (h * sin(gamma));
00093     double yi = (yh1+yh2)/2 + fabs(h * cos(gamma));
00094 
00095     double psichk = atan2((yi-yh2),(xh2-xi));
00096     x = xi - L3 * cos(psichk+alpha);
00097     y = yi + L3 * sin(psichk+alpha);    
00098 }
00099