写字机器人代码

Damn_intuition

1. #define CALIBRATION
   
2. 
   
3. // When in calibration mode, adjust the following factor until the servos move exactly 90 degrees
   
4. #define SERVOFAKTOR 620
   
5. 
   
6. // Zero-position of left and right servo
   
7. // When in calibration mode, adjust the NULL-values so that the servo arms are at all times parallel
   
8. // either to the X or Y axis
   
9. #define SERVOLEFTNULL 1900
   
10. #define SERVORIGHTNULL 984
    
11. 
    
12. #define SERVOPINLIFT  2
    
13. #define SERVOPINLEFT  3
    
14. #define SERVOPINRIGHT 4
    
15. 
    
16. // lift positions of lifting servo
    
17. #define LIFT0 1080 // on drawing surface
    
18. #define LIFT1 925  // between numbers
    
19. #define LIFT2 725  // going towards sweeper
    
20. 
    
21. // speed of liftimg arm, higher is slower
    
22. #define LIFTSPEED 1500
    
23. 
    
24. // length of arms
    
25. #define L1 35
    
26. #define L2 55.1
    
27. #define L3 13.2
    
28. 
    
29. 
    
30. // origin points of left and right servo
    
31. #define O1X 22
    
32. #define O1Y -25
    
33. #define O2X 47
    
34. #define O2Y -25
    
35. 
    
36. 
    
37. 
    
38. #include <Time.h> // see [url=http://playground.arduino.cc/Code/time]http://playground.arduino.cc/Code/time[/url]
    
39. #include <Servo.h>
    
40. 
    
41. int servoLift = 1500;
    
42. 
    
43. Servo servo1;  //
    
44. Servo servo2;  //
    
45. Servo servo3;  //
    
46. 
    
47. volatile double lastX = 75;
    
48. volatile double lastY = 47.5;
    
49. 
    
50. int last_min = 0;
    
51. 
    
52. void setup()
    
53. {
    
54.   // Set current time only the first to values, hh,mm are needed
    
55.   setTime(19,38,0,0,0,0);
    
56. 
    
57.   drawTo(75.2, 47);
    
58.   lift(0);
    
59.   servo1.attach(SERVOPINLIFT);  //  lifting servo
    
60.   servo2.attach(SERVOPINLEFT);  //  left servo
    
61.   servo3.attach(SERVOPINRIGHT);  //  right servo
    
62.   delay(1000);
    
63. 
    
64. }
    
65. 
    
66. void loop()
    
67. {
    
68. 
    
69. #ifdef CALIBRATION
    
70. 
    
71.   // Servohorns will have 90° between movements, parallel to x and y axis
    
72.   drawTo(-3, 29.2);
    
73.   delay(500);
    
74.   drawTo(74.1, 28);
    
75.   delay(500);
    
76. 
    
77. #else
    
78. 
    
79. 
    
80.   int i = 0;
    
81.   if (last_min != minute()) {
    
82. 
    
83.     if (!servo1.attached()) servo1.attach(SERVOPINLIFT);
    
84.     if (!servo2.attached()) servo2.attach(SERVOPINLEFT);
    
85.     if (!servo3.attached()) servo3.attach(SERVOPINRIGHT);
    
86. 
    
87.     lift(0);
    
88. 
    
89.     hour();
    
90.     while ((i+1)*10 <= hour())
    
91.     {
    
92.       i++;
    
93.     }
    
94. 
    
95.     number(3, 3, 111, 1);
    
96.     number(5, 25, i, 0.9);
    
97.     number(19, 25, (hour()-i*10), 0.9);
    
98.     number(28, 25, 11, 0.9);
    
99. 
    
100.     i=0;
     
101.     while ((i+1)*10 <= minute())
     
102.     {
     
103.       i++;
     
104.     }
     
105.     number(34, 25, i, 0.9);
     
106.     number(48, 25, (minute()-i*10), 0.9);
     
107.     lift(2);
     
108.     drawTo(74.2, 47.5);
     
109.     lift(1);
     
110.     last_min = minute();
     
111. 
     
112.     servo1.detach();
     
113.     servo2.detach();
     
114.     servo3.detach();
     
115.   }
     
116. 
     
117. #endif
     
118. 
     
119. }
     
120. 
     
121. // Writing numeral with bx by being the bottom left originpoint. Scale 1 equals a 20 mm high font.
     
122. // The structure follows this principle: move to first startpoint of the numeral, lift down, draw numeral, lift up
     
123. void number(float bx, float by, int num, float scale) {
     
124. 
     
125.   switch (num) {
     
126. 
     
127.   case 0:
     
128.     drawTo(bx + 12 * scale, by + 6 * scale);
     
129.     lift(0);
     
130.     bogenGZS(bx + 7 * scale, by + 10 * scale, 10 * scale, -0.8, 6.7, 0.5);
     
131.     lift(1);
     
132.     break;
     
133.   case 1:
     
134. 
     
135.     drawTo(bx + 3 * scale, by + 15 * scale);
     
136.     lift(0);
     
137.     drawTo(bx + 10 * scale, by + 20 * scale);
     
138.     drawTo(bx + 10 * scale, by + 0 * scale);
     
139.     lift(1);
     
140.     break;
     
141.   case 2:
     
142.     drawTo(bx + 2 * scale, by + 12 * scale);
     
143.     lift(0);
     
144.     bogenUZS(bx + 8 * scale, by + 14 * scale, 6 * scale, 3, -0.8, 1);
     
145.     drawTo(bx + 1 * scale, by + 0 * scale);
     
146.     drawTo(bx + 12 * scale, by + 0 * scale);
     
147.     lift(1);
     
148.     break;
     
149.   case 3:
     
150.     drawTo(bx + 2 * scale, by + 17 * scale);
     
151.     lift(0);
     
152.     bogenUZS(bx + 5 * scale, by + 15 * scale, 5 * scale, 3, -2, 1);
     
153.     bogenUZS(bx + 5 * scale, by + 5 * scale, 5 * scale, 1.57, -3, 1);
     
154.     lift(1);
     
155.     break;
     
156.   case 4:
     
157.     drawTo(bx + 10 * scale, by + 0 * scale);
     
158.     lift(0);
     
159.     drawTo(bx + 10 * scale, by + 20 * scale);
     
160.     drawTo(bx + 2 * scale, by + 6 * scale);
     
161.     drawTo(bx + 12 * scale, by + 6 * scale);
     
162.     lift(1);
     
163.     break;
     
164.   case 5:
     
165.     drawTo(bx + 2 * scale, by + 5 * scale);
     
166.     lift(0);
     
167.     bogenGZS(bx + 5 * scale, by + 6 * scale, 6 * scale, -2.5, 2, 1);
     
168.     drawTo(bx + 5 * scale, by + 20 * scale);
     
169.     drawTo(bx + 12 * scale, by + 20 * scale);
     
170.     lift(1);
     
171.     break;
     
172.   case 6:
     
173.     drawTo(bx + 2 * scale, by + 10 * scale);
     
174.     lift(0);
     
175.     bogenUZS(bx + 7 * scale, by + 6 * scale, 6 * scale, 2, -4.4, 1);
     
176.     drawTo(bx + 11 * scale, by + 20 * scale);
     
177.     lift(1);
     
178.     break;
     
179.   case 7:
     
180.     drawTo(bx + 2 * scale, by + 20 * scale);
     
181.     lift(0);
     
182.     drawTo(bx + 12 * scale, by + 20 * scale);
     
183.     drawTo(bx + 2 * scale, by + 0);
     
184.     lift(1);
     
185.     break;
     
186.   case 8:
     
187.     drawTo(bx + 5 * scale, by + 10 * scale);
     
188.     lift(0);
     
189.     bogenUZS(bx + 5 * scale, by + 15 * scale, 5 * scale, 4.7, -1.6, 1);
     
190.     bogenGZS(bx + 5 * scale, by + 5 * scale, 5 * scale, -4.7, 2, 1);
     
191.     lift(1);
     
192.     break;
     
193. 
     
194.   case 9:
     
195.     drawTo(bx + 9 * scale, by + 11 * scale);
     
196.     lift(0);
     
197.     bogenUZS(bx + 7 * scale, by + 15 * scale, 5 * scale, 4, -0.5, 1);
     
198.     drawTo(bx + 5 * scale, by + 0);
     
199.     lift(1);
     
200.     break;
     
201. 
     
202.   case 111:
     
203. 
     
204.     lift(0);
     
205.     drawTo(70, 46);
     
206.     drawTo(65, 43);
     
207. 
     
208.     drawTo(65, 49);
     
209.     drawTo(5, 49);
     
210.     drawTo(5, 45);
     
211.     drawTo(65, 45);
     
212.     drawTo(65, 40);
     
213. 
     
214.     drawTo(5, 40);
     
215.     drawTo(5, 35);
     
216.     drawTo(65, 35);
     
217.     drawTo(65, 30);
     
218. 
     
219.     drawTo(5, 30);
     
220.     drawTo(5, 25);
     
221.     drawTo(65, 25);
     
222.     drawTo(65, 20);
     
223. 
     
224.     drawTo(5, 20);
     
225.     drawTo(60, 44);
     
226. 
     
227.     drawTo(75.2, 47);
     
228.     lift(2);
     
229. 
     
230.     break;
     
231. 
     
232.   case 11:
     
233.     drawTo(bx + 5 * scale, by + 15 * scale);
     
234.     lift(0);
     
235.     bogenGZS(bx + 5 * scale, by + 15 * scale, 0.1 * scale, 1, -1, 1);
     
236.     lift(1);
     
237.     drawTo(bx + 5 * scale, by + 5 * scale);
     
238.     lift(0);
     
239.     bogenGZS(bx + 5 * scale, by + 5 * scale, 0.1 * scale, 1, -1, 1);
     
240.     lift(1);
     
241.     break;
     
242. 
     
243.   }
     
244. }
     
245. 
     
246. 
     
247. 
     
248. void lift(char lift) {
     
249.   switch (lift) {
     
250.     // room to optimize  !
     
251. 
     
252.   case 0: //850
     
253. 
     
254.       if (servoLift >= LIFT0) {
     
255.       while (servoLift >= LIFT0)
     
256.       {
     
257.         servoLift--;
     
258.         servo1.writeMicroseconds(servoLift);                                
     
259.         delayMicroseconds(LIFTSPEED);
     
260.       }
     
261.     }
     
262.     else {
     
263.       while (servoLift <= LIFT0) {
     
264.         servoLift++;
     
265.         servo1.writeMicroseconds(servoLift);
     
266.         delayMicroseconds(LIFTSPEED);
     
267. 
     
268.       }
     
269. 
     
270.     }
     
271. 
     
272.     break;
     
273. 
     
274.   case 1: //150
     
275. 
     
276.     if (servoLift >= LIFT1) {
     
277.       while (servoLift >= LIFT1) {
     
278.         servoLift--;
     
279.         servo1.writeMicroseconds(servoLift);
     
280.         delayMicroseconds(LIFTSPEED);
     
281. 
     
282.       }
     
283.     }
     
284.     else {
     
285.       while (servoLift <= LIFT1) {
     
286.         servoLift++;
     
287.         servo1.writeMicroseconds(servoLift);
     
288.         delayMicroseconds(LIFTSPEED);
     
289.       }
     
290. 
     
291.     }
     
292. 
     
293.     break;
     
294. 
     
295.   case 2:
     
296. 
     
297.     if (servoLift >= LIFT2) {
     
298.       while (servoLift >= LIFT2) {
     
299.         servoLift--;
     
300.         servo1.writeMicroseconds(servoLift);
     
301.         delayMicroseconds(LIFTSPEED);
     
302.       }
     
303.     }
     
304.     else {
     
305.       while (servoLift <= LIFT2) {
     
306.         servoLift++;
     
307.         servo1.writeMicroseconds(servoLift);                                
     
308.         delayMicroseconds(LIFTSPEED);
     
309.       }
     
310.     }
     
311.     break;
     
312.   }
     
313. }
     
314. 
     
315. 
     
316. void bogenUZS(float bx, float by, float radius, int start, int ende, float sqee) {
     
317.   float inkr = -0.05;
     
318.   float count = 0;
     
319. 
     
320.   do {
     
321.     drawTo(sqee * radius * cos(start + count) + bx,
     
322.     radius * sin(start + count) + by);
     
323.     count += inkr;
     
324.   }
     
325.   while ((start + count) > ende);
     
326. 
     
327. }
     
328. 
     
329. void bogenGZS(float bx, float by, float radius, int start, int ende, float sqee) {
     
330.   float inkr = 0.05;
     
331.   float count = 0;
     
332. 
     
333.   do {
     
334.     drawTo(sqee * radius * cos(start + count) + bx,
     
335.     radius * sin(start + count) + by);
     
336.     count += inkr;
     
337.   }
     
338.   while ((start + count) <= ende);
     
339. }
     
340. 
     
341. 
     
342. void drawTo(double pX, double pY) {
     
343.   double dx, dy, c;
     
344.   int i;
     
345. 
     
346.   // dx dy of new point
     
347.   dx = pX - lastX;
     
348.   dy = pY - lastY;
     
349.   //path lenght in mm, times 4 equals 4 steps per mm
     
350.   c = floor(4 * sqrt(dx * dx + dy * dy));
     
351. 
     
352.   if (c < 1) c = 1;
     
353. 
     
354.   for (i = 0; i <= c; i++) {
     
355.     // draw line point by point
     
356.     set_XY(lastX + (i * dx / c), lastY + (i * dy / c));
     
357. 
     
358.   }
     
359. 
     
360.   lastX = pX;
     
361.   lastY = pY;
     
362. }
     
363. 
     
364. double return_angle(double a, double b, double c) {
     
365.   // cosine rule for angle between c and a
     
366.   return acos((a * a + c * c - b * b) / (2 * a * c));
     
367. }
     
368. 
     
369. void set_XY(double Tx, double Ty)
     
370. {
     
371.   delay(1);
     
372.   double dx, dy, c, a1, a2, Hx, Hy;
     
373. 
     
374.   // calculate triangle between pen, servoLeft and arm joint
     
375.   // cartesian dx/dy
     
376.   dx = Tx - O1X;
     
377.   dy = Ty - O1Y;
     
378. 
     
379.   // polar lemgth (c) and angle (a1)
     
380.   c = sqrt(dx * dx + dy * dy); //
     
381.   a1 = atan2(dy, dx); //
     
382.   a2 = return_angle(L1, L2, c);
     
383. 
     
384.   servo2.writeMicroseconds(floor(((a2 + a1 - M_PI) * SERVOFAKTOR) + SERVOLEFTNULL));
     
385. 
     
386.   // calculate joinr arm point for triangle of the right servo arm
     
387.   a2 = return_angle(L2, L1, c);
     
388.   Hx = Tx + L3 * cos((a1 - a2 + 0.621) + M_PI); //36,5°
     
389.   Hy = Ty + L3 * sin((a1 - a2 + 0.621) + M_PI);
     
390. 
     
391.   // calculate triangle between pen joint, servoRight and arm joint
     
392.   dx = Hx - O2X;
     
393.   dy = Hy - O2Y;
     
394. 
     
395.   c = sqrt(dx * dx + dy * dy);
     
396.   a1 = atan2(dy, dx);
     
397.   a2 = return_angle(L1, (L2 - L3), c);
     
398. 
     
399.   servo3.writeMicroseconds(floor(((a1 - a2) * SERVOFAKTOR) + SERVORIGHTNULL));
     
400. 
     
401. }
     
402. 
     
403. 
     
404. 
     
405. 
     

复制代码

Damn_intuition

下面是机械部分设计，有兴趣的朋友可以用3D打印做一个。
具体原作者是谁已经不知道了，设计由Johannes Heberlein 提供。

sn10

有视频吗？放上来看看

____消失灬

sn10 发表于 2014-3-26 13:57
有视频吗？放上来看看

应该是那个小贱笔  写时间的那个

Super169

是這個吧.
http://www.thingiverse.com/thing:248009

那一小撮人

你好，问一下程序当中bogenUZS，bogenGZS代表什么意思？

那一小撮人

有没有中文注释的

吖---明

很不错哦，顶上！！！！！！！！！！