diff options
| -rw-r--r-- | SnakeMaster/SnakeMaster.pde | 155 |
1 files changed, 155 insertions, 0 deletions
diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde new file mode 100644 index 0000000..1150089 --- /dev/null +++ b/SnakeMaster/SnakeMaster.pde @@ -0,0 +1,155 @@ +#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 { + + } +} + |