Major changes including implementing a regulator to manage speed regulation of the servos

Needs more work to inprove though

1 files changed, 118 insertions, 117 deletions

diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
index 2763852..683816d 100644
--- a/SnakeMaster/SnakeMaster.pde
+++ b/SnakeMaster/SnakeMaster.pde
@@ -12,91 +12,62 @@
     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 <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);
+    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
+
+Servo servo[8];
+int servoTargetAngle[8];
+int servoTargetTime[8];
+int servoLastUpdateTime[8];
+int button = 24;
+
+const int servoSpeed = 0.15; // 60 degrees in 0.4 seconds (400 milliseconds)
+const int minMovementAmmount = 1;
+const int sectionLength = 10; // Length of snake section in centimeters
+
+void setup() {
+  Serial.begin(9600);
+  debug("Setup started",0);
+  //debug("Where in!", 2);
+  // Setup Servos
+  servo[0].attach(2);
+  //debug("Done attaching servos",2);
+  //pinMode(button, INPUT);
+  debug("Finish setup",0);
 }
 
-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();
-    }
+void loop() {
+  debug("----------- Program started ----------------",0);
+  //boolean state = digitalRead(button);
+  //while (state != HIGH) {
+  //  debug("Putton not pressed delaying",0);
+  //  delay(10);
+  //}
+  debug("Button pressed starting routine",0);
+  moveServoTo(0,0,0); // Reset servo to zero
+  debug("Made first movement",0);
+  snakeDelay(2000);
+  debug("Finished first movement",0);
+  moveServoTo(0,180,2000);
+  debug("Made second movement",0);
+  snakeDelay(2000);
+  debug("Finished second movement",0);
+  moveServoTo(0,0,2000);
+  debug("Made third movement",0);
+  snakeDelay(2000);
+  debug("End of loop going round again",0);
 }
 
-int getServoTargetAngle(byte servoNum) {
-    switch (servoNum) {
-    case 1:
-      return servo1TargetAngle;
-    case 2:
-      return servo2TargetAngle;
-    }
+void debug(String message, int messageDebugLevel) {
+  if (messageDebugLevel <= debugLevel) Serial.println(message);
 }
 
-int getServoTargetTime(byte servoNum) {
-    switch (servoNum) {
-    case 1:
-      return servo1TargetTime;
-    case 2:
-      return servo2TargetTime;
-    }
+void debug(int message, int messageDebugLevel) {
+  if (messageDebugLevel <= debugLevel) Serial.println(message);
 }
 
 /*
@@ -105,37 +76,17 @@ int getServoTargetTime(byte servoNum) {
  - 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 moveServoTo(byte servoNum, int angle, int time) {
+  //debug("Setting servo",0);
+  servoTargetAngle[servoNum] = angle;
+  servoTargetTime[servoNum] = time + millis();
 }
 
 void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
-  moveServoTo((sectionNum*2)-1,false,xAngle,time);
-  moveServoTo((sectionNum*2),false,yAngle,time);
+  //debug("Setting section " + sectionNum + " to move to " + xAngle + " x and " + yAngle + " y in " + time " milliseconds");
+  moveServoTo((sectionNum*2)-1,xAngle,time);
+  moveServoTo((sectionNum*2),yAngle,time);
 }
 
 /*
@@ -143,7 +94,7 @@ void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
 
 */
 void snakeArc(byte firstSectionNum, byte lastSectionNum, int xArcRadius, int yArcRadius, int time) {
-  snakeBend(firstSectionNum,lastSectionNum, ((lastSectionNum-firstSectionNum)*sectionLength)/xArcRadius, ((lastSectionNum-firstSectionNum)*sectionLength)/yArcRadius, time)
+  snakeBend(firstSectionNum,lastSectionNum, (((lastSectionNum-firstSectionNum)*sectionLength)/xArcRadius), (((lastSectionNum-firstSectionNum)*sectionLength)/yArcRadius), time);
 }
 
 /*
@@ -155,18 +106,68 @@ void snakeBend(byte firstSectionNum, byte lastSectionNum, int xAngle, int yAngle
   int yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
   
   for (int i=firstSectionNum; i<=lastSectionNum; i++) {
-    moveSectionTo(i,xAnglePerSection,yAnglePerSection, time)
-  }
-  
+    moveSectionTo(i,xAnglePerSection,yAnglePerSection, time);
+  } 
 }
 
-void regulator(byte servoNum) {
-  if (getServoAngle(servoNum) != getServoTargetAngle(servoNum)) {
-    int delayAmount = (getServoTargetTime(servoNum) - millis()) - (abs(angle - startPos)/ servoSpeed);
-      moveServoTo
-      
+/*
+ - 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 {
-    
+    for (int angle=startAngle; angle<endAngle; angle--) {
+      snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
+    }
   }
 }
 
+void snakeDelay(int time) {
+  int endTime = (millis() + time);
+  while (millis() < endTime) regulator();
+}
+
+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);
+    int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
+    debug("Remaining movement",0);
+    debug(remainingMovement,0);
+    if (remainingMovement != 0) {
+      int delayAmount = (servoTargetTime[servoNum] - millis()) - (abs(servo[servoNum].read() - servoTargetAngle[servoNum])/ servoSpeed);
+      debug("Delay Ammount",0);
+      debug(delayAmount,0);
+      if (delayAmount > 0) {
+        if (remainingMovement < minMovementAmmount) {
+          debug("Remaining movement less than min ammount",0);
+          delay(delayAmount);
+          servo[servoNum].write(remainingMovement);
+        } else {
+          debug("Remaining movement greater than min ammount delaying",0);
+          delay(delayAmount/(abs(remainingMovement)/minMovementAmmount));
+          debug(delayAmount/(abs(remainingMovement)/minMovementAmmount),0);
+          if (remainingMovement < 0) {
+            servo[servoNum].write(servo[servoNum].read() - minMovementAmmount);
+          } else {
+            servo[servoNum].write(servo[servoNum].read() + minMovementAmmount);
+          }
+        }
+      } else {
+        servo[servoNum].write(servoTargetAngle[servoNum]);
+      }
+    }
+    //debug("Finished servo movement",0);
+  }   
+}
+