Created
September 5, 2016 04:25
-
-
Save thinkyhead/85a4847ae1fc5aa0d21e807fa26bf3c5 to your computer and use it in GitHub Desktop.
Ambidextrous SCARA Kinematics
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/** | |
* 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 ]; |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment