Ambidextrous SCARA Kinematics

Ambidextrous-SCARA.scad

/**
 * Ambidextrous SCARA Kinematics with optimized mathematics
 */

// Lengths of the arm segments
// If the lengths are the same the maths can be optimized
arm1_length = 5;
arm2_length = 3;
arm_sum = arm1_length + arm2_length;

// Animate over a circular path
tt = ($t * 10) - floor($t * 10);

RIGHT = tt > 0.5;
LEFT = !RIGHT;

$fn = 30;

// Move Z up and down in time
ZPOS = sin($t * 360) * 4 + 5.5;

// Move in a circular path
pos = circular_position(tt);

if (RIGHT)
  translate([LEFT?-arm_sum:0, 0, 0])
    right_arm(pos[0], pos[1], arm1_length, arm2_length, ZPOS);

if (LEFT)
  translate([RIGHT?arm_sum:0, 0, 0])
    left_arm(pos[0], pos[1], arm1_length, arm2_length, ZPOS);

function sq(x) = x * x;
function hypot2(a, b) = sq(a)+sq(b);
function length(a, b) = sqrt(hypot2(a, b));

// Inverse kinematics functions for a scara style arm
// See http://forums.reprap.org/read.php?185,283327

//
// Angle equations for differing arm lengths
//
function angA(L1, L2, D2) = acos((D2 + (sq(L2) - sq(L1))) / (2 * L2 * sqrt(D2)));
function angB(L1, L2, D2) = acos((D2 - hypot2(L2, L1)) / (2 * L1 * L2));
function l_ang2(x, y, L1, L2, D2) = atan2(y, x) - angA(L1, L2, D2);
function r_ang2(x, y, L1, L2, D2) = atan2(y, x) + angA(L1, L2, D2);

//
// Simplified angle equations for identical arm lengths
//
function s_angA(L1, D2) = acos(D2 / (2 * L1 * sqrt(D2)));
function s_angB(L1, D2) = acos((D2 - (2 * sq(L1))) / (2 * sq(L1)));
function sl_ang2(x, y, L1, D2) = atan2(y, x) - s_angA(L1, D2);
function sr_ang2(x, y, L1, D2) = atan2(y, x) + s_angA(L1, D2);

/**
 * Left-Arm solution for SCARA
 */
module left_arm(x, y, L1, L2, Z) {
  D2 = hypot2(x, y);
  B = -((L1 == L2) ? s_angB(L1, D2) : angB(L1, L2, D2));
  A = ((L1 == L2) ? sl_ang2(x, y, L1, D2) : l_ang2(x, y, L1, L2, D2)) - B;
  scara_arm(A, B, L1, L2, Z);
}

/**
 * Right-Arm solution for SCARA
 */
module right_arm(x, y, L1, L2, Z) {
  D2 = hypot2(x, y);
  B = (L1 == L2) ? s_angB(L1, D2) : angB(L1, L2, D2);
  A = ((L1 == L2) ? sr_ang2(x, y, L1, D2) : r_ang2(x, y, L1, L2, D2)) - B;
  scara_arm(A, B, L1, L2, Z);
}

module scara_arm(A, B, L1, L2, Z) {
  ARM_R = 0.3;

  translate([0,0,9.5-Z]) {
    difference() {
      color([1,1,1]) cylinder(r=arm_sum, h=0.25, center=true, $fn=100);
      translate([0,-1.5*arm_sum,0])
        cube([3*arm_sum, 3*arm_sum-8, 10], center=true);
    }
    extrusion(Z-0.6);
  }

  cylinder(r=0.7,h=9.5, $fn=90);
  color([0.5,0.5,1]) translate([0,0,9.5]) cylinder(r=1.7,h=1,center=true);

  color([0.3,0.7,0.4])
  translate([0,0,9.5])
  rotate([0, 0, A]) {      // Arm A
    segment(L1, ARM_R);
    translate([L1, 0, 0]) {
      sphere(r=ARM_R*1.1);
      rotate([0, 0, B]) {  // Arm B
        segment(L2, ARM_R);
        translate([L2,0,-0.7])
          cylinder(r1=0.1, r2=0.5, h=1);
      }
    }
  }
}

// Draw an arm segment with the given color and length.
module segment(l, r) { rotate([0,90]) cylinder(r=r,h=l,$fn=18); }

module curve() polygon([for(a=[0 : 0.0125 : 0.5]) circular_position(a)]);

module extrusion(Z) {
  color([1,0.4,0.5]) linear_extrude(Z-0.1) difference() {
    curve();
    offset(-.05) curve();
  }
}

function circular_position(f) = [ 1.5* sin(f*720+180) * arm2_length/2, (arm_sum-arm2_length/2-0.0001) - cos(f*720+180) * arm2_length/2 ];