void Servotor32::process(Stream *serial){
if(serial->available()) { //process input from the USB
char inChar = (char)serial->read();
switch(inChar){
case '#':
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
break;
case 'D':
printStatus(serial);
break;
case 'P':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
break;
case '\r':
case '\n':
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
inServo = -1;
inPos = -1;
}
numCount = 0;
break;
case 'V':
serial->println("SERVOTOR32_v2.0_lams");
break;
case 'C':
for(int i=0; i<32; i++){
changeServo(i, 1500);
}
serial->println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,-1);
}
serial->println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
changeServo(inServo, -1);
break;
case 'S': // make an arcPing and send distances back over serial
arcPing();
for(int i = 0; i < ARCPING_STEPS; i++)
{
serial->println(arcPingDistances[i]);
}
break;
default:
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
}
void Servotor32::processChar(char inChar){
if (binary) {
switch(inChar){
case '\r':
case '\n':
binary = false;
changeServo(5,inAllPos[0]);
changeServo(6,inAllPos[1]);
changeServo(7,inAllPos[2]);
changeServo(9,inAllPos[3]);
changeServo(10,inAllPos[4]);
changeServo(11,inAllPos[5]);
changeServo(13,inAllPos[6]);
changeServo(14,inAllPos[7]);
changeServo(15,inAllPos[8]);
changeServo(16,inAllPos[9]);
changeServo(17,inAllPos[10]);
changeServo(18,inAllPos[11]);
changeServo(20,inAllPos[12]);
changeServo(21,inAllPos[13]);
changeServo(22,inAllPos[14]);
changeServo(24,inAllPos[15]);
changeServo(25,inAllPos[16]);
changeServo(26,inAllPos[17]);
changeServo(31,inAllPos[18]);
break;
default:
inAllPos[numCount] = byte(inChar)*10;
numCount++;
break;
}
}
else {
switch(inChar){
case '$':
binary = true;
numCount = 0;
break;
case '#':
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
break;
case 'D':
printStatus();
break;
case 'P':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
break;
case '\r':
case '\n':
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
inServo = -1;
inPos = -1;
}
numCount = 0;
break;
case 'V':
Serial.println("SERVOTOR32_v2.1a");
Serial1.println("SERVOTOR32_v2.1a");
break;
case 'C':
for(int i=0; i<32; i++){
changeServo(i, 1500);
}
Serial.println("All Centered");
Serial1.println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,-1);
}
Serial.println("All Turned Off");
Serial1.println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
changeServo(inServo, -1);
break;
case 'S': // make an arcPing and send distances back over serial
arcPing();
for(int i = 0; i < ARCPING_STEPS; i++)
{
Serial.println(arcPingDistances[i]);
Serial1.println(arcPingDistances[i]);
}
delay_ms(200);
break;
case 'M': // send multiple distance readings back over serial
Serial.println(multiPing(5));
delay_ms(200);
break;
default:
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
}
