summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorChristopher Baines <cbaines8@gmail.com>2011-03-29 20:20:39 +0100
committerChristopher Baines <cbaines8@gmail.com>2011-03-29 20:20:39 +0100
commit82a4bd8d21dc456e40627ca326827adbf4d606db (patch)
tree0428bf3da70406648c2500660e185f708f2c9616
parent8c1e4148547569100aa437507efbe74f3a37a85a (diff)
downloadsnakerobot-82a4bd8d21dc456e40627ca326827adbf4d606db.tar
snakerobot-82a4bd8d21dc456e40627ca326827adbf4d606db.tar.gz
Added a servo test routine at the end of the loop functionHEADmaster
Finished modifying the regulator, needs urgent testing! Started writing snakeStraighten and snakeWave functions
-rw-r--r--SnakeMaster/SnakeMaster.pde105
1 files changed, 62 insertions, 43 deletions
diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
index b07d149..f5eec9b 100644
--- a/SnakeMaster/SnakeMaster.pde
+++ b/SnakeMaster/SnakeMaster.pde
@@ -73,6 +73,16 @@ void loop() {
moveServoTo(0,90,0); // Reset servos to zero
moveServoTo(1,90,0);
snakeDelay(3000);*/
+
+ // Snake Servo Test - Move each servo one by one through its full movement
+ for (int servoNum=0; servoNum<numberOfServos; numberOfServos++) {
+ moveServoTo(servoNum, 180, 0);
+ snakeDelay(2000);
+ moveServoTo(servoNum, 0, 0);
+ snakeDelay(2000);
+ moveServoTo(servoNum, 90, 0);
+ snakeDelay(2000);
+ }
}
/*
@@ -182,6 +192,8 @@ void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius,
}
/*
+ - This will begin at the first section and end at the last, this means that it the snake wants
+ to begin coiling at the 7th section and end at the 2nd firstSectionNum=7 lastSectionNum=2
- coilGraduation delay between sections moving means start at the top (in millis)
- reverseDirection false = coil clockwise true = coil anti-clockwise
*/
@@ -195,8 +207,8 @@ void snakeCoil(byte firstSectionNum, byte lastSectionNum, float coilDiameter, fl
snakeArc(firstSectionNum, lastSectionNum, coilDiameter, coilHeight, time);
}
}
- else {
- int timeToMoveSection = time - (abs(coilGraduation) * abs(firstSectionNum-lastSectionNum));
+ else { // TODO This is NOT finished
+ int timeToMoveSection = time - (coilGraduation * abs(firstSectionNum-lastSectionNum));
if (firstSectionNum > lastSectionNum) {
for (int section=lastSectionNum; section>=firstSectionNum; section--) {
moveSectionTo(section, xAngle, int yAngle, int time)
@@ -210,6 +222,11 @@ void snakeCoil(byte firstSectionNum, byte lastSectionNum, float coilDiameter, fl
}
}
+void snakeStraighten(byte firstSectionNum, byte lastSectionNum, int time, int graduation) {
+ int timeToMoveSection = time - (abs(graduation) * abs(firstSectionNum-lastSectionNum));
+
+}
+
// Regulator and Delay Stuff
// This MUST be used instead of delay() or the regulator will not work!
@@ -223,51 +240,52 @@ void regulator() {
for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) {
debug("Regulating ",3);
debugln(servoNum, 3);
- int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
- if (remainingMovement != 0) {
- debug("Remaining movement ",3);
- debugln(remainingMovement,3);
-
- debug("Servo angle ",3);
- debug(servo[servoNum].read(),3);
- debug(" Servo target angle ",3);
- debugln(servoTargetAngle[servoNum],3);
-
- // 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) {
- // 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];
+ if ((servoNextUpdateTime[servoNum] <= millis()) && (servoNextUpdateTime[servoNum] != -1)) {
+ int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
+ if (remainingMovement != 0) {
+ debug("Remaining movement ",3);
+ debugln(remainingMovement,3);
+
+ debug("Servo angle ",3);
+ debug(servo[servoNum].read(),3);
+ debug(" Servo target angle ",3);
+ debugln(servoTargetAngle[servoNum],3);
+
+ // 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) {
+ // 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) + delayAmount;
+ if (remainingMovement < 0) {
+ movement = servo[servoNum].read() - 1 + servoOffset[servoNum];
+ }
+ else {
+ movement = servo[servoNum].read() + 1 + servoOffset[servoNum];
+ }
}
else {
- movement = servo[servoNum].read() + 1 + servoOffset[servoNum];
+ movement = servoTargetAngle[servoNum];
+ servoNextUpdateTime[servoNum] = -1 // basicaly dont update until told otherwise (by the servoMoveTo method setting the value to 0)
}
- }
- else {
- movement = servoTargetAngle[servoNum];
- }
- if (movement > servoSidewaysMovementLimit) {
- servo[servoNum].write(servoSidewaysMovementLimit);
- }
- else if (movement < servoSidewaysMovementLimit) {
- servo[servoNum].write(-servoSidewaysMovementLimit);
- }
- else {
- servo[servoNum].write(movement);
+ if (movement > servoSidewaysMovementLimit) {
+ servo[servoNum].write(servoSidewaysMovementLimit);
+ }
+ else if (movement < servoSidewaysMovementLimit) {
+ servo[servoNum].write(-servoSidewaysMovementLimit);
+ }
+ else {
+ servo[servoNum].write(movement);
+ }
}
-
- servoNextUpdateTime[servoNum] =
- }
- }
+ }
+ }
}
// Debug Stuff
@@ -362,4 +380,5 @@ void debug(float message, int messageDebugLevel) {
-
+
+