summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--SnakeMaster/SnakeMaster.pde266
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);
+ //}
}
}
+
+
+
+
+
generated by cgit v1.2.3 (git 2.45.0) at 2024-11-21 11:56:38 +0000