Home

1. Model display

This robot is made of 2-DOF Planar Parallel Robot with 1-DOF pencil on the vertical plane. The goal is to control 2 motors to drive the pencil go along a straight line, and another motor to make pencil up and down. My Simulink model consists of 6 modules(see picture below).

2. Trajectory display

The pencil goes along a straight line from (0, 0.3, 0.05) to (0.3, 0.5, -0.05) in 5 seconds

I also tested its straightness via Curve Fitting Tool. As you can see, the R-square is 1, which means that the trajectory is perfectly straight.fit

# script solving inverse kinematics 
len1 = .35;
len2 = .35;
len3 = .35;
len4 = .35;
l = .6;
syms x y theta1 theta2 theta4 theta5
eqyl = (len1 * sin(theta1) + len2 * sin(theta1 + theta2) == y);
eqyr = (len3 * sin(theta4 + theta5) + len4 * sin(theta5) == y);
eqxl = (len1 * cos(theta1) + len2 * cos(theta1 + theta2) == x);
eqxr = (len3 * cos(theta4 + theta5) + len4 * cos(theta5) == l - x);
[theta1, theta2, theta4, theta5] = solve(eqxl, eqyl, eqxr, eqyr, theta1, theta2, theta4, theta5);
theta1
theta5

Given the expected values of X, Y coordinates, this function outputs the required value of 2 motor angles.

function [theta5,theta1] = fcn(x, y)
theta1 = 2*atan((7*y + (x^2*(-(x^2 + y^2)*(100*x^2 + 100*y^2 - 49))^(1/2)) ...
    `/(x^2 + y^2) + (y^2*(-(x^2 + y^2)*(100*x^2 + 100*y^2 - 49))^(1/2))...
    /(x^2 + y^2))/(10*x^2 + 7*x + 10*y^2));
theta5 = 2*atan((35*y + ((25*x^2 - 30*x + 25*y^2 + 9)*...
    (- 100*x^2 + 120*x - 100*y^2 + 13))^(1/2))/(50*x^2 - 95*x + 50*y^2 + 39));
end