summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--SnakeMaster/SnakeMaster.pde155
1 files changed, 155 insertions, 0 deletions
diff --git a/SnakeMaster/SnakeMaster.pde b/SnakeMaster/SnakeMaster.pde
new file mode 100644
index 0000000..1150089
--- /dev/null
+++ b/SnakeMaster/SnakeMaster.pde
@@ -0,0 +1,155 @@
+#include <TimedAction.h>
+
+TimedAction servo1Regulator = TimedAction((1/servoSpeed),regulator(1));
+TimedAction servo2Regulator = TimedAction((1/servoSpeed),regulator(2));
+TimedAction servo3Regulator = TimedAction((1/servoSpeed),regulator(3));
+TimedAction servo4Regulator = TimedAction((1/servoSpeed),regulator(4));
+
+/* TODO:
+ - Communication bewteen the other chips,
+ - Finalise the number of servos on this chip and correct the servo code accordingly
+ - Ammend the servo handeling functions to deal with the remote servos
+ - Work out if manual speed regulation of the servos is feasible
+ - Write the comm code for the slaves
+ - Add debug statements to the program using the debug function
+*/
+
+/*
+Snake Sections Snake Servos
+
+Head
+Section 1 Servo 1
+ Servo 2
+Section 2 Servo 3
+ Servo 4
+Section 3 Servo 5 - On Slave Board One
+ Servo 6 - On Slave Board Two
+*/
+
+// Regulator Settings:
+#define minMovementAmmount 1;
+
+#define debug 2; // 0 = No debug output 1 = minimal debug output 2 = maximum debug output
+
+Servo servo1; int servo1TargetAngle; long servo1TargetTime;
+Servo servo2; int servo2TargetAngle; long servo2TargetTime;
+Servo servo3; int servo3TargetAngle; long servo3TargetTime;
+Servo servo4; int servo4TargetAngle; long servo4TargetTime;
+
+#define servoSpeed = 0.15 // 60 degrees in 0.4 seconds (400 milliseconds)
+#define sectionLength 10; // Length of snake section in centimeters
+#define firstServoInSectionInX true // Assuming the first servo in each section is in the horizontal plane
+
+void debug(String message, byte debugLevel) {
+ if (debug >= debugLevel) Serial.println(message);
+}
+
+void moveServoTo(byte servoNum, int angle) {
+ switch (servoNum) {
+ case 1:
+ servo1.write(angle);
+ servo1.
+ break:
+ case 2:
+ servo2.write(angle);
+ break:
+}
+
+int getServoAngle(byte servoNum) {
+ switch (servoNum) {
+ case 1:
+ return servo1.read();
+ case 2:
+ return servo2.read();
+ }
+}
+
+int getServoTargetAngle(byte servoNum) {
+ switch (servoNum) {
+ case 1:
+ return servo1TargetAngle;
+ case 2:
+ return servo2TargetAngle;
+ }
+}
+
+int getServoTargetTime(byte servoNum) {
+ switch (servoNum) {
+ case 1:
+ return servo1TargetTime;
+ case 2:
+ return servo2TargetTime;
+ }
+}
+
+/*
+ - servo The servo to be moved
+ - smoothMovement Use smooth movement?
+ - angle The angle to move the servo to 90 = centre
+ - time The time to spend moving in milliseconds
+*/
+void moveServoTo(byte servoNum, boolean smoothMovement, int angle, int time) {
+ int startPos = servo.read();
+ int moveAngle = abs(angle - startPos);
+ int delayAmount = time - (abs(angle - startPos)/ servoSpeed); // The time the servo must waste if it is to get to the angle on time
+ // WARNING, if this is negitive the servo cant make the movement in time
+
+ if (delayAmount <= 0) { // If cant make the movement just try anyway, else move in increments of 1 degree while taking breaks
+ moveServoTo(servoNum,angle);
+ } else if (smoothMovement) {
+ // TODO
+ } else {
+ int wasteTimePerDegree = moveAngle/delayAmount;
+ if (angle > startPos) {
+ for (int i=0; i<moveAngle; i++) {
+ moveServoTo(servoNum,(startPos + i));
+ delay(1/servoSpeed); // Wait for the time taken to move one degree
+ delay(wasteTimePerDegree);
+ }
+ } else {
+ for (int i=0; i<moveAngle; i++) {
+ moveServoTo(servoNum,(startPos - i));
+ delay(1/servoSpeed); // Wait for the time taken to move one degree
+ delay(wasteTimePerDegree);
+ }
+ }
+ }
+}
+
+void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
+ moveServoTo((sectionNum*2)-1,false,xAngle,time);
+ moveServoTo((sectionNum*2),false,yAngle,time);
+}
+
+/*
+ - xArcRad
+
+*/
+void snakeArc(byte firstSectionNum, byte lastSectionNum, int xArcRadius, int yArcRadius, int time) {
+ snakeBend(firstSectionNum,lastSectionNum, ((lastSectionNum-firstSectionNum)*sectionLength)/xArcRadius, ((lastSectionNum-firstSectionNum)*sectionLength)/yArcRadius, time)
+}
+
+/*
+ - xArcRad
+
+*/
+void snakeBend(byte firstSectionNum, byte lastSectionNum, int xAngle, int yAngle, int time) {
+ int xAnglePerSection = xAngle/(lastSectionNum - firstSectionNum);
+ int yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
+
+ for (int i=firstSectionNum; i<=lastSectionNum; i++) {
+ moveSectionTo(i,xAnglePerSection,yAnglePerSection, time)
+ }
+
+}
+
+void regulator(byte servoNum) {
+ if (getServoAngle(servoNum) != getServoTargetAngle(servoNum)) {
+ int delayAmount = (getServoTargetTime(servoNum) - millis()) - (abs(angle - startPos)/ servoSpeed);
+ moveServoTo
+
+ } else {
+
+ }
+}
+