# Difference between revisions of "Robotic Arm: Team C-Tango: Tango"

## MatLab Code for Physical Model

### gotoAngles.m

% Robot Arm Simulation

s=instrfind;  %Find any serial links (we can have only 1) delete(s);  %... and delete. %Create a new serial communications link s=serial('COM1','Baudrate',115200,'Terminator','CR'); fopen(s);  %... and open it

%gotoAngles(s,0,0,1000)

sqX = [5.5 5.5 7.5 7.5 5.5 5.5]; sqY = [1 2 2 0 0 1];

for i = 1:6

```   [sqX(i),sqY(i)]

[thetaA,thetaB] = inverseKinematics(4.5,3.875,sqX(i),sqY(i));

gotoAngles(s,-thetaA,-thetaB,2000)

```

end

for i = 5.5:.05:7.5

```   y = 1 + sqrt(1-(i-6.5)^2);
[thetaA,thetaB] = inverseKinematics(4.5,3.875,i,y);
gotoAngles(s,-thetaA,-thetaB,50)

```

end

for i = 7.5:-.05:5.5

```   y = 1 - sqrt(1-(i-6.5)^2);
[thetaA,thetaB] = inverseKinematics(4.5,3.875,i,y);
gotoAngles(s,-thetaA,-thetaB,50)

```

end

### inverseKinematics.m

function [theta_a, theta_b] = inverseKinematics(la, lb, x2, y2) %Calculate locations of arms of robot

%Equations given on website theta_a=atan2(y2,x2)-acos((la^2+x2^2+y2^2-lb^2)/(2*la*sqrt(x2^2+y2^2))); theta_b=atan2(((y2-la*sin(theta_a))/lb),((x2-la*cos(theta_a))/lb)); %Convert from radians to degrees theta_a=theta_a*180/pi; theta_b=theta_b*180/pi; theta_b = theta_b-theta_a;

### robotcontrol.m

% Robot Arm Simulation

s=instrfind;  %Find any serial links (we can have only 1) delete(s);  %... and delete. %Create a new serial communications link s=serial('COM1','Baudrate',115200,'Terminator','CR'); fopen(s);  %... and open it

%gotoAngles(s,0,0,1000)

sqX = [5.5 5.5 7.5 7.5 5.5 5.5]; sqY = [1 2 2 0 0 1];

for i = 1:6

```   [sqX(i),sqY(i)]

[thetaA,thetaB] = inverseKinematics(4.5,3.875,sqX(i),sqY(i));

gotoAngles(s,-thetaA,-thetaB,2000)

```

end

for i = 5.5:.05:7.5

```   y = 1 + sqrt(1-(i-6.5)^2);
[thetaA,thetaB] = inverseKinematics(4.5,3.875,i,y);
gotoAngles(s,-thetaA,-thetaB,50)

```

end

for i = 7.5:-.05:5.5

```   y = 1 - sqrt(1-(i-6.5)^2);
[thetaA,thetaB] = inverseKinematics(4.5,3.875,i,y);
gotoAngles(s,-thetaA,-thetaB,50)

```

end