diff options
| -rw-r--r-- | SnakeMaster/SnakeMaster.pde | 14 |
1 files changed, 6 insertions, 8 deletions
diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde index f1dce2c..5b6c77d 100644 --- a/SnakeMaster/SnakeMaster.pde +++ b/SnakeMaster/SnakeMaster.pde @@ -17,7 +17,7 @@ #include <math.h> #include <Servo.h> -const int debugLevel = 2; // 0 = No debug output 1 = main debug output 2 = low level movement debug output +const int debugLevel = 2; // 0 = No debug output 1 = main debug output 2 = low level movement debug output 3 = regulator output const int numberOfServos=2; Servo servo[numberOfServos]; @@ -82,13 +82,11 @@ void loop() { */ 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) { - //debug("Setting section " + sectionNum + " to move to " + xAngle + " x and " + yAngle + " y in " + time " milliseconds"); moveServoTo((sectionNum*2)-2,xAngle,time); moveServoTo((sectionNum*2)-1,yAngle,time); } @@ -153,11 +151,11 @@ void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius, for (int angle=startAngle; angle<endAngle; angle=angle+5) { debug("Angle ",2); debugln(angle,2); - /*debug("Snake bend ",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);*/ + 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); @@ -168,11 +166,11 @@ void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius, 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("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);*/ + 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); @@ -320,4 +318,4 @@ void debug(float message, int messageDebugLevel) { -
+ |