// measure distances over a 180 degree arc and save them into the array arcPingDistances with size ARCPING_STEPS
void Servotor32::arcPing() {
// min and max pos for servo
int minPos = 500;
int maxPos = 2500;
for(int i=0;i<ARCPING_STEPS; i++)
{
// calculate servopos for current step.
int pos = (int)((float)(maxPos - minPos) / (ARCPING_STEPS-1) * i + minPos);
// set servo to desired pos
changeServo(31,pos);
// if its the first position, give the servo enough time to go there from any previous position
if (i == 0)
{
delay_ms(300);
}
else // if its not the first step, then the position-difference and therefore time needed are smaller.
{
delay_ms(500 / ARCPING_STEPS);
}
// make a multiPing and save result into the array arcPingDistances
arcPingDistances[i] = multiPing(5);
}
// set servo back to middle pos, making head look forward
changeServo(31,1500);
// give the servo some time to reach pos
delay_ms(400);
// shut down servo
changeServo(31,-1);
}
// measure distances over a 180 degree arc and save them into the array arcPingDistances with size ARCPING_STEPS
void Servotor32::arcPing() {
for(int i=0;i<ARCPING_STEPS; i++)
{
int minPos = 500;
int maxPos = 2500;
int pos = (int)((float)(maxPos - minPos) / (ARCPING_STEPS-1) * i + minPos);
changeServo(31,pos);
if (i == 0)
{
delay_ms(300);
}
else
{
delay_ms(500 / ARCPING_STEPS);
}
arcPingDistances[i] = multiPing(5);
}
changeServo(31,1500);
delay_ms(400);
changeServo(31,-1);
}
void Servotor32::processChar(char inChar){
switch(inChar){
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.0");
Serial1.println("SERVOTOR32_v2.0");
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 'Q':
Serial.print("CM: ");
Serial.println(ping());
Serial1.print("CM: ");
Serial1.println(multiPing(10));
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;
}
}
}
