Homework Copying Robot | Handwriting Copying Robot

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}