summaryrefslogtreecommitdiff
path: root/SnakeMaster
diff options
context:
space:
mode:
authorChristopher Baines <cbaines8@gmail.com>2011-03-26 19:16:26 +0000
committerChristopher Baines <cbaines8@gmail.com>2011-03-26 19:16:26 +0000
commit7c34801c3908aac90b59a6d92e8e3a8ebb9c7a33 (patch)
tree859475bf9ef9877610a8e920b904baa0ea61868f /SnakeMaster
parent7ebc7b470ec2dbb8ba5c0fc947afd9392e6c1976 (diff)
downloadsnakerobot-7c34801c3908aac90b59a6d92e8e3a8ebb9c7a33.tar
snakerobot-7c34801c3908aac90b59a6d92e8e3a8ebb9c7a33.tar.gz
Major changes including implementing a regulator to manage speed regulation of the servos
Needs more work to inprove though
Diffstat (limited to 'SnakeMaster')
-rw-r--r--SnakeMaster/SnakeMaster.pde235
1 files changed, 118 insertions, 117 deletions
diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
index 2763852..683816d 100644
--- a/SnakeMaster/SnakeMaster.pde
+++ b/SnakeMaster/SnakeMaster.pde
@@ -12,91 +12,62 @@
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);
+ along with this program. If not, see <http://www.gnu.org/licenses/>.*/
+
+#include <math.h>
+#include <Servo.h>
+
+int debugLevel = 2; // 0 = No debug output 1 = minimal debug output 2 = maximum debug output
+
+Servo servo[8];
+int servoTargetAngle[8];
+int servoTargetTime[8];
+int servoLastUpdateTime[8];
+int button = 24;
+
+const int servoSpeed = 0.15; // 60 degrees in 0.4 seconds (400 milliseconds)
+const int minMovementAmmount = 1;
+const int sectionLength = 10; // Length of snake section in centimeters
+
+void setup() {
+ Serial.begin(9600);
+ debug("Setup started",0);
+ //debug("Where in!", 2);
+ // Setup Servos
+ servo[0].attach(2);
+ //debug("Done attaching servos",2);
+ //pinMode(button, INPUT);
+ debug("Finish setup",0);
}
-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();
- }
+void loop() {
+ debug("----------- Program started ----------------",0);
+ //boolean state = digitalRead(button);
+ //while (state != HIGH) {
+ // debug("Putton not pressed delaying",0);
+ // delay(10);
+ //}
+ debug("Button pressed starting routine",0);
+ moveServoTo(0,0,0); // Reset servo to zero
+ debug("Made first movement",0);
+ snakeDelay(2000);
+ debug("Finished first movement",0);
+ moveServoTo(0,180,2000);
+ debug("Made second movement",0);
+ snakeDelay(2000);
+ debug("Finished second movement",0);
+ moveServoTo(0,0,2000);
+ debug("Made third movement",0);
+ snakeDelay(2000);
+ debug("End of loop going round again",0);
}
-int getServoTargetAngle(byte servoNum) {
- switch (servoNum) {
- case 1:
- return servo1TargetAngle;
- case 2:
- return servo2TargetAngle;
- }
+void debug(String message, int messageDebugLevel) {
+ if (messageDebugLevel <= debugLevel) Serial.println(message);
}
-int getServoTargetTime(byte servoNum) {
- switch (servoNum) {
- case 1:
- return servo1TargetTime;
- case 2:
- return servo2TargetTime;
- }
+void debug(int message, int messageDebugLevel) {
+ if (messageDebugLevel <= debugLevel) Serial.println(message);
}
/*
@@ -105,37 +76,17 @@ int getServoTargetTime(byte servoNum) {
- 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 moveServoTo(byte servoNum, int angle, int time) {
+ //debug("Setting servo",0);
+ servoTargetAngle[servoNum] = angle;
+ servoTargetTime[servoNum] = time + millis();
}
void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
- moveServoTo((sectionNum*2)-1,false,xAngle,time);
- moveServoTo((sectionNum*2),false,yAngle,time);
+ //debug("Setting section " + sectionNum + " to move to " + xAngle + " x and " + yAngle + " y in " + time " milliseconds");
+ moveServoTo((sectionNum*2)-1,xAngle,time);
+ moveServoTo((sectionNum*2),yAngle,time);
}
/*
@@ -143,7 +94,7 @@ void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
*/
void snakeArc(byte firstSectionNum, byte lastSectionNum, int xArcRadius, int yArcRadius, int time) {
- snakeBend(firstSectionNum,lastSectionNum, ((lastSectionNum-firstSectionNum)*sectionLength)/xArcRadius, ((lastSectionNum-firstSectionNum)*sectionLength)/yArcRadius, time)
+ snakeBend(firstSectionNum,lastSectionNum, (((lastSectionNum-firstSectionNum)*sectionLength)/xArcRadius), (((lastSectionNum-firstSectionNum)*sectionLength)/yArcRadius), time);
}
/*
@@ -155,18 +106,68 @@ void snakeBend(byte firstSectionNum, byte lastSectionNum, int xAngle, int yAngle
int yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
for (int i=firstSectionNum; i<=lastSectionNum; i++) {
- moveSectionTo(i,xAnglePerSection,yAnglePerSection, time)
- }
-
+ moveSectionTo(i,xAnglePerSection,yAnglePerSection, time);
+ }
}
-void regulator(byte servoNum) {
- if (getServoAngle(servoNum) != getServoTargetAngle(servoNum)) {
- int delayAmount = (getServoTargetTime(servoNum) - millis()) - (abs(angle - startPos)/ servoSpeed);
- moveServoTo
-
+/*
+ - startAngle/endAngle = right=0 working round anticlockwise
+
+*/
+void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, int arcRadius, int startAngle, int endAngle, boolean reverseDirection, int time) {
+ int arcAngle = asin(arcRadius/(sectionLength*(lastSectionNum-firstSectionNum)));
+ if (!reverseDirection) {
+ for (int angle=startAngle; angle<endAngle; angle++) {
+ snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
+ }
} else {
-
+ for (int angle=startAngle; angle<endAngle; angle--) {
+ snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
+ }
}
}
+void snakeDelay(int time) {
+ int endTime = (millis() + time);
+ while (millis() < endTime) regulator();
+}
+
+void regulator() {
+ //debug("Regulating",0);
+ for (byte servoNum=0; servoNum<=0; servoNum++) { // servoNum<=8 needs correcting
+ debug("in for loop for servo",0);
+ debug(servoNum,0);
+ debug("Servo angle",0);
+ debug(servo[servoNum].read(),0);
+ debug("Servo target angle",0);
+ debug(servoTargetAngle[servoNum],0);
+ int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
+ debug("Remaining movement",0);
+ debug(remainingMovement,0);
+ if (remainingMovement != 0) {
+ int delayAmount = (servoTargetTime[servoNum] - millis()) - (abs(servo[servoNum].read() - servoTargetAngle[servoNum])/ servoSpeed);
+ debug("Delay Ammount",0);
+ debug(delayAmount,0);
+ if (delayAmount > 0) {
+ if (remainingMovement < minMovementAmmount) {
+ debug("Remaining movement less than min ammount",0);
+ delay(delayAmount);
+ servo[servoNum].write(remainingMovement);
+ } else {
+ debug("Remaining movement greater than min ammount delaying",0);
+ delay(delayAmount/(abs(remainingMovement)/minMovementAmmount));
+ debug(delayAmount/(abs(remainingMovement)/minMovementAmmount),0);
+ if (remainingMovement < 0) {
+ servo[servoNum].write(servo[servoNum].read() - minMovementAmmount);
+ } else {
+ servo[servoNum].write(servo[servoNum].read() + minMovementAmmount);
+ }
+ }
+ } else {
+ servo[servoNum].write(servoTargetAngle[servoNum]);
+ }
+ }
+ //debug("Finished servo movement",0);
+ }
+}
+