Hopefully fixed regulator problems and started to inprove the debuging

1 files changed, 117 insertions, 66 deletions

diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
index 683816d..94256f6 100644
--- a/SnakeMaster/SnakeMaster.pde
+++ b/SnakeMaster/SnakeMaster.pde
@@ -1,29 +1,29 @@
 /*  Copyright 2011 Christopher Baines <cbaines8@gmail.com>
-    Copyright 2011 Adam "Insert other details here if wished"
-
-    This program is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.*/
+ Copyright 2011 Adam "Insert other details here if wished"
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.*/
 
 #include <math.h>
 #include <Servo.h>
 
-int debugLevel = 2; // 0 = No debug output 1 = minimal debug output 2 = maximum debug output
+const int debugLevel = 2; // 0 = No debug output  1 = main debug output 2 = low level movement debug output
 
-Servo servo[8];
-int servoTargetAngle[8];
-int servoTargetTime[8];
-int servoLastUpdateTime[8];
-int button = 24;
+const int numberOfServos=1;
+Servo servo[numberOfServos];
+int servoTargetAngle[numberOfServos];
+int servoTargetTime[numberOfServos];
+//int servoLastUpdateTime[numberOfServos]; Currently not used
 
 const int servoSpeed = 0.15; // 60 degrees in 0.4 seconds (400 milliseconds)
 const int minMovementAmmount = 1;
@@ -31,43 +31,35 @@ const int sectionLength = 10; // Length of snake section in centimeters
 
 void setup() {
   Serial.begin(9600);
-  debug("Setup started",0);
+  debug("Setup started",1);
   //debug("Where in!", 2);
   // Setup Servos
   servo[0].attach(2);
   //debug("Done attaching servos",2);
   //pinMode(button, INPUT);
-  debug("Finish setup",0);
+  debug("Finish setup",1);
 }
 
 void loop() {
-  debug("----------- Program started ----------------",0);
+  debug("----------- Program started ----------------",1);
   //boolean state = digitalRead(button);
   //while (state != HIGH) {
   //  debug("Putton not pressed delaying",0);
   //  delay(10);
   //}
-  debug("Button pressed starting routine",0);
+  debug("Button pressed starting routine",1);
   moveServoTo(0,0,0); // Reset servo to zero
-  debug("Made first movement",0);
+  debug("Made first movement",1);
   snakeDelay(2000);
-  debug("Finished first movement",0);
+  debug("Finished first movement",1);
   moveServoTo(0,180,2000);
-  debug("Made second movement",0);
+  debug("Made second movement",1);
   snakeDelay(2000);
-  debug("Finished second movement",0);
+  debug("Finished second movement",1);
   moveServoTo(0,0,2000);
-  debug("Made third movement",0);
+  debug("Made third movement",1);
   snakeDelay(2000);
-  debug("End of loop going round again",0);
-}
-
-void debug(String message, int messageDebugLevel) {
-  if (messageDebugLevel <= debugLevel) Serial.println(message);
-}
-
-void debug(int message, int messageDebugLevel) {
-  if (messageDebugLevel <= debugLevel) Serial.println(message);
+  debug("End of loop going round again",1);
 }
 
 /*
@@ -75,7 +67,7 @@ void debug(int message, int messageDebugLevel) {
  - 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, int angle, int time) {
   //debug("Setting servo",0);
@@ -91,20 +83,20 @@ void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int 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);
   } 
@@ -112,15 +104,16 @@ void snakeBend(byte firstSectionNum, byte lastSectionNum, int xAngle, int yAngle
 
 /*
  - startAngle/endAngle = right=0 working round anticlockwise
-
-*/
+ 
+ */
 void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, int arcRadius, int startAngle, int endAngle, boolean reverseDirection, int time) {
   int arcAngle = asin(arcRadius/(sectionLength*(lastSectionNum-firstSectionNum)));
   if (!reverseDirection) {
     for (int angle=startAngle; angle<endAngle; angle++) {
       snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
     }
-  } else {
+  } 
+  else {
     for (int angle=startAngle; angle<endAngle; angle--) {
       snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
     }
@@ -133,41 +126,99 @@ void snakeDelay(int time) {
 }
 
 void regulator() {
-  //debug("Regulating",0);
   for (byte servoNum=0; servoNum<=0; servoNum++) { // servoNum<=8 needs correcting
-    debug("in for loop for servo",0);
-    debug(servoNum,0);
-    debug("Servo angle",0);
-    debug(servo[servoNum].read(),0);
-    debug("Servo target angle",0);
-    debug(servoTargetAngle[servoNum],0);
+    debug("Regulating ",2);
+    debugln(servoNum, 2);
     int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
-    debug("Remaining movement",0);
-    debug(remainingMovement,0);
     if (remainingMovement != 0) {
+      debug("Remaining movement ",2);
+      debugln(remainingMovement,2);
+
+      debug("Servo angle ",2);
+      debug(servo[servoNum].read(),2);
+      debug(" Servo target angle ",2);
+      debugln(servoTargetAngle[servoNum],2);
+
       int delayAmount = (servoTargetTime[servoNum] - millis()) - (abs(servo[servoNum].read() - servoTargetAngle[servoNum])/ servoSpeed);
-      debug("Delay Ammount",0);
-      debug(delayAmount,0);
+      debug("Delay Ammount",2);
+      debugln(delayAmount,2);
       if (delayAmount > 0) {
-        if (remainingMovement < minMovementAmmount) {
-          debug("Remaining movement less than min ammount",0);
+        if (abs(remainingMovement) < minMovementAmmount) {
+          debug("Remaining movement less than min ammount",2);
           delay(delayAmount);
           servo[servoNum].write(remainingMovement);
-        } else {
-          debug("Remaining movement greater than min ammount delaying",0);
+        } 
+        else {
+          debug("Remaining movement greater than min ammount delaying",2);
+          debugln(delayAmount/(abs(remainingMovement)/minMovementAmmount),2);
           delay(delayAmount/(abs(remainingMovement)/minMovementAmmount));
-          debug(delayAmount/(abs(remainingMovement)/minMovementAmmount),0);
           if (remainingMovement < 0) {
             servo[servoNum].write(servo[servoNum].read() - minMovementAmmount);
-          } else {
+          } 
+          else {
             servo[servoNum].write(servo[servoNum].read() + minMovementAmmount);
           }
         }
-      } else {
+      } 
+      else {
         servo[servoNum].write(servoTargetAngle[servoNum]);
       }
     }
-    //debug("Finished servo movement",0);
   }   
 }
 
+void debugln(String message, int messageDebugLevel) {
+  if (messageDebugLevel <= debugLevel) {
+    if (messageDebugLevel == 1) {
+      Serial.println("  "+message);
+    } 
+    else if (messageDebugLevel == 2) {
+      Serial.println("    "+message);
+    } 
+    else {
+      Serial.println(message);
+    }
+  }
+}
+
+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);
+    }
+  }
+}
+
+void debug(String message, int messageDebugLevel) {
+  if (messageDebugLevel <= debugLevel) {
+    if (messageDebugLevel == 1) {
+      Serial.print("  "+message);
+    } 
+    else if (messageDebugLevel == 2) {
+      Serial.print("    "+message);
+    } 
+    else {
+      Serial.print(message);
+    }
+  }
+}
+
+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);
+    }
+  }
+}