2 Dimensional Delta Robot with Servo Motor & Arduino

1#include <Servo.h>
2
3Servo ml,mr;    
4
5int i,j;
6
7float
8  pi= 3.1428;
9float l=6; //l = 6 cm length of link 
10
11float x,y,dl,dr,hl,hr;
12float
13  pl1,pl2,pr1,pr2;
14int tl,tr;
15
16float angle_left(float x, float y){
17  Serial.println("Left
18  : ");
19  
20  dl= sqrt((x*x)+(y*y)); //distace between origin left and (x,y)
21  point
22  
23  hl=sqrt((l*l)-((dl/2)*(dl/2))); //height of the triangle
24  
25
26  pl1=atan2(hl,dl/2); // phi 1, angle between d and axis
27  
28  pl2=atan2(y,x);
29  // phi 2, angle between d and l 
30  
31  tl= (pl1+pl2)*180/pi; // anglie between
32  axis and l
33
34  Serial.println(hl);
35  Serial.println(pl1*180/pi);
36  Serial.println(pl2*180/pi);
37
38  Serial.println(tl);
39  return tl;
40  }
41
42float angle_right(float x, float
43  y){
44  x-=6;
45
46  Serial.println("Right : ");
47  
48  dr= sqrt(((x)*(x))+(y*y));
49  //distace between origin right and (x,y) point
50
51  hr=sqrt((l*l)-((dr/2)*(dr/2)));
52  //height of the triangle
53
54  pr1=atan2(hr,dr/2); // phi 1, angle between d
55  and axis
56
57  pr2= atan2(y,x); // phi 2, angle between d and l 
58
59  tr=(pr2-pr1)*180/pi;
60
61
62  Serial.println(hr);
63  Serial.println(pr1*180/pi);
64  Serial.println(pr2*180/pi);
65
66  Serial.println(tr);
67  return tr;
68  }
69
70// Extreme Left Point
71void
72  xleft(){ 
73  mr.write(110);
74  ml.write(110);
75  }
76
77// Extreme Right
78  Point
79void xright(){ 
80  mr.write(45);
81  ml.write(45);
82  }
83
84
85//
86  Extreme Top Point
87void xtop(){
88  mr.write(90);
89  ml.write(90);
90}
91
92//Extreme
93  Bottom Point 
94void xbottom(){
95  mr.write(60);
96  ml.write(120);
97}
98
99void
100  vertical_straight_line(){
101  x=3;
102  y=6;
103
104  for(y=6;y<12;y+=0.5){ 
105
106    ml.write(angle_left(x,y));
107    mr.write(angle_right(x,y));
108    delay(50);
109
110  }
111
112  for(y=11;y>5;y-=0.5){ 
113    ml.write(angle_left(x,y));
114    mr.write(angle_right(x,y));
115
116    delay(50);
117   }
118}
119
120
121void horizontal_straight_line(){
122  x=0;
123
124  y=6;
125  for (x;x<7;x+=1){
126    ml.write(angle_left(x,y));
127    mr.write(angle_right(x,y));
128    
129    delay(500);
130   }
131  
132}
133
134 
135void setup() {
136  ml.attach(5);
137  
138  mr.attach(6);
139  Serial.begin(9600);
140}
141
142void loop() {
143  //int
144  a[8][2]=[[1, 1], [0, 2], [1, 3], [2, 3], [3, 3], [4, 2], [3, 1], [2, 1]] //making
145  0
146  //int a[12][2]={{1, 1}, {0, 2}, {0, 3}, {1, 4}, {2, 5}, {3, 5}, {4, 4}, {5,
147  3}, {5, 2}, {4, 1}, {3, 0}, {2, 0}}; //making O or dimond
148  //int a[12][2]={{0,0},
149  {1,1}, {2,2}, {3,3}, {4,4}, {5,5},{5,5}, {4,4}, {3,3}, {2,2}, {1,1}, {0,0}};// making
150  / digonal line
151  //int a[20][2]={{0,0},{0,1},{0,2},{0,3},{0,4},{0,5},{1,5},{2,5},{3,5},{4,5},{5,5},{5,4},{5,3},{5,2},{5,1},{5,0},{4,0},{3,0},{2,0},{1,0}};
152  // making [] square
153 
154  for (i=0;i<sizeof(a)/4;i+=1){
155    x=a[i][0];
156
157    y=a[i][1]+5;
158    
159    ml.write(angle_left(x,y));
160    mr.write(angle_right(x,y));
161
162    delay(300);  
163  }
164 
165  x=1;
166  y=1*2;
167  y+=3;
168  ml.write(angle_left(x,y));
169
170  mr.write(angle_right(x,y));
171  
172  delay(500);
173
174  x=0;
175  y=2*2;
176
177  y+=3;
178  ml.write(angle_left(x,y));
179  mr.write(angle_right(x,y));
180  
181
182  delay(500);
183
184  x=1;
185  y=3*2;
186  y+=3;
187  ml.write(angle_left(x,y));
188
189  mr.write(angle_right(x,y));
190  
191  delay(500);
192
193  x=2;
194  y=3*2;
195
196  y+=3;
197  ml.write(angle_left(x,y));
198  mr.write(angle_right(x,y));
199  
200
201  delay(500);
202
203  x=3;
204  y=3*2;
205  y+=3;
206  ml.write(angle_left(x,y));
207
208  mr.write(angle_right(x,y));
209  
210  delay(500);
211
212  x=4;
213  y=2*2;
214
215  y+=3;
216  ml.write(angle_left(x,y));
217  mr.write(angle_right(x,y));
218  
219
220  delay(500);
221
222  x=3;
223  y=1*2;
224  y+=3;
225  ml.write(angle_left(x,y));
226
227  mr.write(angle_right(x,y));
228  
229  delay(500);
230
231  x=2;
232  y=1*2;
233
234  y+=3;
235  ml.write(angle_left(x,y));
236  mr.write(angle_right(x,y));
237  
238
239  delay(500);
240  
241  
242  x=3;
243  y=11;
244  ml.write(angle_left(x,y));
245
246  mr.write(angle_right(x,y));
247  
248  
249  xtop();
250  delay(1000);
251  xright();
252
253  delay(300);
254  xbottom();
255  delay(1000);
256  xleft();
257  delay(300);
258
259  
260}

1#include <Servo.h>
2
3Servo ml,mr;    
4
5int i,j;
6
7float  pi= 3.1428;
8float l=6; //l = 6 cm length of link 
9
10float x,y,dl,dr,hl,hr;
11float  pl1,pl2,pr1,pr2;
12int tl,tr;
13
14float angle_left(float x, float y){
15  Serial.println("Left  : ");
16  
17  dl= sqrt((x*x)+(y*y)); //distace between origin left and (x,y)  point
18  
19  hl=sqrt((l*l)-((dl/2)*(dl/2))); //height of the triangle
20  
21  pl1=atan2(hl,dl/2); // phi 1, angle between d and axis
22  
23  pl2=atan2(y,x);  // phi 2, angle between d and l 
24  
25  tl= (pl1+pl2)*180/pi; // anglie between  axis and l
26
27  Serial.println(hl);
28  Serial.println(pl1*180/pi);
29  Serial.println(pl2*180/pi);
30  Serial.println(tl);
31  return tl;
32  }
33
34float angle_right(float x, float  y){
35  x-=6;
36
37  Serial.println("Right : ");
38  
39  dr= sqrt(((x)*(x))+(y*y));  //distace between origin right and (x,y) point
40
41  hr=sqrt((l*l)-((dr/2)*(dr/2)));  //height of the triangle
42
43  pr1=atan2(hr,dr/2); // phi 1, angle between d  and axis
44
45  pr2= atan2(y,x); // phi 2, angle between d and l 
46
47  tr=(pr2-pr1)*180/pi;
48
49  Serial.println(hr);
50  Serial.println(pr1*180/pi);
51  Serial.println(pr2*180/pi);
52  Serial.println(tr);
53  return tr;
54  }
55
56// Extreme Left Point
57void  xleft(){ 
58  mr.write(110);
59  ml.write(110);
60  }
61
62// Extreme Right  Point
63void xright(){ 
64  mr.write(45);
65  ml.write(45);
66  }
67
68
69//  Extreme Top Point
70void xtop(){
71  mr.write(90);
72  ml.write(90);
73}
74
75//Extreme  Bottom Point 
76void xbottom(){
77  mr.write(60);
78  ml.write(120);
79}
80
81void  vertical_straight_line(){
82  x=3;
83  y=6;
84
85  for(y=6;y<12;y+=0.5){ 
86    ml.write(angle_left(x,y));
87    mr.write(angle_right(x,y));
88    delay(50);
89  }
90
91  for(y=11;y>5;y-=0.5){ 
92    ml.write(angle_left(x,y));
93    mr.write(angle_right(x,y));
94    delay(50);
95   }
96}
97
98
99void horizontal_straight_line(){
100  x=0;
101  y=6;
102  for (x;x<7;x+=1){
103    ml.write(angle_left(x,y));
104    mr.write(angle_right(x,y));    
105    delay(500);
106   }
107  
108}
109
110 
111void setup() {
112  ml.attach(5);  
113  mr.attach(6);
114  Serial.begin(9600);
115}
116
117void loop() {
118  //int  a[8][2]=[[1, 1], [0, 2], [1, 3], [2, 3], [3, 3], [4, 2], [3, 1], [2, 1]] //making  0
119  //int a[12][2]={{1, 1}, {0, 2}, {0, 3}, {1, 4}, {2, 5}, {3, 5}, {4, 4}, {5,  3}, {5, 2}, {4, 1}, {3, 0}, {2, 0}}; //making O or dimond
120  //int a[12][2]={{0,0},  {1,1}, {2,2}, {3,3}, {4,4}, {5,5},{5,5}, {4,4}, {3,3}, {2,2}, {1,1}, {0,0}};// making  / digonal line
121  //int a[20][2]={{0,0},{0,1},{0,2},{0,3},{0,4},{0,5},{1,5},{2,5},{3,5},{4,5},{5,5},{5,4},{5,3},{5,2},{5,1},{5,0},{4,0},{3,0},{2,0},{1,0}};  // making [] square
122 
123  for (i=0;i<sizeof(a)/4;i+=1){
124    x=a[i][0];
125    y=a[i][1]+5;
126    
127    ml.write(angle_left(x,y));
128    mr.write(angle_right(x,y));
129    delay(300);  
130  }
131 
132  x=1;
133  y=1*2;
134  y+=3;
135  ml.write(angle_left(x,y));
136  mr.write(angle_right(x,y));
137  
138  delay(500);
139
140  x=0;
141  y=2*2;
142  y+=3;
143  ml.write(angle_left(x,y));
144  mr.write(angle_right(x,y));
145  
146  delay(500);
147
148  x=1;
149  y=3*2;
150  y+=3;
151  ml.write(angle_left(x,y));
152  mr.write(angle_right(x,y));
153  
154  delay(500);
155
156  x=2;
157  y=3*2;
158  y+=3;
159  ml.write(angle_left(x,y));
160  mr.write(angle_right(x,y));
161  
162  delay(500);
163
164  x=3;
165  y=3*2;
166  y+=3;
167  ml.write(angle_left(x,y));
168  mr.write(angle_right(x,y));
169  
170  delay(500);
171
172  x=4;
173  y=2*2;
174  y+=3;
175  ml.write(angle_left(x,y));
176  mr.write(angle_right(x,y));
177  
178  delay(500);
179
180  x=3;
181  y=1*2;
182  y+=3;
183  ml.write(angle_left(x,y));
184  mr.write(angle_right(x,y));
185  
186  delay(500);
187
188  x=2;
189  y=1*2;
190  y+=3;
191  ml.write(angle_left(x,y));
192  mr.write(angle_right(x,y));
193  
194  delay(500);
195  
196  
197  x=3;
198  y=11;
199  ml.write(angle_left(x,y));
200  mr.write(angle_right(x,y));
201  
202  
203  xtop();
204  delay(1000);
205  xright();
206  delay(300);
207  xbottom();
208  delay(1000);
209  xleft();
210  delay(300);
211  
212}