Revision 0:9a3fc5ff8de3, committed 2015-05-15

Comitter:

Bobty

Date:

Fri May 15 08:50:58 2015 +0000

Commit message:

Initial revision

Changed in this revision

diff -r 000000000000 -r 9a3fc5ff8de3 ScaraArms.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ScaraArms.cpp	Fri May 15 08:50:58 2015 +0000
@@ -0,0 +1,99 @@
+// SCARA_Arms library
+// Code to compute the shoulder angles required to move a Dual-Arm SCARA robot to required x,y position
+// More information at http://robdobson.com/scara-arm-calculations
+// Copyright (C) Rob Dobson 2013-2015, MIT License
+
+#include "mbed.h"
+#include "math.h"
+#include "ScaraArms.h"
+
+const double PI =  3.14159;
+
+// Description of a Dual-Arm SCARA robot
+// The two arms of the robot are akin to two human arms with the hands permanently clasped together
+// The arms are always in the same plane (generally horizontal) and the hands hold a stick (also in the same plane)
+// On the end of the stick is the tool (pen/actuayor/nozzle/laser etc)
+
+// Constructor
+// The measurements required to describe a Dual-Arm SCARA robot of the kind described above are:
+// 1) Distance between shoulders
+// 2) Distance from shoulder to elbow
+// 3) Distance from elbow to hand
+// 4) Distance from hand to tool (length of stick holding the tool)
+
+ScaraArms::ScaraArms(double betweenShouldersMM, double shoulderToElbowMM, double elbowToHandMM, double handToToolMM)
+{
+    _betweenShouldersMM = betweenShouldersMM;
+    _shoulderToElbowMM = shoulderToElbowMM;
+    _elbowToHandMM = elbowToHandMM;
+    _handToToolMM = handToToolMM;
+}
+
+// Compute the angles at the shoulders given the required XY position of the tool in radians
+// Note that the way the angles are measured is that theta1 (the left arm angle) is measured counter-clockwise from 3 O'Clock
+// theta2 (the right arm angle) is measured clockwise from 9 O'Clock
+// To calculate both clockwise from 9 O'Clock use PI-theta2
+// To convert the angles to degrees multiply by 180/PI
+// The XY position has it's origin (0,0) at the left shoulder
+void ScaraArms::ConvertXYtoScara(double x, double y, double &theta1, double &theta2)
+{
+    double L1 = _shoulderToElbowMM;
+    double L2 = _elbowToHandMM;
+    double L3 = _handToToolMM;
+    double alpha = PI/4;
+    double C = _betweenShouldersMM;
+    double L4 = sqrt(L2*L2+L3*L3-2*L2*L3*cos(PI-alpha));
+    double eta = acos((L3*L3+L4*L4-L2*L2)/(2*L3*L4));    
+
+//    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);
+
+    double d3 = sqrt((C-x)*(C-x)+y*y);
+    double theta2c = atan2(y, (C-x));
+    double theta2d = acos((d3*d3+L1*L1-L4*L4) / (2*L1*d3));
+    theta2 = theta2c+theta2d;
+    
+    double x4 = C+L1*cos(PI-theta2);
+    double y4 = L1*sin(PI-theta2);
+    
+    double delta = atan2(x4-x,y-y4);
+    double x1 = x + L3*sin(delta - eta);
+    double y1 = y - L3*cos(delta - eta);
+
+    double d1 = sqrt(x1*x1+y1*y1);
+    double theta1a = atan2(y1, x1);
+    double theta1b = acos((d1*d1+L1*L1-L2*L2) / (2*L1*d1));
+    
+    theta1 = theta1a+theta1b;
+}
+
+// Compute the XY position of the tool given the angles at the shoulders
+// See the comments above for the reverse calculation concerning the way the angles are measured and the origin 
+// of the XY coordinates
+// These calculations use a different mathematical model as the conversion in this direction is simpler and
+// it provides a useful cross check
+void ScaraArms::ConvertScaratoXY(double theta1, double theta2, double &x, double &y)
+{
+    double L1 = _shoulderToElbowMM;
+    double L2 = _elbowToHandMM;
+    double L3 = _handToToolMM;
+    double alpha = PI/4;
+    double C = _betweenShouldersMM;
+
+    // Uses different maths as a cross check
+    double theta1rev = theta1;
+    double theta2rev = PI - theta2;
+    double xh1 = L1 * cos(theta1rev);
+    double yh1 = sin(theta1rev) * L1;
+    double xh2 = C + L1 * cos(theta2rev);
+    double yh2 = sin(theta2rev) * L1;
+    double gamma = atan2((yh1-yh2), (xh1-xh2));
+    double l = sqrt(pow(xh1-xh2,2)+pow(yh1-yh2,2));
+    double h = sqrt(pow(L2,2)-pow(l/2,2));
+    double xi = (xh1+xh2)/2 + (h * sin(gamma));
+    double yi = (yh1+yh2)/2 + fabs(h * cos(gamma));
+
+    double psichk = atan2((yi-yh2),(xh2-xi));
+    x = xi - L3 * cos(psichk+alpha);
+    y = yi + L3 * sin(psichk+alpha);    
+}
+

diff -r 000000000000 -r 9a3fc5ff8de3 ScaraArms.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ScaraArms.h	Fri May 15 08:50:58 2015 +0000
@@ -0,0 +1,47 @@
+// SCARA_Arms library
+// Code to compute the shoulder angles required to move a Dual-Arm SCARA robot to required x,y position
+// More information at http://robdobson.com/scara-arm-calculations
+// Copyright (C) Rob Dobson 2013-2015, MIT License
+
+#ifndef SCARAARMS_H
+#define SCARAARMS_H
+
+// Description of a Dual-Arm SCARA robot
+// The two arms of the robot are akin to two human arms with the hands permanently clasped together
+// The arms are always in the same plane (generally horizontal) and the hands hold a stick (also in the same plane)
+// On the end of the stick is the tool (pen/actuayor/nozzle/laser etc)
+
+class ScaraArms
+{
+public:
+    // Constructor
+    // The measurements required to describe a Dual-Arm SCARA robot of the kind described above are:
+    // 1) Distance between shoulders
+    // 2) Distance from shoulder to elbow
+    // 3) Distance from elbow to hand
+    // 4) Distance from hand to tool (length of stick holding the tool)
+    ScaraArms(double betweenShouldersMM, double shoulderToElbowMM, double elbowToHandMM, double handToToolMM);
+    
+    // Compute the angles at the shoulders given the required XY position of the tool in radians
+    // Note that the way the angles are measured is that theta1 (the left arm angle) is measured counter-clockwise from 3 O'Clock
+    // theta2 (the right arm angle) is measured clockwise from 9 O'Clock
+    // To calculate both clockwise from 9 O'Clock use PI-theta2
+    // To convert the angles to degrees multiply by 180/PI
+    // The XY position has it's origin (0,0) at the left shoulder
+    void ConvertXYtoScara(double x, double y, double &theta1, double &theta2);
+
+    // Compute the XY position of the tool given the angles at the shoulders
+    // See the comments above for the reverse calculation concerning the way the angles are measured and the origin 
+    // of the XY coordinates
+    // These calculations use a different mathematical model as the conversion in this direction is simpler and
+    // it provides a useful cross check
+    void ConvertScaratoXY(double theta1, double theta2, double &x, double &y);
+    
+private:
+    double _betweenShouldersMM;
+    double _shoulderToElbowMM;
+    double _elbowToHandMM;
+    double _handToToolMM;
+};
+
+#endif
\ No newline at end of file