Exemple #1
0
void
GyroWarningEliminate()
{
    GyroDebug(0);
    GyroReinit();
    GyroAngleRadGet();
    GyroAngleAbsGet();
    GyroValidGet();
    GyroWarningEliminate();
}
Exemple #2
0
void
LcdDisplayStatus()
{
    string str;
    TControllerButtons    Buttons;

    // Select display Item
    Buttons = nLCDButtons;
    if( vexRT[ Btn7D ] )
        Buttons = kButtonLeft;
    if( vexRT[ Btn7R ] )
        Buttons = kButtonRight;

    if( (Buttons == kButtonLeft) || (Buttons == kButtonRight) )
        {
        if( Buttons == kButtonRight )
           {
            mode++;
            if(mode >= kLcdDispNumber)
                mode = kLcdDispStart;
            }
        if( Buttons == kButtonLeft )
            {
            mode--;
            if(mode < kLcdDispStart)
                mode = (kLcdDispNumber-1);
            }

        clearLCDLine(0);
        clearLCDLine(1);

        switch(mode)
            {
            case    kLcdDispDriveSpeed:
                displayLCDString(0,0, "Speed Drive     ");
                break;
            case    kLcdDispDriveCurrent:
                displayLCDString(0,0, "Current Drive   ");
                break;
            case    kLcdDispDriveTemp:
                displayLCDString(0,0, "Temp Drive      ");
                break;
            case    kLcdDispDriveMotors:
                displayLCDString(0,0, "Motor Drive     ");
                break;
            case    kLcdDispDrivePosition:
                displayLCDString(0,0, "Position Drive  ");
                break;

            case    kLcdDispArmSpeed:
                displayLCDString(0,0, "Speed Arm/Intake");
                break;
            case    kLcdDispArmCurrent:
                displayLCDString(0,0, "Current Arm/Intk");
                break;
            case    kLcdDispArmPosition:
                displayLCDString(0,0, "Position Arm    ");
                break;

            case    kLcdDispSonar:
                displayLCDString(0,0, "Rear Sonar      ");
                break;
            case    kLcdDispLineFollow:
                displayLCDString(0,0, "Line Sensors    ");
                break;
            case    kLcdDispI2CStatus:
                displayLCDString(0,0, "I2C Status      ");
                break;
            case    kLcdDispSysStatus:
                displayLCDString(0,0, "Status          ");
                break;

            case    kLcdDispGyro:
                displayLCDString(0,0, "Gyro            ");
                break;
            case    kLcdDispCalibrate:
                displayLCDString(0,0, "Calibrate intake");
                break;

            default:
                displayLCDString(0,0, "Err             ");
                break;
            }

        do {
            Buttons = nLCDButtons;
            if( vexRT[ Btn7D ] )
                Buttons = kButtonLeft;
            if( vexRT[ Btn7R ] )
                Buttons = kButtonRight;
            wait1Msec(10);
            } while( Buttons != kButtonNone );


        wait1Msec(250);
        }

   switch( mode )
        {
        case    kLcdDispDriveSpeed:
            LcdDisplaySpeeds( MotorLF, MotorRF, MotorLB, MotorRB );
            break;

        case    kLcdDispDriveCurrent:
            LcdDisplayCurrent( MotorLF, MotorRF, MotorLB, MotorRB );
            break;

        case    kLcdDispDriveTemp:
            LcdDisplayTemperature( MotorLF, MotorRF, MotorLB, MotorRB );
            break;

        case    kLcdDispDriveMotors:
            LcdDisplayMotors( MotorLF, MotorRF, MotorLB, MotorRB );
            break;

        case    kLcdDispDrivePosition:
            LcdDisplayMotorEncoders( MotorLF, MotorRF, MotorLB, MotorRB );
            break;


        case    kLcdDispArmSpeed:
            LcdDisplaySpeeds( MotorArmL, MotorArmR, MotorIL, MotorIR );
            break;

        case    kLcdDispArmCurrent:
            LcdDisplayCurrent( MotorArmL, MotorArmR, MotorIL, MotorIR);
            break;

        case    kLcdDispArmPosition:
            LcdDisplayMotorEncoders( MotorArmR );
            break;

        case    kLcdDispSonar:
            LcdDisplaySonarStatus( RearSonar );
            break;

        case    kLcdDispLineFollow:
            LcdDisplayLineFollowStatus( LineFollowC, LineFollowB, LineFollowA );
            break;

        case    kLcdDispI2CStatus:
#if kRobotCVersionNumeric >= 360
            sprintf( str, "RST %2d ERR %d", i2cstat.nTotalAddressResets, i2cstat.bI2CNeverResponded );
            displayLCDString(1, 0, str);
#endif
            break;

        case    kLcdDispSysStatus:
            LcdDisplaySysStatus();
            break;

        case    kLcdDispGyro:
            GyroDebug(0);
            break;

        case    kLcdDispCalibrate:
            displayLCDString(0,0,"Calibrate intake ");
            break;

       default:
            displayLCDString(0, 0, "error" );
            break;
        }

    // Reinit Gyro
    if( mode == kLcdDispGyro && nLCDButtons == kButtonCenter )
        {
        // Wait for release
        while( nLCDButtons != kButtonNone )
            wait1Msec( 25 );
        // Kill and reinit Gyro
        GyroReinit();

        clearLCDLine(0);
        }

   // Calibrate intake system
   if( mode == kLcdDispCalibrate && nLCDButtons == kButtonCenter )
        {
        // Wait for release
        while( nLCDButtons != kButtonNone )
            wait1Msec( 25 );

        // recal intake
        IntakeSystemUnfold();
        displayLCDString(0,0,"Done             ");
        }
}