diff options
| -rw-r--r-- | SnakeMaster/SnakeMaster.pde | 130 |
1 files changed, 87 insertions, 43 deletions
diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde index 5b6c77d..b07d149 100644 --- a/SnakeMaster/SnakeMaster.pde +++ b/SnakeMaster/SnakeMaster.pde @@ -20,13 +20,14 @@ 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; +const int servoSidewaysMovementLimit=85; Servo servo[numberOfServos]; int servoTargetAngle[numberOfServos]; +int servoOffset[numberOfServos]; int servoTargetTime[numberOfServos]; -//int servoLastUpdateTime[numberOfServos]; Currently not used +int servoNextUpdateTime[numberOfServos]; const int servoSpeed = 0.15; // 60 degrees in 0.4 seconds (400 milliseconds) -const int minMovementAmmount = 1; const float sectionLength = 10; // Length of snake section in centimeters void setup() { @@ -84,6 +85,7 @@ void loop() { void moveServoTo(byte servoNum, int angle, int time) { servoTargetAngle[servoNum] = angle; servoTargetTime[servoNum] = time + millis(); + servoNextUpdateTime[servoNum] = 0; // Set update time to 0 to force regulator to pay attention } void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) { @@ -179,16 +181,46 @@ void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius, } } +/* + - coilGraduation delay between sections moving means start at the top (in millis) + - reverseDirection false = coil clockwise true = coil anti-clockwise + */ + +void snakeCoil(byte firstSectionNum, byte lastSectionNum, float coilDiameter, float coilHeight, boolean reverseDirection, int time, int coilGraduation) { + if (coilGraduation == 0) { + if (firstSectionNum > lastSectionNum) { + snakeArc(lastSectionNum, firstSectionNum, coilDiameter, coilHeight, time); + } + else { + snakeArc(firstSectionNum, lastSectionNum, coilDiameter, coilHeight, time); + } + } + else { + int timeToMoveSection = time - (abs(coilGraduation) * abs(firstSectionNum-lastSectionNum)); + if (firstSectionNum > lastSectionNum) { + for (int section=lastSectionNum; section>=firstSectionNum; section--) { + moveSectionTo(section, xAngle, int yAngle, int time) + } + } + else { + for (int section=firstSectionNum; section<=lastSectionNum; section++) { + + } + } + } +} + // Regulator and Delay Stuff // This MUST be used instead of delay() or the regulator will not work! void snakeDelay(int time) { int endTime = (millis() + time); + regulator(); while (millis() < endTime) regulator(); } void regulator() { - for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) { // servoNum<=8 needs correcting + for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) { debug("Regulating ",3); debugln(servoNum, 3); int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read(); @@ -201,30 +233,39 @@ void regulator() { debug(" Servo target angle ",3); debugln(servoTargetAngle[servoNum],3); - int delayAmount = (servoTargetTime[servoNum] - millis()) - (abs(servo[servoNum].read() - servoTargetAngle[servoNum])/ servoSpeed); + // The delay ammount per one degree of movement + int delayAmount = ((servoTargetTime[servoNum] - millis()) - (abs(remainingMovement)/servoSpeed))/abs(remainingMovement); debug("Delay Ammount",3); debugln(delayAmount,3); + + int movement; + if (delayAmount > 0) { - if (abs(remainingMovement) < minMovementAmmount) { - debug("Remaining movement less than min ammount",3); - delay(delayAmount); - servo[servoNum].write(remainingMovement); - } + // Next update time = time for one degree of movement + delay ammount per one degree of movement ((time left - min time needed)/movem + // nts remaining) + servoNextUpdateTime[servoNum] = (1/servoSpeed) + ((servoTargetTime[servoNum] - millis()) - (abs(remainingMovement)/servoSpeed))/abs(remainingMovement); + if (remainingMovement < 0) { + movement = servo[servoNum].read() - 1 + servoOffset[servoNum]; + } else { - 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); - } - else { - servo[servoNum].write(servo[servoNum].read() + minMovementAmmount); - } + movement = servo[servoNum].read() + 1 + servoOffset[servoNum]; } + } + else { + movement = servoTargetAngle[servoNum]; + } + + if (movement > servoSidewaysMovementLimit) { + servo[servoNum].write(servoSidewaysMovementLimit); + } + else if (movement < servoSidewaysMovementLimit) { + servo[servoNum].write(-servoSidewaysMovementLimit); } else { - servo[servoNum].write(servoTargetAngle[servoNum]); + servo[servoNum].write(movement); } + + servoNextUpdateTime[servoNum] = } } } @@ -246,18 +287,18 @@ void debugln(String 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 <= debugLevel) { +/*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) { @@ -288,18 +329,18 @@ void debug(String 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 <= debugLevel) { +/*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) { @@ -319,3 +360,6 @@ void debug(float message, int messageDebugLevel) { + + +
|