From 7a327de4c26026528c6b11715dd2d0097cd9a07a Mon Sep 17 00:00:00 2001 From: Christopher Baines Date: Sat, 26 Mar 2011 20:09:24 +0000 Subject: Hopefully fixed regulator problems and started to inprove the debuging --- SnakeMaster/SnakeMaster.pde | 183 ++++++++++++++++++++++++++++---------------- 1 file 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 - 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 .*/ + 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 .*/ #include #include -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 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); + } + } +} -- cgit v1.2.3