示例#1
0
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();
    }

}
示例#2
0
//==============================================================================
// 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;
}