Major changes, inproved snake draw circle

1 files changed, 180 insertions, 86 deletions

diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
index e89bef3..f1dce2c 100644
--- a/SnakeMaster/SnakeMaster.pde
+++ b/SnakeMaster/SnakeMaster.pde
@@ -19,7 +19,7 @@
 
 const int debugLevel = 2; // 0 = No debug output  1 = main debug output 2 = low level movement debug output
 
-const int numberOfServos=1;
+const int numberOfServos=2;
 Servo servo[numberOfServos];
 int servoTargetAngle[numberOfServos];
 int servoTargetTime[numberOfServos];
@@ -27,39 +27,51 @@ int servoTargetTime[numberOfServos];
 
 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
+const float sectionLength = 10; // Length of snake section in centimeters
 
 void setup() {
   Serial.begin(9600);
-  debug("Setup started",1);
+  debugln("Setup started",1);
   //debug("Where in!", 2);
   // Setup Servos
   servo[0].attach(2);
+  servo[1].attach(3);
   //debug("Done attaching servos",2);
-  //pinMode(button, INPUT);
-  debug("Finish setup",1);
+  pinMode(31, INPUT);
+  debugln("Finish setup",1);
 }
 
 void loop() {
-  debug("----------- Program started ----------------",1);
-  //boolean state = digitalRead(button);
-  //while (state != HIGH) {
-  //  debug("Putton not pressed delaying",0);
-  //  delay(10);
-  //}
-  debug("Button pressed starting routine",1);
-  moveServoTo(0,0,0); // Reset servo to zero
-  debug("Made first movement",1);
-  snakeDelay(2000);
-  debug("Finished first movement",1);
-  moveServoTo(0,180,2000);
-  debug("Made second movement",1);
-  snakeDelay(2000);
-  debug("Finished second movement",1);
-  moveServoTo(0,0,2000);
-  debug("Made third movement",1);
-  snakeDelay(2000);
-  debug("End of loop going round again",1);
+  debugln("----------- Program started ----------------",1);
+  while (digitalRead(31) != HIGH) {
+    delay(10);
+  }
+  debugln("Button pressed starting routine",1);
+  moveServoTo(0,90,0); // Reset servos to zero
+  moveServoTo(1,90,0);
+  snakeDelay(3000);
+  debugln("Finished reseting servos",1);
+  debugln("Starting circle",1);
+  //snakeBend(1,1, 90,0,0);
+  //snakeDelay(2000);
+  //snakeBend(1 ,1, 0, 90, 0);
+  //snakeDelay(2000);
+  snakeDrawCircle(1,1,8,0,359,true,5000);
+  snakeDelay(5000);
+  debugln("Circle ended",1);
+  /*debugln("Moving both to 180",1);
+   moveServoTo(0,180,0);
+   moveServoTo(1,180,0);
+   snakeDelay(3000);
+   debugln("Finished second movement",1);
+   moveServoTo(0,0,0);
+   moveServoTo(1,0,0);
+   snakeDelay(3000);
+   debugln("Made third movement",1);
+   debugln("End of loop going round again",1);
+   moveServoTo(0,90,0); // Reset servos to zero
+   moveServoTo(1,90,0);
+   snakeDelay(3000);*/
 }
 
 /*
@@ -77,8 +89,8 @@ void moveServoTo(byte servoNum, int angle, int time) {
 
 void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int 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);
+  moveServoTo((sectionNum*2)-2,xAngle,time);
+  moveServoTo((sectionNum*2)-1,yAngle,time);
 }
 
 /*
@@ -94,8 +106,21 @@ void snakeArc(byte firstSectionNum, byte lastSectionNum, int xArcRadius, int yAr
  
  */
 void snakeBend(byte firstSectionNum, byte lastSectionNum, int xAngle, int yAngle, int time) {
-  int xAnglePerSection = xAngle/(lastSectionNum - firstSectionNum);
-  int yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
+  debug("Snake bend ",2);
+  debug(xAngle,2);
+  debug(" x ",2);
+  debug(yAngle,2);
+  debugln(" y",2);
+  int xAnglePerSection;
+  int yAnglePerSection;
+  if (lastSectionNum != firstSectionNum) {
+    xAnglePerSection = xAngle/(lastSectionNum - lastSectionNum);
+    yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
+  } 
+  else {
+    xAnglePerSection = xAngle;
+    yAnglePerSection = yAngle;
+  }
 
   for (int i=firstSectionNum; i<=lastSectionNum; i++) {
     moveSectionTo(i,xAnglePerSection,yAnglePerSection, time);
@@ -106,16 +131,52 @@ 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)));
+void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius, int startAngle, int endAngle, boolean reverseDirection, int time) {
+  float arcAngle;
+  int endTime = millis() + time;
+  if (lastSectionNum != firstSectionNum) {
+    arcAngle = asin(arcRadius/(sectionLength*(lastSectionNum-firstSectionNum)));
+  } 
+  else {
+    debug("Arc Radius ",2);
+    debugln(arcRadius,2);
+    debug("Section Length ",2);
+    debugln(sectionLength,2);
+    float radiusOverLength = (float)arcRadius/(float)sectionLength;
+    debug("One over the other ",2);
+    debugln(radiusOverLength,2);
+    arcAngle = (180.0/3.14)*asin(radiusOverLength);
+  }
+  debug("Arc angle ",2);
+  debugln(arcAngle,2);
   if (!reverseDirection) {
-    for (int angle=startAngle; angle<endAngle; angle++) {
-      snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
+    for (int angle=startAngle; angle<endAngle; angle=angle+5) {
+      debug("Angle ",2);
+      debugln(angle,2);
+      /*debug("Snake bend ",2);
+      debug(cos(angle*(3.14/180.0))*arcAngle,2);
+      debug(" x ",2);
+      debug(sin(angle*(3.14/180.0))*arcAngle,2);
+      debugln(" y",2);*/
+      snakeBend(firstSectionNum, lastSectionNum, 90.0+cos(angle*(3.14/180.0))*arcAngle, 90.0+sin(angle*(3.14/180.0))*arcAngle, time/(endAngle-startAngle));
+      debug("Delay ",2);
+      debugln((endTime-millis())/(endAngle-angle),2);
+      snakeDelay((endTime-millis())/(endAngle-angle));
     }
   } 
   else {
-    for (int angle=startAngle; angle<endAngle; angle--) {
-      snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
+    for (int angle=endAngle; angle>startAngle; angle=angle-5) { // Start Angle and End Angle are reversed in this loop as it is moving in reverse
+      debug("Angle ",2);
+      debugln(angle,2);
+      /*debug("Snake bend ",2);
+      debug(cos(angle*(3.14/180.0))*arcAngle,2);
+      debug(" x ",2);
+      debug(sin(angle*(3.14/180.0))*arcAngle,2);
+      debugln(" y",2);*/
+      snakeBend(firstSectionNum, lastSectionNum, 90.0+cos(angle*(3.14/180.0))*arcAngle, 90.0+sin(angle*(3.14/180.0))*arcAngle, time/(endAngle-startAngle));
+      debug("Delay ",2);
+      debugln((endTime-millis())/(angle-startAngle),2);
+      snakeDelay((endTime-millis())/(angle-startAngle));
     }
   }
 }
@@ -129,31 +190,31 @@ void snakeDelay(int time) {
 }
 
 void regulator() {
-  for (byte servoNum=0; servoNum<=0; servoNum++) { // servoNum<=8 needs correcting
-    debug("Regulating ",2);
-    debugln(servoNum, 2);
+  for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) { // servoNum<=8 needs correcting
+    debug("Regulating ",3);
+    debugln(servoNum, 3);
     int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
     if (remainingMovement != 0) {
-      debug("Remaining movement ",2);
-      debugln(remainingMovement,2);
+      debug("Remaining movement ",3);
+      debugln(remainingMovement,3);
 
-      debug("Servo angle ",2);
-      debug(servo[servoNum].read(),2);
-      debug(" Servo target angle ",2);
-      debugln(servoTargetAngle[servoNum],2);
+      debug("Servo angle ",3);
+      debug(servo[servoNum].read(),3);
+      debug(" Servo target angle ",3);
+      debugln(servoTargetAngle[servoNum],3);
 
       int delayAmount = (servoTargetTime[servoNum] - millis()) - (abs(servo[servoNum].read() - servoTargetAngle[servoNum])/ servoSpeed);
-      debug("Delay Ammount",2);
-      debugln(delayAmount,2);
+      debug("Delay Ammount",3);
+      debugln(delayAmount,3);
       if (delayAmount > 0) {
         if (abs(remainingMovement) < minMovementAmmount) {
-          debug("Remaining movement less than min ammount",2);
+          debug("Remaining movement less than min ammount",3);
           delay(delayAmount);
           servo[servoNum].write(remainingMovement);
         } 
         else {
-          debug("Remaining movement greater than min ammount delaying",2);
-          debugln(delayAmount/(abs(remainingMovement)/minMovementAmmount),2);
+          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);
@@ -174,56 +235,89 @@ void regulator() {
 
 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);
-    }
+    /*if (messageDebugLevel == 1) {
+     Serial.println("  "+message);
+     } 
+     else if (messageDebugLevel == 2) {
+     Serial.println("    "+message);
+     } 
+     else {*/
+    Serial.println(message);
+    //}
   }
 }
 
-void debugln(int 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 == 1) {
+     Serial.println("  "+message);
+     } 
+     else if (messageDebugLevel == 2) {
+     Serial.println("    "+message);
+     } 
+     else {
+    Serial.println(message);
+    //}
+  }
+}*/
+
+void debugln(float 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);
-    }
+    /*if (messageDebugLevel == 1) {
+     Serial.print("  "+message);
+     } 
+     else if (messageDebugLevel == 2) {
+     Serial.print("    "+message);
+     } 
+     else {*/
+    Serial.print(message);
+    //}
   }
 }
 
-void debug(int 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 == 1) {
+     Serial.print("  "+message);
+     } 
+     else if (messageDebugLevel == 2) {
+     Serial.print("    "+message);
+     } 
+     else {
+    Serial.print(message);
+    //}
+  }
+}*/
+
+void debug(float message, int messageDebugLevel) {
+  if (messageDebugLevel <= debugLevel) {
+    /*if (messageDebugLevel == 1) {
+     Serial.print("  "+message);
+     } 
+     else if (messageDebugLevel == 2) {
+     Serial.print("    "+message);
+     } 
+     else {*/
+    Serial.print(message);
+    //}
   }
 }
+
+
+
+
+