diff options
author | Christopher Baines <cbaines8@gmail.com> | 2011-03-26 20:09:24 +0000 |
---|---|---|
committer | Christopher Baines <cbaines8@gmail.com> | 2011-03-26 20:09:24 +0000 |
commit | 7a327de4c26026528c6b11715dd2d0097cd9a07a (patch) | |
tree | 5305bf967a698a71899fdb8c048c9634060b6fc6 | |
parent | 7c34801c3908aac90b59a6d92e8e3a8ebb9c7a33 (diff) | |
download | snakerobot-7a327de4c26026528c6b11715dd2d0097cd9a07a.tar snakerobot-7a327de4c26026528c6b11715dd2d0097cd9a07a.tar.gz |
Hopefully fixed regulator problems and started to inprove the debuging
-rw-r--r-- | SnakeMaster/SnakeMaster.pde | 183 |
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); + } + } +} |