1

Robotics project

Youssef Mohammed Fawzy Agour 19106323

Youssef Ibrahim 19104279

Mohammed Sadek 19108623

Mohammed Khaled 19105188

Mohammed Hatem 19105346

2
3
4
5

MATLAB codes

• Forward
clear all;
clc;

a = arduino('COM4', 'Mega2560', 'Libraries', 'Servo');

base = servo(a, 'D5');

shoulder = servo(a, 'D7');
effector = servo(a, 'D6');

l0 = 0.09;
l1 = 0.12;
l2 = 0.118;

while(1)

Volts1 = readVoltage(a, 'A0');

angle1= (180/5)*Volts1;

Volts2 = readVoltage(a, 'A1');

angle2= (180/5)*Volts2;

Volts3 = readVoltage(a, 'A2');

angle3= (180/5)*Volts3;

thetamat=[angle1 angle2 angle3];

B1=thetamat(1)
B2=thetamat(2)
B3=thetamat(3)

%writing cond in range 0 to 1

writePosition(base,thetamat(1)/180)
writePosition(shoulder,thetamat(2)/180)
writePosition(effector,thetamat(3)/180)

end
6

• Inverse kinematics

% Setup Arduino and servos

clc;
clear;
a = arduino('COM4', 'Mega2560', 'Libraries', 'Servo');
base = servo(a, 'D5');
shoulder = servo(a, 'D7');
effector = servo(a, 'D6');

% Define link lengths (meters)

L1 = 6; % Length of the first link
L2 = 12; % Length of the second link
L3 = 8; % Length of the third link

while true
% Input target coordinates
X = input('Enter the X coordinate: ');
Y = input('Enter the Y coordinate: ');
Z = input('Enter the Z coordinate: ');

% Inverse kinematics calculations

s = Z - L1;
r = sqrt(X^2 + Y^2);
K = sqrt(r^2 + s^2);
D = ((K^2) - (L2^2) - (L3^2)) / (-2 * L2 * L3);

% Check if the target is reachable

if (r^2 + s^2) > (L2 + L3)^2 || (r^2 + s^2) < (abs(L2 - L3))^2
disp('Target position unreachable with given link lengths');
continue;
end

Beta = atan2d(sqrt(1 - D^2), D);

Gamma = atan2d(s, r);

% Inverse kinematics output

Theta_1_inv = atan2d(Y, X);
Theta_3_inv = 180 - Beta;

k1 = L2 + L3 * cosd(Theta_3_inv);
k2 = L3 * sind(Theta_3_inv);
Theta_2_inv = Gamma - atan2d(k2, k1);

% Map function to scale angles to servo positions (0-1 range)

writePosition(base, map(Theta_1_inv, 0, 180, 0, 1));
writePosition(shoulder, map(Theta_2_inv, -180, 180, 0, 1));
writePosition(effector, map(Theta_3_inv, -180, 180, 0, 1));

end

% Map function definition

7

function pos = map(value, fromLow, fromHigh, toLow, toHigh)

% Ensure values are within the expected range
pos = ((value - fromLow) / (fromHigh - fromLow)) * (toHigh - toLow) + toLow;
% Clamp the value to be within [0, 1]
pos = min(max(pos, toLow), toHigh);
end
point in cloud

• Circular path

fclose(serial("COM4"));
clear;
a = arduino('COM4', 'Mega2560', 'Libraries', 'Servo');
s1 = servo(a, 'D5'); % base
s2 = servo(a, 'D7'); % shoulder
s3 = servo(a, 'D6'); % wrist

% Parameters for the circle

a_center = 10; % X-center of the circle
b_center = 10; % Y-center of the circle
R = 8; % Radius

% Link lengths
a1 = 4;
a2 = 12;
a3 = 11.8;

% Set up the figure for plotting

figure;
hold on;
axis equal; % Ensure the plot is not distorted
xlim([a_center - R - 5, a_center + R + 5]);
ylim([b_center - R - 5, b_center + R + 5]);
title('Real-Time Plot of Robot Drawing a Circle');
xlabel('X Position');
ylabel('Y Position');

% Base angle offset to make the circle more planar

base_angle_offset = 10; % Adjust this value to fine-tune the planarity
previous_angle1 = 0;
8

while true
% Loop over angle t from 0 to 2*pi
for t = 0:0.05:2*pi % Smaller step size for smoother circle
% Calculate the current target point on the circle
x = R * cos(t) + a_center;
y = R * sin(t) + b_center;
z = 10; % Z height (constant for a flat circle)

% Inverse kinematics calculations

n = sqrt(x^2 + y^2);
theta1 = atan2d(y, x) -30 %second varaiable is for the offset ;

% Handle angle discontinuities

if abs(theta1 - previous_angle1) > 180
theta1 = theta1 - sign(theta1 - previous_angle1) * 360;
end
previous_angle1 = theta1;

D = (x^2 + y^2 - a2^2 - a3^2) / (-2 * a2 * a3);

if abs(D) > 1
warning('D value out of range, skipping this iteration');
continue;
end

B = atan2d(sqrt(1 - D^2), D);

theta3 = 180 - B;
gamma = atan2d(z - a1, n);
alpha = atan2d(a3 * cosd(theta3), a2 + a3 * sind(theta3));
theta2 = gamma - alpha;

% Print all angles

fprintf('Theta1: %.4f degrees, Theta2: %.4f degrees, Theta3: %.4f degrees\n',
theta1, theta2, theta3);

% Adjust angles and ensure they are within valid servo ranges
angle1 = theta1 / 180;
angle2 = (theta2 + 40) / 180;
angle3 = (theta3 - 45) / 180;

% Clamp angles to the range [0, 1]

angle1 = min(max(angle1, 0), 1);
angle2 = min(max(angle2, 0), 1);
angle3 = min(max(angle3, 0), 1);

% Write positions to servos

writePosition(s1, abs(angle1));
writePosition(s2, abs(angle2));
writePosition(s3, abs(angle3));

% Plot the current (x, y) point

plot(x, y , 'ro'); % 'ro' plots a red circle at each point
drawnow; % Update the plot in real-time

pause(0.2); % Adjusted pause for smoother transitions

9

end
end

• square on floor
• clear;
• clc;
•
• % Initialize Arduino and Servo Connections
• a = arduino('COM4', 'Mega2560', 'Libraries', 'Servo');
•
• base = servo(a, 'D5');
• shoulder = servo(a, 'D7');
• effector = servo(a, 'D6');
• writePosition(base,10/180)
• pause(1)
• writePosition(shoulder,40/180)
• pause(1)
• writePosition(effector,150/180)
•
• % Arm Segment Lengths
• L1 = 9; % Length of link 1
• L2 = 12; % Length of link 2
• L3 = 11.8; % Length of link 3
•
• % Set of target coordinates
• coordinates = [
• -2.3913, -0.42165, 14.6644;
• -2.2818, -0.83049, 14.6644;
• -2.2818, -0.83049, 18.7625;
• -2.3913, -0.42165, 18.7625
• ];
•
• % Loop over each set of coordinates
• while 1
• for i = 1:size(coordinates, 1)
• X = coordinates(i, 1);
• Y = coordinates(i, 2);
• Z = coordinates(i, 3);
•
• % Inverse kinematics
• s = Z - L1;
• r = sqrt(X^2 + Y^2);
• K = sqrt(r^2 + s^2);
• D = ((K^2) - (L2^2) - (L3^2)) / (-2 * L2 * L3);
• Beta = atan2d(sqrt(1 - D^2), D);
•
• if (r^2 + s^2) > (L2 + L3)^2 || (r^2 + s^2) < (abs(L2 - L3))^2
• error('Target position unreachable with given link lengths');
10

• end
•
• Gamma = atand(s / r);
•
• % Inverse kinematics output
• Theta_1_inv = atan2d(Y, X);
• Theta_3_inv = 180 - Beta;
•
• k1 = L2 + L3 * cosd(Theta_3_inv);
• k2 = L3 * sind(Theta_3_inv);
• Theta_2_inv = Gamma - atan2d(k2, k1);
•
• % Adjusting for servo position range
• Theta_1_inv = Theta_1_inv + 180;
•
• % Display the calculated angles
• disp(['Coordinate set ', num2str(i), ':']);
• disp(['Theta_1 (Base Angle) = ', num2str(Theta_1_inv), ' degrees']);
• disp(['Theta_3 (Effector Angle) = ', num2str(Theta_3_inv), ' degrees']);
• disp('------------------------------');
•
• % Writing Servo Positions (Convert angles to the [0, 1] range for servos)
• writePosition(base, Theta_1_inv / 180);
• writePosition(effector, Theta_3_inv / 180);
•
• % Pause to allow motion
• pause(1);
• end
• end

• square on wall
• clear;
• clc;
•
• % Initialize Arduino and Servo Connections
• a = arduino('COM4', 'Mega2560', 'Libraries', 'Servo');
•
• base = servo(a, 'D5');
• shoulder = servo(a, 'D7');
• effector = servo(a, 'D6');
• writePosition(base,10/180)
• pause(1)
• writePosition(shoulder,120/180)
• pause(1)
• writePosition(effector,150/180)
•
• % Arm Segment Lengths
11

• L1 = 9; % Length of link 1
• L2 = 12; % Length of link 2
• L3 = 11.8; % Length of link 3
•
• % Set of target coordinates
• coordinates = [
• -2.3913, -0.42165, 14.6644;
• -2.2818, -0.83049, 14.6644;
• -2.2818, -0.83049, 18.7625;
• -2.3913, -0.42165, 18.7625
• ];
•
• % Loop over each set of coordinates
• while 1
• for i = 1:size(coordinates, 1)
• X = coordinates(i, 1);
• Y = coordinates(i, 2);
• Z = coordinates(i, 3);
•
• % Inverse kinematics
• s = Z - L1;
• r = sqrt(X^2 + Y^2);
• K = sqrt(r^2 + s^2);
• D = ((K^2) - (L2^2) - (L3^2)) / (-2 * L2 * L3);
• Beta = atan2d(sqrt(1 - D^2), D);
•
• if (r^2 + s^2) > (L2 + L3)^2 || (r^2 + s^2) < (abs(L2 - L3))^2
• error('Target position unreachable with given link lengths');
• end
•
• Gamma = atand(s / r);
•
• % Inverse kinematics output
• Theta_1_inv = atan2d(Y, X);
• Theta_3_inv = 180 - Beta;
•
• k1 = L2 + L3 * cosd(Theta_3_inv);
• k2 = L3 * sind(Theta_3_inv);
• Theta_2_inv = Gamma - atan2d(k2, k1);
•
• % Adjusting for servo position range
• Theta_1_inv = Theta_1_inv + 180;
•
• % Display the calculated angles
• disp(['Coordinate set ', num2str(i), ':']);
• disp(['Theta_1 (Base Angle) = ', num2str(Theta_1_inv), ' degrees']);
• disp(['Theta_3 (Effector Angle) = ', num2str(Theta_3_inv), ' degrees']);
• disp('------------------------------');
•
• % Writing Servo Positions (Convert angles to the [0, 1] range for servos)
• writePosition(base, Theta_1_inv / 180);
12

• writePosition(effector, Theta_3_inv / 180);

•
• % Pause to allow motion
• pause(1);
• end
• end

• VISION

clear all
clc
L1 = 9; % Length of link 1
L2 = 12; % Length of link 2
L3 = 11.8; % Length of link 3
a = arduino('COM4', 'Mega2560', 'Libraries', 'Servo');

base = servo(a, 'D5');

shoulder = servo(a, 'D7');
effector = servo(a, 'D6');

writePosition(base,60/180)
writePosition(shoulder,60/180)
writePosition(effector,60/180)
for i=0:1:4
% Define initial servo positions
P1=30; % Gripper 30 is open, 100 is closed
P2=85; % Fixed position
P3=100; % Initial position
P4=50; % Increase (y=>inc z=>dec)
P5=30; % Increase (y=>inc z=>dec)
P6=0; % Increase (dec x)
% Control constants
K_p=0.1; % Proportional gain
T=3; % Threshold for position adjustment
% Initialize video input object
v = videoinput('winvideo', 1, 'MJPG_640x480');
bc=[0 0]; % Position at the webcam
q=[0 0];
target =[85 70]; % Desired x y position in the webcam
R=[0 0 0 0 0 0]; % Servo position array
% Set video object properties
set(v, 'FramesPerTrigger', Inf);
set(v, 'ReturnedColorspace', 'rgb')
v.FrameGrabInterval = 5;
% Start video acquisition
start(v)
while(v.FramesAcquired<=1000)
13

% Get the current frame

data = getsnapshot(v);
% Image processing: subtract red component, filter noise, convert to binary
diff_im = imsubtract(data(:,:,1), rgb2gray(data));
diff_im = medfilt2(diff_im, [3 3]);
diff_im = im2bw(diff_im,0.18);
imshow(diff_im)

% Remove small objects

diff_im = bwareaopen(diff_im,300);
% Label connected components
bw = bwlabel(diff_im, 8);
% Analyze blobs in the image
stats = regionprops(bw, 'BoundingBox', 'Centroid');
% Display the image
imshow(data)
hold on
% Draw rectangles around blobs and display centroid positions
for object = 1:length(stats)
bb = stats(object).BoundingBox;
bc = stats(object).Centroid;
rectangle('Position',bb,'EdgeColor','r','LineWidth',2)
plot(bc(1),bc(2), '-m+')
a=text(bc(1)+15,bc(2), strcat('X: ', num2str(round(bc(1))), ' Y: ',
num2str(round(bc(2)))));
set(a, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 12,
'Color', 'yellow');
end

x = round(bc(1));
y = round(bc(2));
z = 30;

Matrix_Camera_to_Base = [0 0 -1 0.6 ;
-1 0 0 0 ;
0 -1 0 0.23 ;
0 0 0 1 ] ;
Point_Camera = [ x/100;

y/100;

z/100;
1 ];

Base_Camera_Relation = Matrix_Camera_to_Base * Point_Camera;

display(Base_Camera_Relation);

pause(0.0001); % Delay time in seconds

if x >=450 && y<=200

writePosition(base,90/180);
writePosition(shoulder,85/180);
writePosition(effector,60/180);
14

elseif x >=450 && y>=300

writePosition(base,90/180);
writePosition(shoulder,50/180);
writePosition(effector,60/180);

elseif x <=350 && y<=200

writePosition(base,40/180);
writePosition(shoulder,85/180);
writePosition(effector,60/180);
elseif x <=350 && y>=300
writePosition(base,40/180);
writePosition(shoulder,50/180);
writePosition(effector,60/180);
else
writePosition(base,60/180)
writePosition(shoulder,60/180)
writePosition(effector,60/180)
end

end

% Perform downward movement (not shown in the code provided)

% Stop video acquisition

stop(v);
% Flush image data
flushdata(v);

% Pause for a certain duration

pause(1)

% Perform object catching action (not shown in the code provided)

% Perform upward movement (not shown in the code provided)

% Go to desired drop position (not shown in the code provided)

% Drop the object (not shown in the code provided)

% Go to a middle point after task completion (not shown in the code provided)

pause(2) % Pause before next iteration

end