# Robot Artist

## Introduction

When we were calibrating and constructing our robot arm, we used a motor connected to an "upper arm" of 15cm, and a second motor with a "lower arm" connected to the end of the upper arm. We tried to make a right arm, but somewhere we messed up with the calibration so it is backwards which unfortunately does not allow as much motion. When both arms were at 0 degrees, the whole arm was straight out. Looking back, it would have been better if 0 was a 90 degree angle instead. When we were writing the code to test, we continually ran in to problems with motors and connecting to the calibration. We realized that we wanted to have a for loop that would update the position of the arms using the invKin function. Our invKin function worked before in the previous lab, but we often received errors when working with our actual robot arm. We had code for both a circle and a square, but they would not work properly. A major issue that we were unable to overcome was matching up the distance we put in and having the arm move the appropriate length. For example, we would type in for the arm to move a centimeter to the left, but the arm would instead only move a tenth of a centimeter. By the end, we were able to make a small triangle using a for loop and our functions. Given more time, we believe we could have worked out all the bugs so that we could make a larger circle inscribed in a square.

## Control Script

### Functions

getPCs.m

```
function [pcountA pcountB] =  getPCs(thetaA, thetaB)
% The input to this function are two angles.
% These angles conform to lines given by:
%    thetaA = mA*pcA + bA
%    thetaB = mB*pcB + bB
%
% The function returns the proper values of pwa and pwb.
%
% The values of mA, bA, mB, and bB are given below.
mA=0.3593;  bA=-44.362;
mB=0.3710;  bB=-45.231;
pcountA=(thetaA - bA)/mA;
pcountB=(thetaB - bB)/mB;
end
```

gotoAngles.m

```function gotoAngles(a, thetaA, thetaB)
% This function turns servos to specified angles (in degrees).
%   a is "E5SHield" object.
%   thetaA is the angle of one motor.
%   thetaB is the angle of the second motor.

[pcA,pcB]= getPCs(thetaA,thetaB)
pcAint=int16(pcA);
pcBint=int16(pcB);
a.servoWrite(1,pcAint)
a.servoWrite(5,pcBint)
```

invKin.m

```function [thetaA thetaB] = invKin(x,y)
% This function does the inverse kinematics for the E5 robot,
% Inputs:
%   x, y: the desired (x,y) location of the pencil
% Outputs:
%   thetaA, thetaB:  the angles of the two servo motors.
La=6.5;
Lb=5;
thetaA=(180/pi)*(atan2(y,x)-acos(((La^2)+(x^2+y^2)-(Lb^2))/(2*La*(sqrt(x^2+y^2)))));
thetaB=((180/pi)*(atan2(((y-La*sind(thetaA))/Lb),(x-La*cosd(thetaA))/Lb)))-thetaA;
```

### Controls

```%% Making A Circle Inscribed in a Square

[thetaA thetaB]= invKin(4,5); % Define starting point
thetaB = thetaB - 90;
[thetaA thetaB]
gotoAngles(a,thetaA, thetaB); % Move starting point
pause(0.5);

% Square

for i=1:200 % Move arm from (4,5) to (6,5) in 100 points
[thetaA thetaB]= invKin(4+i/100,5);
thetaB = thetaB - 90;
[thetaA thetaB];
gotoAngles(a,thetaA, thetaB);
pause(.01)
end

for i=1:200 % Move arm from (6,5) to (6,7) in 100 points
[thetaA thetaB]= invKin(6,5+i/100);
thetaB = thetaB - 90;
[thetaA thetaB];
gotoAngles(a,thetaA, thetaB);
pause(.01)
end

for i=1:200 % Move arm from (6,7) to (4,7) in 100 points
[thetaA thetaB]= invKin(6-i/100,7);
thetaB = thetaB - 90;
[thetaA thetaB];
gotoAngles(a,thetaA, thetaB);
pause(.01)
end

for i=1:200 % Move arm from (4,7) to (4,5) in 100 points
[thetaA thetaB]= invKin(4,7-i/100);
thetaB = thetaB - 90;
[thetaA thetaB];
gotoAngles(a,thetaA, thetaB);
pause(.01)
end

% Circle inscribed in Square

for i=0:360 % Move arm to starting point of circle (4.13, 5.5) and iterate around (6,5) in a circle of radius 1
x=5+cosd(i-120)
y=6+sind(i-120)
[thetaA thetaB]= invKin(x,y);
thetaB = thetaB - 90;
[thetaA thetaB]
gotoAngles(a,thetaA, thetaB);
pause(0.01);
end

```