Opps, the arduino IDE moved the file, this is the updated file for the last commit in the new location.

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 {
+    
+  }
+}
+