From dd82c51c9492c27d3e3c37f20e3147d6f10c99f3 Mon Sep 17 00:00:00 2001 From: Christopher Baines Date: Mon, 28 Mar 2011 15:38:24 +0100 Subject: Major changes, inproved snake draw circle --- SnakeMaster/SnakeMaster.pde | 266 ++++++++++++++++++++++++++++++-------------- 1 file 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; anglestartAngle; 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); + //} } } + + + + + -- cgit v1.2.3