void FindServoZeroPoints()
{
// not clean but...
int data;
short sSN; // which servo number
boolean fNew = true; // is this a new servo to work with?
boolean fExit = false; // when to exit
int ich;
word wCenter;
// OK lets move all of the servos to their zero point.
InitializeServos();
Serial.println("Find Servo Zeros.\n$-Exit, +- changes, *-change servo");
// Lets move all of the servos to their default location...
for (sSN=0; sSN < MNUMSERVOS; sSN++)
g_aServos[sSN]->SetDutyUS(rcd.aServos[sSN].wCenter);
sSN = 0; // start at our first servo.
while(!fExit) {
if (fNew) {
wCenter = rcd.aServos[sSN].wCenter;
Serial.print("Servo: ");
Serial.print(g_apszServos[sSN]);
Serial.print("(");
Serial.print(wCenter-1500, DEC);
Serial.println(")");
// Now lets wiggle the servo
MoveServo(g_aServos[sSN], wCenter+250, 500);
MoveServo(g_aServos[sSN], wCenter-250, 500);
MoveServo(g_aServos[sSN], wCenter, 500);
fNew = false;
}
//get user entered data
data = Serial.read();
//if data received
if (data !=-1) {
if (data == '$')
fExit = true; // not sure how the keypad will map so give NL, CR, LF... all implies exit
else if ((data == '+') || (data == '-')) {
if (data == '+')
rcd.aServos[sSN].wCenter += 5; // increment by 5us
else
rcd.aServos[sSN].wCenter -= 5; // increment by 5us
Serial.print(" ");
Serial.println(rcd.aServos[sSN].wCenter, DEC);
// Lets try to use attach to change the offsets...
MoveServo(g_aServos[sSN], rcd.aServos[sSN].wCenter, 100);
} else if ((data >= '0') && (data < ('0'+ MNUMSERVOS))) {
// direct enter of which servo to change
fNew = true;
sSN = data - '0';
} else if (data == '*') {
// direct enter of which servo to change
fNew = true;
sSN++;
if (sSN == MNUMSERVOS)
sSN = 0;
}
}
}
Serial.print("Find Servo exit ");
for (sSN=0; sSN < MNUMSERVOS; sSN++){
Serial.print(" ");
Serial.print(g_aServos[sSN]->GetDutyNS()/1000, DEC);
}
Serial.print("\nSave Changes? Y/N: ");
//get user entered data
while (((data = Serial.read()) == -1) || ((data >= 10) && (data <= 15)))
;
if ((data == 'Y') || (data == 'y')) {
// call off to save away the updated data.
rcd.Save();
} else {
g_fServosInit = false; // Lets go back and reinit...
rcd.Load();
}
}
//==============================================================================
// TerminalMonitor - Simple background task checks to see if the user is asking
// us to do anything, like update debug levels ore the like.
//==============================================================================
boolean TerminalMonitor(void)
{
uint8_t szCmdLine[20];
int ich;
int ch;
// See if we need to output a prompt.
if (g_fShowDebugPrompt) {
Serial.println("Rover Debug Monitor");
Serial.println("D - Toggle debug on or off");
#ifdef BBB_SERVO_SUPPORT
Serial.println("O - Enter Servo offset mode");
Serial.println("S <SN> <ANGLE> - Move Servo");
#endif
g_fShowDebugPrompt = false;
}
// First check to see if there is any characters to process.
if (ich = Serial.available()) {
ich = 0;
// For now assume we receive a packet of data from serial monitor, as the user has
// to click the send button...
for (ich=0; ich < sizeof(szCmdLine); ich++) {
ch = Serial.read(); // get the next character
if ((ch == -1) || ((ch >= 10) && (ch <= 15)))
break;
szCmdLine[ich] = ch;
}
szCmdLine[ich] = '\0'; // go ahead and null terminate it...
Serial.print("Serial Cmd Line:");
Serial.write(szCmdLine, ich);
Serial.println("!!!");
// So see what are command is.
if (ich == 0) {
g_fShowDebugPrompt = true;
} else if ((ich == 1) && ((szCmdLine[0] == 'd') || (szCmdLine[0] == 'D'))) {
g_fDebugOutput = !g_fDebugOutput;
if (g_fDebugOutput)
Serial.println("Debug is on");
else
Serial.println("Debug is off");
#ifdef BBB_SERVO_SUPPORT
} else if ((ich == 1) && ((szCmdLine[0] == 'o') || (szCmdLine[0] == 'O'))) {
FindServoZeroPoints();
} else if (((szCmdLine[0] == 's') || (szCmdLine[0] == 'S'))) {
// ok lets grab a servo number
int iServo = 0;
int iAngle = 0;
int i;
for (i=1; i< ich; i++) {
if (szCmdLine[i] != ' ')
break;
}
if ((szCmdLine[i] >= '0') && (szCmdLine[i] <= '9')) {
iServo = szCmdLine[i++] - '0';
// now angle
while (szCmdLine[i] == ' ')
i++;
while ((szCmdLine[i] >= '0') && (szCmdLine[i] <= '9')) {
iAngle = iAngle*10 + szCmdLine[i++] - '0';
}
// Ok lets try moving the servo there...
g_aServos[iServo]->SetDutyUS(iAngle);
}
#endif
}
return true;
}
return false;
}