%=========================================================================
% Homework #1
% MEA 263 D
%=========================================================================

clear all; clc;
syms t1 t2 t3 L1 L2 L3 d d1 d2 d3
syms t4 t5 t6 d4 L4
problemnumber=0;
for i = 1:8
    problemnumber=i;
    p = problemnumber;

if p == 1
%% Problem #1

DH = [0 0 0 0 0 0;...
      0 L1 0 0 0 0;...
      0 L2 0 0 0 0];

Robot1 = DH2Robot(DH,0)

T0_1 = Robot1.A([1],[t1 t2 0])
T1_2 = Robot1.A([2],[t1 t2 0])
T2_t = Robot1.A([3],[t1 t2 0])
Robot1.A([1 2 3],[t1 t2 0])
end

if p == 2
%% Problem #2

DH = [0 0 0 0 0 0;...
      0 L1 0 0 0 0;...
      0 L2 0 0 0 0];

Robot1 = DH2Robot(DH,0)

T0_1 = Robot1.A([1],[t1 t2 t3])
T1_2 = Robot1.A([2],[t1 t2 t3])
T2_3 = Robot1.A([3],[t1 t2 t3])
Robot1.A([1 2 3],[t1 t2 t3])
end


if p == 3
%% Problem #3

DH = [0 0 0 0 0 0;...
      -90 0 0 0 0 0;...
      0 L2 0 0 0 0;...
      0 L3 0 0 0 0];

Robot1 = DH2Robot(DH,0)
%Robot1.plot([0 0 0 0])

T0_1 = Robot1.A([1],[t1 t2 t3 0])
T1_2 = Robot1.A([2],[t1 t2 t3 0])
T2_3 = Robot1.A([3],[t1 t2 t3 0])
T2_4 = Robot1.A([4],[t1 t2 t3 0])
Robot1.A([1 2 3 4],[t1 t2 t3 0])
end


if p == 4
%% Problem #4

DH = [0 0 0 0 0 0;...
      90 0 L2 0 0 0;...
      -90 0 d3 0 1 0];

Robot1 = DH2Robot(DH,0)
%Robot1.plot([0.3 0.4 4],'workspace',[-20 20 -20 20 -20 20])

T0_1 = Robot1.A([1],[t1 t2 d])
T1_3 = Robot1.A([2],[t1 t2 d])
T3_4 = Robot1.A([3],[t1 t2 d])
Robot1.A([1 2 3],[t1 t2 d])
end


if p == 5
%% Problem #5

DH = [0 0 0 0 0 0;...
      0 0 d2 90 1 0;...
      90 0 d3 0 1 0];

Robot1 = DH2Robot(DH,0)
%Robot1.plot([0 0 0],'workspace',[-20 20 -20 20 -20 20])

T0_1 = Robot1.A([1],[t1 d2 d3])
T1_2 = Robot1.A([2],[t1 d2 d3])
T2_3 = Robot1.A([3],[t1 d2 d3])
Robot1.A([1 2 3],[t1 d2 d3])
end


if p == 6
%% Problem #6

DH = [0 0 0 t4 0 0;...
      90 0 0 t5 0 0;...
      -90 0 0 t6 0 0];

Robot1 = DH2Robot(DH,0)
%Robot1.plot([0 0 0],'workspace',[-20 20 -20 20 -20 20])

T3_4 = Robot1.A([1],[t4 t5 t6])
T4_5 = Robot1.A([2],[t4 t5 t6])
T5_6 = Robot1.A([3],[t4 t5 t6])
Robot1.A([1 2 3],[t4 t5 t6])
end


if p == 7
%% Problem #7

DH = [0 0 0 t1 0 0;...
      0 L1 0 t2 0 0;...
      0 L2 0 t3 0 0;...
      0 0 -d4 0 1 0];

Robot1 = DH2Robot(DH,0)
%Robot1.plot([0 0 0 -5],'workspace',[-20 20 -20 20 -20 20])

T0_1 = Robot1.A([1],[t1 t2 t3 d4])
T1_2 = Robot1.A([2],[t1 t2 t3 d4])
T2_3 = Robot1.A([3],[t1 t2 t3 d4])
T3_4 = Robot1.A([4],[t1 t2 t3 d4])
Robot1.A([1 2 3 4],[t1 t2 t3 d4])
end


if p == 8
%% Problem #8

DH = [0 0 d1 0 1 0;...
      0 0 L1 t2 0 0;...
      0 L2 0 t3 0 0;...
      0 L3 0 t4 0 0;...
      0 0 -L4 0 0 0];

Robot1 = DH2Robot(DH,0)
%Robot1.plot([0 0 0 0 0],'workspace',[-20 20 -20 20 -20 20])

T0_1 = Robot1.A([1],[d1 t2 t3 t4 0])
T1_2 = Robot1.A([2],[d1 t2 t3 t4 0])
T2_3 = Robot1.A([3],[d1 t2 t3 t4 0])
T3_4 = Robot1.A([4],[d1 t2 t3 t4 0])
T4_5 = Robot1.A([5],[d1 t2 t3 t4 0])
Robot1.A([1 2 3 4 5],[d1 t2 t3 t4 0])
end

end