diff options
| -rw-r--r-- | SnakeMaster.pde | 104 |
1 files changed, 0 insertions, 104 deletions
diff --git a/SnakeMaster.pde b/SnakeMaster.pde deleted file mode 100644 index ae29b5d..0000000 --- a/SnakeMaster.pde +++ /dev/null @@ -1,104 +0,0 @@ -#include <TimedAction.h> - - - -TimedAction regulateServos = TimedAction(2,regulator); - -/* -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 -*/ - -Servo servo1; int servo1RequiredAngle; -Servo servo2; int servo2RequiredAngle; -Servo servo3; int servo3RequiredAngle; -Servo servo4; int servo4RequiredAngle; - -#define servoSpeed = 60/0.4 -int sectionLength = 10; // Length of snake section in centimeters -int firstServoInSectionInX = true // Assuming the first servo in each section is in the horizontal plane - -void moveServoTo(byte servoNum, int angle) { - switch (servoNum) { - case 1: - servo1.write(angle); - servo1. - break: - case 2: - servo2.write(angle); - break: -} - -/* - - 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() { - - -} - |