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
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
|
/* Copyright 2011 Christopher Baines <cbaines8@gmail.com>
Copyright 2011 Adam "Insert other details here if wished"
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <TimedAction.h>
TimedAction servo1Regulator = TimedAction((1/servoSpeed),regulator(1));
TimedAction servo2Regulator = TimedAction((1/servoSpeed),regulator(2));
TimedAction servo3Regulator = TimedAction((1/servoSpeed),regulator(3));
TimedAction servo4Regulator = TimedAction((1/servoSpeed),regulator(4));
/* TODO:
- Communication bewteen the other chips,
- Finalise the number of servos on this chip and correct the servo code accordingly
- Ammend the servo handeling functions to deal with the remote servos
- Work out if manual speed regulation of the servos is feasible
- Write the comm code for the slaves
- Add debug statements to the program using the debug function
*/
/*
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
*/
// Regulator Settings:
#define minMovementAmmount 1;
#define debug 2; // 0 = No debug output 1 = minimal debug output 2 = maximum debug output
Servo servo1; int servo1TargetAngle; long servo1TargetTime;
Servo servo2; int servo2TargetAngle; long servo2TargetTime;
Servo servo3; int servo3TargetAngle; long servo3TargetTime;
Servo servo4; int servo4TargetAngle; long servo4TargetTime;
#define servoSpeed = 0.15 // 60 degrees in 0.4 seconds (400 milliseconds)
#define sectionLength 10; // Length of snake section in centimeters
#define firstServoInSectionInX true // Assuming the first servo in each section is in the horizontal plane
void debug(String message, byte debugLevel) {
if (debug >= debugLevel) Serial.println(message);
}
void moveServoTo(byte servoNum, int angle) {
switch (servoNum) {
case 1:
servo1.write(angle);
servo1.
break:
case 2:
servo2.write(angle);
break:
}
int getServoAngle(byte servoNum) {
switch (servoNum) {
case 1:
return servo1.read();
case 2:
return servo2.read();
}
}
int getServoTargetAngle(byte servoNum) {
switch (servoNum) {
case 1:
return servo1TargetAngle;
case 2:
return servo2TargetAngle;
}
}
int getServoTargetTime(byte servoNum) {
switch (servoNum) {
case 1:
return servo1TargetTime;
case 2:
return servo2TargetTime;
}
}
/*
- 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(byte servoNum) {
if (getServoAngle(servoNum) != getServoTargetAngle(servoNum)) {
int delayAmount = (getServoTargetTime(servoNum) - millis()) - (abs(angle - startPos)/ servoSpeed);
moveServoTo
} else {
}
}
|