diff options
| -rw-r--r-- | SnakeMaster/SnakeMaster.pde | 235 |
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); + } +} + |