diff --git a/PinkConnection2-Firmware-Servo-HelenMitzi/PinkConnection2-Firmware-Servo-HelenMitzi.ino b/PinkConnection2-Firmware-Servo-HelenMitzi/PinkConnection2-Firmware-Servo-HelenMitzi.ino index 0d5fdf9..1ff08f6 100644 --- a/PinkConnection2-Firmware-Servo-HelenMitzi/PinkConnection2-Firmware-Servo-HelenMitzi.ino +++ b/PinkConnection2-Firmware-Servo-HelenMitzi/PinkConnection2-Firmware-Servo-HelenMitzi.ino @@ -3,9 +3,10 @@ #include #include -Servo servoChannel[19]; -byte onDegrees[19] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; // these are incomplete, i need to tune them when i get the servos in -byte offDegrees[19] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; +Servo servoChannel[11]; +byte onDegrees[19] = { 45, 0, 0, 0, 0, 0, 0, 45, 135, 135, 90, 90 , 90, 90 , 90, 90 , 0, 0, 0 }; // these are incomplete, i need to tune them when i get the servos in +byte offDegrees[19] = { 0, 0, 0, 0, 0, 0, 0, 90, 90 , 90 , 0 , 180, 0 , 180, 0 , 180, 0, 0, 0 }; +byte mapping[19] = { 0, 0, 0, 0, 0, 0, 0, 2 , 2 , 1 , 3 , 6 , 4 , 7 , 5 , 8 , 0, 0, 0 }; int byte1 = 0; int byte2 = 0; @@ -22,7 +23,8 @@ void setup() { servoChannel[5].attach(7); servoChannel[6].attach(8); servoChannel[7].attach(9); - for (int i = 0; i < 19; i++) servoChannel[i].write(offDegrees[i]); + servoChannel[8].attach(10); + for (int i = 0; i < 19; i++) servoChannel[mapping[i]].write(offDegrees[i]); Serial.begin(9600); } @@ -34,70 +36,71 @@ void loop() { byte4 = Serial.read(); byte5 = Serial.read(); - if ((byte1 & 64) && (byte2 & 64)) - if (byte1 & 1) { - servoChannel[0].write(onDegrees[0]); - digitalWrite(A0, 1); - } else { - servoChannel[0].write(offDegrees[0]); - digitalWrite(A0, 0); - } + if ((byte1 & 64) && (byte2 & 64) && (byte3 & 64) && (byte4 & 64) && (byte5 & 64)) + { + if (byte1 & 1) servoChannel[mapping[0]].write(onDegrees[0]); + else servoChannel[mapping[0]].write(offDegrees[0]); - if (byte1 & 2) { - servoChannel[1].write(onDegrees[1]); - digitalWrite(A1, 1); - } else { - servoChannel[1].write(offDegrees[1]); - digitalWrite(A1, 0); - } + //if (byte1 & 2) servoChannel[mapping[1]].write(onDegrees[1]); + //else servoChannel[mapping[1]].write(offDegrees[1]); - if (byte1 & 4) { - servoChannel[2].write(onDegrees[2]); - digitalWrite(A2, 1); - } else { - servoChannel[2].write(offDegrees[2]); - digitalWrite(A2, 0); - } + //if (byte1 & 4) servoChannel[mapping[2]].write(onDegrees[2]); + //else servoChannel[mapping[2]].write(offDegrees[2]); - if (byte1 & 8) { - servoChannel[2].write(onDegrees[3]); - digitalWrite(A3, 1); - } else { - //servoChannel[2].write(offDegrees[3]); dont - digitalWrite(A3, 0); - } + //if (byte1 & 8) servoChannel[mapping[3]].write(onDegrees[3]); + //else servoChannel[mapping[3]].write(offDegrees[3]); - if (byte2 & 1) { - servoChannel[4].write(onDegrees[4]); - digitalWrite(A4, 1); - } else { - servoChannel[4].write(offDegrees[4]); - digitalWrite(A4, 0); - } - if (byte2 & 2) { - servoChannel[4].write(onDegrees[5]); - digitalWrite(A5, 1); - } else { - //servoChannel[4].write(offDegrees[5]); dont - digitalWrite(A5, 0); - } + //if (byte2 & 1) servoChannel[mapping[4]].write(onDegrees[4]); + //else servoChannel[mapping[4]].write(offDegrees[4]); - if (byte2 & 4) { - servoChannel[6].write(onDegrees[6]); - digitalWrite(10, 1); - } else { - servoChannel[6].write(offDegrees[6]); - digitalWrite(10, 0); - } + //if (byte2 & 2) servoChannel[mapping[5]].write(onDegrees[5]); + //else servoChannel[mapping[5]].write(offDegrees[5]); - if (byte2 & 8) { - servoChannel[7].write(onDegrees[7]); - digitalWrite(11, 1); - } else { - servoChannel[7].write(offDegrees[7]); - digitalWrite(11, 0); - } - + //if (byte2 & 4) servoChannel[mapping[6]].write(onDegrees[6]); + //else servoChannel[mapping[6]].write(offDegrees[6]); + + if (byte2 & 8) servoChannel[mapping[7]].write(onDegrees[7]); + else servoChannel[mapping[7]].write(offDegrees[7]); + + + if (byte3 & 1) servoChannel[mapping[8]].write(onDegrees[8]); + //else servoChannel[mapping[8]].write(offDegrees[8]); + + if (byte3 & 2) servoChannel[mapping[9]].write(onDegrees[9]); + else servoChannel[mapping[9]].write(offDegrees[9]); + + if (byte3 & 4) servoChannel[mapping[10]].write(onDegrees[10]); + else servoChannel[mapping[10]].write(offDegrees[10]); + + if (byte3 & 8) servoChannel[mapping[11]].write(onDegrees[11]); + else servoChannel[mapping[11]].write(offDegrees[11]); + + + if (byte4 & 1) servoChannel[mapping[12]].write(onDegrees[12]); + else servoChannel[mapping[12]].write(offDegrees[12]); + + if (byte4 & 2) servoChannel[mapping[13]].write(onDegrees[13]); + else servoChannel[mapping[13]].write(offDegrees[13]); + + if (byte4 & 4) servoChannel[mapping[14]].write(onDegrees[14]); + else servoChannel[mapping[14]].write(offDegrees[14]); + + if (byte4 & 8) servoChannel[mapping[15]].write(onDegrees[15]); + else servoChannel[mapping[15]].write(offDegrees[15]); + + + //if (byte5 & 1) servoChannel[mapping[16]].write(onDegrees[16]); + //else servoChannel[mapping[16]].write(offDegrees[16]); + + //if (byte5 & 2) servoChannel[mapping[17]].write(onDegrees[17]); + //else servoChannel[mapping[17]].write(offDegrees[17]); + + //if (byte5 & 4) servoChannel[mapping[18]].write(onDegrees[18]); + //else servoChannel[mapping[18]].write(offDegrees[18]); + + //if (byte5 & 8) servoChannel[mapping[0]].write(onDegrees[0]); + //else servoChannel[mapping[0]].write(offDegrees[0]); + } } } diff --git a/ServoWorker/ServoWorker.ino b/ServoWorker/ServoWorker.ino new file mode 100644 index 0000000..99cd853 --- /dev/null +++ b/ServoWorker/ServoWorker.ino @@ -0,0 +1,48 @@ +// tp1sm + +#include + +Servo servoChannel[11]; + +void setup() +{ + servoChannel[0].attach(2); + servoChannel[1].attach(3); + servoChannel[2].attach(4); + servoChannel[3].attach(5); + servoChannel[4].attach(6); + servoChannel[5].attach(7); + servoChannel[6].attach(8); + servoChannel[7].attach(9); + servoChannel[8].attach(10); + //servoChannel[10].attach(5); +} + +void loop() +{ + servoChannel[0].write(0); + servoChannel[1].write(90); + servoChannel[2].write(90); + servoChannel[3].write(0); + servoChannel[4].write(0); + servoChannel[5].write(0); + servoChannel[6].write(180); + servoChannel[7].write(180); + servoChannel[8].write(180); + //servoChannel[10].write(160); + delay(500); + servoChannel[2].write(45); + //servoChannel[10].write(120); + delay(500); + servoChannel[0].write(45); + servoChannel[1].write(135); + servoChannel[2].write(135); + servoChannel[3].write(90); + servoChannel[4].write(90); + servoChannel[5].write(90); + servoChannel[6].write(90); + servoChannel[7].write(90); + servoChannel[8].write(90); + //servoChannel[10].write(135); + delay(1000); +}