基于arduino的记时机器人.docx

1、基于arduino的记时机器人/ units: mm; microseconds; radians/ origin: bottom left of drawing surface/ delete or mark the next line as comment when done with calibration /#define CALIBRATION/ When in calibration mode, adjust the following factor until the servos move exactly 90 degrees#define SERVOFAKTOR 620/ Z

2、ero-position of left and right servo/ When in calibration mode, adjust the NULL-values so that the servo arms are at all times parallel/ either to the X or Y axis#define SERVOLEFTNULL 1900#define SERVORIGHTNULL 984#define SERVOPINLIFT 2#define SERVOPINLEFT 3#define SERVOPINRIGHT 4/ lift positions of

3、 lifting servo#define LIFT0 980/1080 / on drawing surface#define LIFT1 825/925 / between numbers#define LIFT2 625/725 / going towards sweeper/ speed of liftimg arm, higher is slower#define LIFTSPEED 1500/ length of arms#define L1 35#define L2 55.1#define L3 13.2/ origin points of left and right serv

4、o #define O1X 22#define O1Y -25#define O2X 47#define O2Y -25#include / see http:/playground.arduino.cc/Code/time#include int servoLift = 1500;Servo servo1; / Servo servo2; / Servo servo3; / volatile double lastX = 75;volatile double lastY = 47.5;int last_min = 0;void setup() / Set current time only

5、the first to values, hh,mm are needed setTime(12,18,0,0,0,0); drawTo(75.2, 47); lift(0); servo1.attach(SERVOPINLIFT); / lifting servo servo2.attach(SERVOPINLEFT); / left servo servo3.attach(SERVOPINRIGHT); / right servo delay(1000); void loop() #ifdef CALIBRATION / Servohorns will have 90 between mo

6、vements, parallel to x and y axis drawTo(-3, 29.2); delay(500); drawTo(74.1, 28); delay(500);#else int i = 0; if (last_min != minute() if (!servo1.attached() servo1.attach(SERVOPINLIFT); if (!servo2.attached() servo2.attach(SERVOPINLEFT); if (!servo3.attached() servo3.attach(SERVOPINRIGHT); lift(0);

7、 hour(); while (i+1)*10 = hour() i+; number(3, 3, 111, 1); number(5, 25, i, 1); number(19, 25, (hour()-i*10), 1); number(28, 25, 11, 1); i=0; while (i+1)*10 = LIFT0) while (servoLift = LIFT0) servoLift-; servo1.writeMicroseconds(servoLift); delayMicroseconds(LIFTSPEED); else while (servoLift = LIFT1

8、) while (servoLift = LIFT1) servoLift-; servo1.writeMicroseconds(servoLift); delayMicroseconds(LIFTSPEED); else while (servoLift = LIFT2) while (servoLift = LIFT2) servoLift-; servo1.writeMicroseconds(servoLift); delayMicroseconds(LIFTSPEED); else while (servoLift ende);void bogenGZS(float bx, float

9、 by, float radius, int start, int ende, float sqee) float inkr = 0.05; float count = 0; do drawTo(sqee * radius * cos(start + count) + bx, radius * sin(start + count) + by); count += inkr; while (start + count) = ende);void drawTo(double pX, double pY) double dx, dy, c; int i; / dx dy of new point d

10、x = pX - lastX; dy = pY - lastY; /path lenght in mm, times 4 equals 4 steps per mm c = floor(4 * sqrt(dx * dx + dy * dy); if (c 1) c = 1; for (i = 0; i = c; i+) / draw line point by point set_XY(lastX + (i * dx / c), lastY + (i * dy / c); lastX = pX; lastY = pY;double return_angle(double a, double b

11、, double c) / cosine rule for angle between c and a return acos(a * a + c * c - b * b) / (2 * a * c);void set_XY(double Tx, double Ty) delay(1); double dx, dy, c, a1, a2, Hx, Hy; / calculate triangle between pen, servoLeft and arm joint / cartesian dx/dy dx = Tx - O1X; dy = Ty - O1Y; / polar lemgth

12、(c) and angle (a1) c = sqrt(dx * dx + dy * dy); / a1 = atan2(dy, dx); / a2 = return_angle(L1, L2, c); servo2.writeMicroseconds(floor(a2 + a1 - M_PI) * SERVOFAKTOR) + SERVOLEFTNULL); / calculate joinr arm point for triangle of the right servo arm a2 = return_angle(L2, L1, c); Hx = Tx + L3 * cos(a1 -

13、a2 + 0.621) + M_PI); /36,5 Hy = Ty + L3 * sin(a1 - a2 + 0.621) + M_PI); / calculate triangle between pen joint, servoRight and arm joint dx = Hx - O2X; dy = Hy - O2Y; c = sqrt(dx * dx + dy * dy); a1 = atan2(dy, dx); a2 = return_angle(L1, (L2 - L3), c); servo3.writeMicroseconds(floor(a1 - a2) * SERVOFAKTOR) + SERVORIGHTNULL);