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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
|
#include <TimedAction.h>
TimedAction regulateServos = TimedAction(2,regulator);
/*
Snake Sections Snake Servos
Head
Section 1 Servo 1
Servo 2
Section 2 Servo 3
Servo 4
Section 3 Servo 5 - On Slave Board One
Servo 6 - On Slave Board Two
*/
Servo servo1; int servo1RequiredAngle;
Servo servo2; int servo2RequiredAngle;
Servo servo3; int servo3RequiredAngle;
Servo servo4; int servo4RequiredAngle;
#define servoSpeed = 60/0.4
int sectionLength = 10; // Length of snake section in centimeters
int firstServoInSectionInX = true // Assuming the first servo in each section is in the horizontal plane
void moveServoTo(byte servoNum, int angle) {
switch (servoNum) {
case 1:
servo1.write(angle);
servo1.
break:
case 2:
servo2.write(angle);
break:
}
/*
- servo The servo to be moved
- smoothMovement Use smooth movement?
- angle The angle to move the servo to 90 = centre
- time The time to spend moving in milliseconds
*/
void moveServoTo(byte servoNum, boolean smoothMovement, int angle, int time) {
int startPos = servo.read();
int moveAngle = abs(angle - startPos);
int delayAmount = time - (abs(angle - startPos)/ servoSpeed); // The time the servo must waste if it is to get to the angle on time
// WARNING, if this is negitive the servo cant make the movement in time
if (delayAmount <= 0) { // If cant make the movement just try anyway, else move in increments of 1 degree while taking breaks
moveServoTo(servoNum,angle);
} else if (smoothMovement) {
// TODO
} else {
int wasteTimePerDegree = moveAngle/delayAmount;
if (angle > startPos) {
for (int i=0; i<moveAngle; i++) {
moveServoTo(servoNum,(startPos + i));
delay(1/servoSpeed); // Wait for the time taken to move one degree
delay(wasteTimePerDegree);
}
} else {
for (int i=0; i<moveAngle; i++) {
moveServoTo(servoNum,(startPos - i));
delay(1/servoSpeed); // Wait for the time taken to move one degree
delay(wasteTimePerDegree);
}
}
}
}
void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
moveServoTo((sectionNum*2)-1,false,xAngle,time);
moveServoTo((sectionNum*2),false,yAngle,time);
}
/*
- xArcRad
*/
void snakeArc(byte firstSectionNum, byte lastSectionNum, int xArcRadius, int yArcRadius, int time) {
snakeBend(firstSectionNum,lastSectionNum, ((lastSectionNum-firstSectionNum)*sectionLength)/xArcRadius, ((lastSectionNum-firstSectionNum)*sectionLength)/yArcRadius, time)
}
/*
- xArcRad
*/
void snakeBend(byte firstSectionNum, byte lastSectionNum, int xAngle, int yAngle, int time) {
int xAnglePerSection = xAngle/(lastSectionNum - firstSectionNum);
int yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
for (int i=firstSectionNum; i<=lastSectionNum; i++) {
moveSectionTo(i,xAnglePerSection,yAnglePerSection, time)
}
}
void regulator() {
}
|