Initial commit

1 files changed, 104 insertions, 0 deletions

diff --git a/SnakeMaster.pde b/SnakeMaster.pde
new file mode 100644
index 0000000..ae29b5d
--- /dev/null
+++ b/SnakeMaster.pde
@@ -0,0 +1,104 @@
+#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() {
+  
+  
+}
+