106 lines
3.7 KiB
C++
106 lines
3.7 KiB
C++
// PINKCONNECTION2 Client Program for Helen Henny/Mitzi Mozzarella Servo
|
|
|
|
#include <Servo.h>
|
|
#include <string.h>
|
|
|
|
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 };
|
|
byte offDegrees[19] = { 0, 0, 0, 0, 0, 0, 0, 90, 90 , 90 , 180, 0 , 0 , 180, 180, 0 , 0, 0, 0 };
|
|
byte mapping[19] = { 0, 0, 0, 0, 0, 0, 0, 2 , 2 , 1 , 6 , 3 , 4 , 7 , 8 , 5 , 0, 0, 0 };
|
|
|
|
int byte1 = 0;
|
|
int byte2 = 0;
|
|
int byte3 = 0;
|
|
int byte4 = 0;
|
|
int byte5 = 0;
|
|
|
|
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);
|
|
for (int i = 0; i < 19; i++) servoChannel[mapping[i]].write(offDegrees[i]);
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void loop() {
|
|
if (Serial.available() > 4) {
|
|
byte1 = Serial.read();
|
|
byte2 = Serial.read();
|
|
byte3 = Serial.read();
|
|
byte4 = Serial.read();
|
|
byte5 = Serial.read();
|
|
|
|
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[mapping[1]].write(onDegrees[1]);
|
|
//else servoChannel[mapping[1]].write(offDegrees[1]);
|
|
|
|
//if (byte1 & 4) servoChannel[mapping[2]].write(onDegrees[2]);
|
|
//else servoChannel[mapping[2]].write(offDegrees[2]);
|
|
|
|
//if (byte1 & 8) servoChannel[mapping[3]].write(onDegrees[3]);
|
|
//else servoChannel[mapping[3]].write(offDegrees[3]);
|
|
|
|
|
|
//if (byte2 & 1) servoChannel[mapping[4]].write(onDegrees[4]);
|
|
//else servoChannel[mapping[4]].write(offDegrees[4]);
|
|
|
|
//if (byte2 & 2) servoChannel[mapping[5]].write(onDegrees[5]);
|
|
//else servoChannel[mapping[5]].write(offDegrees[5]);
|
|
|
|
//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]);
|
|
}
|
|
}
|
|
}
|