-
Notifications
You must be signed in to change notification settings - Fork 0
/
Project_2.m
83 lines (66 loc) · 4.34 KB
/
Project_2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
% Abedin Sherifi
function Tower_of_Hanoi()
cp_data = [0, 0, -.25, 0];
%% Move #1
%Grasp disc 4 from Peg 1 and ungrasp on Peg 3 in disc position 1
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(0.0, 0.0, -0.25, 1.0, 4.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 1.0, cp_data);
%% Move #2
%Grasp disc 3 from Peg 1 and ungrasp on Peg 2 in disc position 1
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 3.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 1.0, cp_data);
%% Move #3
%Grasp disc 1 from Peg 3, and ungrasp on Peg 2, in disc postion 2
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 1.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 2.0, cp_data);
%% Move #4
%Grasp disc 2 from Peg 1, and ungrasp on Peg 3, in disc postion 1
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 2.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 1.0, cp_data);
%% Move #5
%Grasp disc 2 from Peg 2, and ungrasp on Peg 1, in disc postion 2
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 2.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 2.0, cp_data);
%% Move #6
%Grasp disc 1 from Peg 2, and ungrasp on Peg 3, in disc postion 2
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 1.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 2.0, cp_data);
%% Move #7
%Grasp disc 2 from Peg 1, and ungrasp on Peg 3, in disc postion 3
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 2.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 3.0, cp_data);
%% Move #8
%Grasp disc 1 from Peg 1, and ungrasp on Peg 2, in disc postion 1
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 1.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 1.0, cp_data);
%% Move #9
%Grasp disc 3 from Peg 3, and ungrasp on Peg 2, in disc postion 2
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 3.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 2.0, cp_data);
%% Move #10
%Grasp disc 2 from Peg 3, and ungrasp on Peg 1, in disc postion 1
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 2.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 1.0, cp_data);
%% Move #11
%Grasp disc 2 from Peg 2, and ungrasp on Peg 1, in disc postion 2
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 2.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 2.0, cp_data);
%% Move #12
%Grasp disc 1 from Peg 3, and ungrasp on Peg 2, in disc postion 2
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 1.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 2.0, cp_data);
%% Move #13
%Grasp disc 2 from Peg 1, and ungrasp on Peg 3, in disc postion 1
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 2.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 1.0, cp_data);
%% Move #14
%Grasp disc 1 from Peg 1, and ungrasp on Peg 2, in disc postion 3
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 1.0, 1.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 3.0, cp_data);
%% Move #15
%Grasp disc 1 from Peg 3, and ungrasp on Peg 2, in disc postion 4
[th1_goal, th2_goal, d3_goal, cp_data] = grasp_disc(th1_goal, th2_goal, d3_goal, 3.0, 1.0, cp_data);
[th1_goal, th2_goal, d3_goal, cp_data] = ungrasp_disc(th1_goal, th2_goal, d3_goal, 2.0, 4.0, cp_data);
% Write data to text file
dlmwrite("sim_data.txt", cp_data, " ");
end