From 7c34801c3908aac90b59a6d92e8e3a8ebb9c7a33 Mon Sep 17 00:00:00 2001 From: Christopher Baines Date: Sat, 26 Mar 2011 19:16:26 +0000 Subject: Major changes including implementing a regulator to manage speed regulation of the servos Needs more work to inprove though --- SnakeMaster/SnakeMaster.pde | 235 ++++++++++++++++++++++---------------------- 1 file 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 . -*/ - -#include - -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 .*/ + +#include +#include + +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 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); + } +} + -- cgit v1.2.3