void
GyroWarningEliminate()
{
GyroDebug(0);
GyroReinit();
GyroAngleRadGet();
GyroAngleAbsGet();
GyroValidGet();
GyroWarningEliminate();
}
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 ");
}
}