Major changes to the regulator, removed delay and added update times

1 files changed, 87 insertions, 43 deletions

diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
index 5b6c77d..b07d149 100644
--- a/SnakeMaster/SnakeMaster.pde
+++ b/SnakeMaster/SnakeMaster.pde
@@ -20,13 +20,14 @@
 const int debugLevel = 2; // 0 = No debug output 1 = main debug output 2 = low level movement debug output 3 = regulator output
 
 const int numberOfServos=2;
+const int servoSidewaysMovementLimit=85;
 Servo servo[numberOfServos];
 int servoTargetAngle[numberOfServos];
+int servoOffset[numberOfServos];
 int servoTargetTime[numberOfServos];
-//int servoLastUpdateTime[numberOfServos]; Currently not used
+int servoNextUpdateTime[numberOfServos];
 
 const int servoSpeed = 0.15; // 60 degrees in 0.4 seconds (400 milliseconds)
-const int minMovementAmmount = 1;
 const float sectionLength = 10; // Length of snake section in centimeters
 
 void setup() {
@@ -84,6 +85,7 @@ void loop() {
 void moveServoTo(byte servoNum, int angle, int time) {
   servoTargetAngle[servoNum] = angle;
   servoTargetTime[servoNum] = time + millis();
+  servoNextUpdateTime[servoNum] = 0; // Set update time to 0 to force regulator to pay attention 
 }
 
 void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
@@ -179,16 +181,46 @@ void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius,
   }
 }
 
+/*
+ - coilGraduation delay between sections moving means start at the top (in millis)
+ - reverseDirection false = coil clockwise true = coil anti-clockwise
+ */
+
+void snakeCoil(byte firstSectionNum, byte lastSectionNum, float coilDiameter, float coilHeight, boolean reverseDirection, int time, int coilGraduation) {
+  if (coilGraduation == 0) {
+    if (firstSectionNum > lastSectionNum) {
+      snakeArc(lastSectionNum, firstSectionNum, coilDiameter, coilHeight, time);
+    } 
+    else  {
+      snakeArc(firstSectionNum, lastSectionNum, coilDiameter, coilHeight, time);
+    }
+  } 
+  else {
+    int timeToMoveSection = time - (abs(coilGraduation) * abs(firstSectionNum-lastSectionNum));
+    if (firstSectionNum > lastSectionNum) {
+      for (int section=lastSectionNum; section>=firstSectionNum; section--) {
+        moveSectionTo(section, xAngle, int yAngle, int time)
+        } 
+      } 
+    else {
+      for (int section=firstSectionNum; section<=lastSectionNum; section++) {
+
+      } 
+    }
+  }
+}
+
 // Regulator and Delay Stuff
 
 // This MUST be used instead of delay() or the regulator will not work!
 void snakeDelay(int time) {
   int endTime = (millis() + time);
+  regulator();
   while (millis() < endTime) regulator();
 }
 
 void regulator() {
-  for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) { // servoNum<=8 needs correcting
+  for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) {
     debug("Regulating ",3);
     debugln(servoNum, 3);
     int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
@@ -201,30 +233,39 @@ void regulator() {
       debug(" Servo target angle ",3);
       debugln(servoTargetAngle[servoNum],3);
 
-      int delayAmount = (servoTargetTime[servoNum] - millis()) - (abs(servo[servoNum].read() - servoTargetAngle[servoNum])/ servoSpeed);
+      // The delay ammount per one degree of movement
+      int delayAmount = ((servoTargetTime[servoNum] - millis()) - (abs(remainingMovement)/servoSpeed))/abs(remainingMovement);
       debug("Delay Ammount",3);
       debugln(delayAmount,3);
+
+      int movement;
+
       if (delayAmount > 0) {
-        if (abs(remainingMovement) < minMovementAmmount) {
-          debug("Remaining movement less than min ammount",3);
-          delay(delayAmount);
-          servo[servoNum].write(remainingMovement);
-        } 
+        // Next update time = time for one degree of movement + delay ammount per one degree of movement ((time left - min time needed)/movem
+        // nts remaining)
+        servoNextUpdateTime[servoNum] = (1/servoSpeed) + ((servoTargetTime[servoNum] - millis()) - (abs(remainingMovement)/servoSpeed))/abs(remainingMovement);
+        if (remainingMovement < 0) {
+          movement = servo[servoNum].read() - 1 + servoOffset[servoNum];
+        }
         else {
-          debug("Remaining movement greater than min ammount delaying",3);
-          debugln(delayAmount/(abs(remainingMovement)/minMovementAmmount),3);
-          delay(delayAmount/(abs(remainingMovement)/minMovementAmmount));
-          if (remainingMovement < 0) {
-            servo[servoNum].write(servo[servoNum].read() - minMovementAmmount);
-          } 
-          else {
-            servo[servoNum].write(servo[servoNum].read() + minMovementAmmount);
-          }
+          movement = servo[servoNum].read() + 1 + servoOffset[servoNum];
         }
+      }
+      else {
+        movement = servoTargetAngle[servoNum];
+      }
+
+      if (movement > servoSidewaysMovementLimit) {
+        servo[servoNum].write(servoSidewaysMovementLimit);
+      } 
+      else if (movement < servoSidewaysMovementLimit) {
+        servo[servoNum].write(-servoSidewaysMovementLimit);
       } 
       else {
-        servo[servoNum].write(servoTargetAngle[servoNum]);
+        servo[servoNum].write(movement);
       }
+
+      servoNextUpdateTime[servoNum] = 
     }
   }   
 }
@@ -246,18 +287,18 @@ void debugln(String message, int messageDebugLevel) {
 }
 
 /*void debugln(int message, int messageDebugLevel) {
-  if (messageDebugLevel <= debugLevel) {
-    /*if (messageDebugLevel == 1) {
-     Serial.println("  "+message);
-     } 
-     else if (messageDebugLevel == 2) {
-     Serial.println("    "+message);
-     } 
-     else {
-    Serial.println(message);
-    //}
-  }
-}*/
+ if (messageDebugLevel <= debugLevel) {
+/*if (messageDebugLevel == 1) {
+ Serial.println("  "+message);
+ } 
+ else if (messageDebugLevel == 2) {
+ Serial.println("    "+message);
+ } 
+ else {
+ Serial.println(message);
+ //}
+ }
+ }*/
 
 void debugln(float message, int messageDebugLevel) {
   if (messageDebugLevel <= debugLevel) {
@@ -288,18 +329,18 @@ void debug(String message, int messageDebugLevel) {
 }
 
 /*void debug(int message, int messageDebugLevel) {
-  if (messageDebugLevel <= debugLevel) {
-    /*if (messageDebugLevel == 1) {
-     Serial.print("  "+message);
-     } 
-     else if (messageDebugLevel == 2) {
-     Serial.print("    "+message);
-     } 
-     else {
-    Serial.print(message);
-    //}
-  }
-}*/
+ if (messageDebugLevel <= debugLevel) {
+/*if (messageDebugLevel == 1) {
+ Serial.print("  "+message);
+ } 
+ else if (messageDebugLevel == 2) {
+ Serial.print("    "+message);
+ } 
+ else {
+ Serial.print(message);
+ //}
+ }
+ }*/
 
 void debug(float message, int messageDebugLevel) {
   if (messageDebugLevel <= debugLevel) {
@@ -319,3 +360,6 @@ void debug(float message, int messageDebugLevel) {
 
 
 
+
+
+