Added a servo test routine at the end of the loop functionHEADmaster

Finished modifying the regulator, needs urgent testing!
Started writing snakeStraighten and snakeWave functions

1 files changed, 62 insertions, 43 deletions

diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
index b07d149..f5eec9b 100644
--- a/SnakeMaster/SnakeMaster.pde
+++ b/SnakeMaster/SnakeMaster.pde
@@ -73,6 +73,16 @@ void loop() {
    moveServoTo(0,90,0); // Reset servos to zero
    moveServoTo(1,90,0);
    snakeDelay(3000);*/
+   
+   // Snake Servo Test - Move each servo one by one through its full movement
+   for (int servoNum=0; servoNum<numberOfServos; numberOfServos++) {
+     moveServoTo(servoNum, 180, 0);
+     snakeDelay(2000);
+     moveServoTo(servoNum, 0, 0);
+     snakeDelay(2000);
+     moveServoTo(servoNum, 90, 0);
+     snakeDelay(2000);
+   }  
 }
 
 /*
@@ -182,6 +192,8 @@ void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius,
 }
 
 /*
+ - This will begin at the first section and end at the last, this means that it the snake wants
+   to begin coiling at the 7th section and end at the 2nd firstSectionNum=7 lastSectionNum=2
  - coilGraduation delay between sections moving means start at the top (in millis)
  - reverseDirection false = coil clockwise true = coil anti-clockwise
  */
@@ -195,8 +207,8 @@ void snakeCoil(byte firstSectionNum, byte lastSectionNum, float coilDiameter, fl
       snakeArc(firstSectionNum, lastSectionNum, coilDiameter, coilHeight, time);
     }
   } 
-  else {
-    int timeToMoveSection = time - (abs(coilGraduation) * abs(firstSectionNum-lastSectionNum));
+  else { // TODO This is NOT finished
+    int timeToMoveSection = time - (coilGraduation * abs(firstSectionNum-lastSectionNum));
     if (firstSectionNum > lastSectionNum) {
       for (int section=lastSectionNum; section>=firstSectionNum; section--) {
         moveSectionTo(section, xAngle, int yAngle, int time)
@@ -210,6 +222,11 @@ void snakeCoil(byte firstSectionNum, byte lastSectionNum, float coilDiameter, fl
   }
 }
 
+void snakeStraighten(byte firstSectionNum, byte lastSectionNum, int time, int graduation) {
+  int timeToMoveSection = time - (abs(graduation) * abs(firstSectionNum-lastSectionNum));
+  
+}
+
 // Regulator and Delay Stuff
 
 // This MUST be used instead of delay() or the regulator will not work!
@@ -223,51 +240,52 @@ void regulator() {
   for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) {
     debug("Regulating ",3);
     debugln(servoNum, 3);
-    int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
-    if (remainingMovement != 0) {
-      debug("Remaining movement ",3);
-      debugln(remainingMovement,3);
-
-      debug("Servo angle ",3);
-      debug(servo[servoNum].read(),3);
-      debug(" Servo target angle ",3);
-      debugln(servoTargetAngle[servoNum],3);
-
-      // 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) {
-        // 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];
+    if ((servoNextUpdateTime[servoNum] <= millis()) && (servoNextUpdateTime[servoNum] != -1)) {
+      int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
+      if (remainingMovement != 0) {
+        debug("Remaining movement ",3);
+        debugln(remainingMovement,3);
+
+        debug("Servo angle ",3);
+        debug(servo[servoNum].read(),3);
+        debug(" Servo target angle ",3);
+        debugln(servoTargetAngle[servoNum],3);
+
+        // 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) {
+          // 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) + delayAmount;
+          if (remainingMovement < 0) {
+            movement = servo[servoNum].read() - 1 + servoOffset[servoNum];
+          }
+          else {
+            movement = servo[servoNum].read() + 1 + servoOffset[servoNum];
+          }
         }
         else {
-          movement = servo[servoNum].read() + 1 + servoOffset[servoNum];
+          movement = servoTargetAngle[servoNum];
+          servoNextUpdateTime[servoNum] = -1 // basicaly dont update until told otherwise (by the servoMoveTo method setting the value to 0)
         }
-      }
-      else {
-        movement = servoTargetAngle[servoNum];
-      }
 
-      if (movement > servoSidewaysMovementLimit) {
-        servo[servoNum].write(servoSidewaysMovementLimit);
-      } 
-      else if (movement < servoSidewaysMovementLimit) {
-        servo[servoNum].write(-servoSidewaysMovementLimit);
-      } 
-      else {
-        servo[servoNum].write(movement);
+        if (movement > servoSidewaysMovementLimit) {
+          servo[servoNum].write(servoSidewaysMovementLimit);
+        } 
+        else if (movement < servoSidewaysMovementLimit) {
+          servo[servoNum].write(-servoSidewaysMovementLimit);
+        } 
+        else {
+          servo[servoNum].write(movement);
+        }
       }
-
-      servoNextUpdateTime[servoNum] = 
-    }
-  }   
+    }   
+  }
 }
 
 // Debug Stuff
@@ -362,4 +380,5 @@ void debug(float message, int messageDebugLevel) {
 
 
 
-

+
+