void pre_auton()
{
bLCDBacklight = true; // Turn on LCD Backlight
bStopTasksBetweenModes = true;
bDisplayCompetitionStatusOnLcd = false;
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
string mainBattery;
bool getI2C = testI2C();
if(nImmediateBatteryLevel/1000.0>7 && getI2C == true)
{
displayLCDString(0, 0, "Systems: GREEN");
displayLCDString(1, 0, "Ready to Begin!");
}
else if(nImmediateBatteryLevel/1000.0>7 && getI2C == false)
{
displayLCDCenteredString(0, "I2C Fault!!!");
displayLCDCenteredString(1, "Check Wires!!!");
}
else if(getI2C == false && nImmediateBatteryLevel/1000.0<7)
{
displayLCDCenteredString(0, "I2C Fault!!!");
displayLCDCenteredString(1, "Battery Fault!!!");
}
else
{
displayLCDCenteredString(0, "REPLACE BATT!!!");
displayLCDString(1, 0, "Main V: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V');
displayNextLCDString(mainBattery);
}
}
//This function updates the backlight, applies screen strings if they have
//changed, and applies menu hints. Up to now, variables topLCDLine and
//bottomLCDLine have been set to the desired strings. This keeps the LCD from
//being cleared and set excessively (which causes ugly blinking).
void outputLCD()
{
updateBacklight();
if (changedString(topLCDLine))
{
clearLCDLine(0);
displayLCDCenteredString(0,topLCDLine.curr);
}
if (changedString(bottomLCDLine))
{
clearLCDLine(1);
displayLCDCenteredString(1,bottomLCDLine.curr);
}
#ifdef MENU_WRAP //Wrap
if (sysState.curr != AUTONOMOUS && sysError==ERR_NONE)
{
displayLCDString(0,0, "<");
displayLCDString(0,15,">");
}
#else //No wrap
if (sysState.curr != AUTONOMOUS && sysError==ERR_NONE)
{
if (menuItemIndex>0) displayLCDString(0,0, "<"); //If not at the first item, show prev arrow
if (menuItemIndex<(int)M_NO_ITEMS-1) displayLCDString(0,15,">"); //If not at the last item, show next arrow
}
#endif
setLastString(&topLCDLine);
setLastString(&bottomLCDLine);
}
// Main user task
task main()
{
char str[32];
bLCDBacklight = true;
// Start the flywheel control task
startTask( FwControlTask );
// Main user control loop
while(1)
{
// Different speeds set by buttons
if( vexRT[ Btn8L ] == 1 )
FwVelocitySet( &flywheel, 144, 0.55 );
if( vexRT[ Btn8U ] == 1 )
FwVelocitySet( &flywheel, 120, 0.38 );
if( vexRT[ Btn8R ] == 1 )
FwVelocitySet( &flywheel, 50, 0.2 );
if( vexRT[ Btn8D ] == 1 )
FwVelocitySet( &flywheel, 00, 0 );
// Display useful things on the LCD
sprintf( str, "%4d %4d %5.2f", flywheel.target, flywheel.current, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", flywheel.drive, flywheel.drive_at_zero );
displayLCDString(1, 0, str );
// Don't hog the cpu :)
wait1Msec(10);
}
}
void batteryLCD(){//displays battery levels on LCD
if (time1[T1]%100 == 0){
switch(batteryLCDBool){
case true:
clearLCDLine(0);
displayLCDString(0, 0, "Primary: ");
if (nAvgBatteryLevel < 5500){
displayNextLCDString("Replace");
}
else if (nAvgBatteryLevel < 6500){
displayNextLCDString("Low");
}
else{
displayNextLCDString("Good");
}
break;
case false:
ExpanderBatteryLevel = SensorValue[ExpanderBattery] / 7;
clearLCDLine(0);
displayLCDString(0, 0, "Secondary: ");
if (ExpanderBatteryLevel < 550){
displayNextLCDString("Replace");
}
else if (ExpanderBatteryLevel < 650){
displayNextLCDString("Low");
}
else{
displayNextLCDString("Good");
}
break;
}
}
}
void
LcdDisplaySonarStatus( tSensors s1 )
{
string str;
displayLCDString(0, 0, "Rear Sonar ");
sprintf( str, "%4d", SensorValue[ s1 ] );
displayLCDString(1, 0, str);
}
void lcd(){
batteryMain = (nImmediateBatteryLevel/1000.);
if(nLCDButtons ==7){
displayLCDString(1,0,"Let's go bowling");
}
else{
displayLCDString(0,0,batteryMain);
}
}
void
LcdDisplayLineFollowStatus( tSensors s1, tSensors s2, tSensors s3 )
{
string str;
displayLCDString(0, 0, "Line Sensors ");
sprintf( str, "%4d %4d %4d ", SensorValue[ s1 ], SensorValue[ s2 ], SensorValue[ s3 ] );
displayLCDString(1, 0, str);
}
void
LcdDisplayTemperature( tMotor m1, tMotor m2, tMotor m3, tMotor m4 )
{
string str;
sprintf( str, "%7.2f %7.2f", SmartMotorGetTemperature( m1 ), SmartMotorGetTemperature( m2 ));
displayLCDString(0, 0, str);
sprintf( str, "%7.2f %7.2f", SmartMotorGetTemperature( m3 ), SmartMotorGetTemperature( m4 ));
displayLCDString(1, 0, str);
}
/* Display Main and Power Expander Battery Voltages on LCD */
void lcdDisplayVoltage() {
lcdClear();
string s = "Main: ";
sprintf(s,"%dmV",nImmediateBatteryLevel); //Format LCD line 1
displayLCDString(0, 0, s); //Display main battery on LCD
s= "Expander: ";
sprintf(s,"%dmV",SensorValue[Expander]*21.93); //Format LCD line 1
displayLCDString(1, 0, s); //Display power expander battery on LCD
wait10Msec(50);
}
void
LcdDisplaySysStatus( )
{
string str;
sprintf(str,"VBatt %7.2f ", nAvgBatteryLevel/1000.0 );
displayLCDString(0, 0, str);
//sprintf(str,"%s %s", kRobotCVersion, pzFirmwareVersion );
displayLCDString(1, 0, kRobotCVersion);
displayLCDString(1, 6, pzFirmwareVersion);
}
void
LcdDisplayCurrent( tMotor m1, tMotor m2, tMotor m3 = (-1), tMotor m4 = (-1))
{
string str;
sprintf( str, "%7.2f %7.2f", SmartMotorGetCurrent( m1 ), SmartMotorGetCurrent( m2 ));
displayLCDString(0, 0, str);
if(m3!=(-1)) {
sprintf( str, "%7.2f %7.2f", SmartMotorGetCurrent( m3 ), SmartMotorGetCurrent( m4 ));
displayLCDString(1, 0, str);
}
}
void
LcdDisplayMotors( tMotor m1, tMotor m2, tMotor m3 = (-1), tMotor m4 = (-1) )
{
string str;
sprintf( str, "%4d %4d ", motor[ m1 ], motor[ m2 ]);
displayLCDString(0, 0, str);
if(m3!=(-1)) {
sprintf( str, "%4d %4d ", motor[ m3 ], motor[ m4 ]);
displayLCDString(1, 0, str);
}
}
void
LcdDisplayStatus( long enabledTime )
{
string str;
sprintf(str,"VBatt %7.2f ", nAvgBatteryLevel/1000.0 );
displayLCDString(0, 0, str);
if( bIfiAutonomousMode )
sprintf(str, "Autonomous %.2f", (nPgmTime-enabledTime)/1000.0);
else
sprintf(str, "Driver %.2f", (nPgmTime-enabledTime)/1000.0);
displayLCDString(1, 0, str);
}
// Main user task
task main()
{
// Line 211
char str[32];
bLCDBacklight = true;
// Start the flywheel control task
startTask( FwControlTask );
// Main user control loop
//startTask( Collect );
while(1)
{
// Different speeds set by buttons
if( vexRT[ Btn8L ] == 1 )
FwVelocitySet( &flywheel, 2325, 0.7 ); //2500 norm
if( vexRT[ Btn8U ] == 1 )
FwVelocitySet( &flywheel, 120, 0.38 );
if( vexRT[ Btn8R ] == 1 )
FwVelocitySet( &flywheel, 50, 0.2 );
if( vexRT[ Btn8D ] == 1 )
FwVelocitySet( &flywheel, 00, 0 );
/*
if(vexRT[Btn6U] == 1){
motor[CollectionA] = 127;
motor[CollectionB] = 127;
}else if(vexRT[Btn6D] == 1){
motor[CollectionA] = -127;
motor[CollectionB] = -127;
}else{
motor[CollectionA] = 127;
motor[CollectionB] = 127;
}*/
// Display useful things on the LCD
sprintf( str, "%4d %4d %5.2f", flywheel.target, flywheel.current, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", flywheel.drive, flywheel.drive_at_zero );
displayLCDString(1, 0, str );
// Don't hog the cpu :)
wait1Msec(10);
}
}
void
GyroDebug( int displayLine )
{
string str;
if( theGyro.valid )
{
// display current value
sprintf(str,"Gyro %5.1f ", theGyro.angle );
displayLCDString(displayLine, 0, str);
}
else
displayLCDString(displayLine, 0, "Init Gyro.." );
}
/*-----------------------------------------------------------------------------*/
task leftFwControlTask()
{
fw_controller *fw = lFly;
// We are using Speed geared motors
// Set the encoder ticks per revolution
fw->ticks_per_rev = fw->MOTOR_TPR;
while(1)
{
// debug counter
fw->counter++;
// Calculate velocity
getEncoderAndTimeStamp(lFlyBottom,fw->e_current, fw->encoder_timestamp);
FwCalculateSpeed(fw);
// Set current speed for the tbh calculation code
fw->current = fw->v_current;
// Do the velocity TBH calculations
FwControlUpdateVelocityTbh( fw ) ;
// Scale drive into the range the motors need
fw->motor_drive = fw->drive * (FW_MAX_POWER/127);
// Final Limit of motor values - don't really need this
if( fw->motor_drive > 127 ) fw->motor_drive = 127;
if( fw->motor_drive < -127 ) fw->motor_drive = -127;
// and finally set the motor control value
//if(fw->current < fw->target - 20) {
// setRightFwSpeed( 70 );
//} else
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
setLeftFwSpeed(fw->motor_drive);
str = sprintf( str, "%4d %4d %5.2f", fw->target, fw->current, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
str = sprintf( str, "%4.2f %4.2f ", fw->drive, fw->drive_at_zero );
displayLCDString(1, 0, str );
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}
task usercontrol()
{
bLCDBacklight = true;
startTask(speedControl);
while (true)
{
if(vexRT[Btn8U] == 1){
/*
autoDrive(-265, 265, 70, false);
wait1Msec(500);
autoDrive(1100, 1100, 70, true);
wait1Msec(500);
autoDrive(330, -330, 70, false);
wait1Msec(500);
autoDrive(-200, 200, 70, false);
wait1Msec(500);
autoDrive(1100, 1100, 70, true);
wait1Msec(500);
autoDrive(190, -190, 70, false);
*/
autoDrive(-210, 210, 70, false);
wait1Msec(500);
autoDrive(2100, 2120, 70, true);
wait1Msec(500);
autoDrive(260, -260, 70, false);
wait1Msec(500);
}
if(vexRT[Btn8D] == 1){
autoDrive(500, 500, 70, false);
autoDrive(-500, -500, 70, false);
}
//Display main battery
displayLCDString(0, 0, "Btry");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display flywheel controls
clearLCDLine(1);
displayLCDString(1, 0, "F:");
sprintf(mainBattery2, "%d %d %d", target, SensorValue[leftEncoder], SensorValue[rightEncoder]); //Build the value to be displayed
displayNextLCDString(mainBattery2);
}
}
task LCD() {
bLCDBacklight = true; // Turn on LCD Backlight
string mainBattery, backupBattery;
while(true) // An infinite loop to keep the program running until you terminate it
{
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display the Backup battery voltage
//displayLCDString(1, 0, "Backup: ");
//sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V'); //Build the value to be displayed
//displayNextLCDString(backupBattery);
//Sensor values
displayLCDNumber(1, 0, -SensorValue(TensionEncoder));
displayLCDNumber(1, 4, SensorValue(BowEncoder));
//Short delay for the LCD refresh rate
wait1Msec(100);
}
}
/* Pulses all motors and verifies that sensors are connected */
void powerOnSelfTest() {
lcdClear();
displayLCDString(0,0,"~Running POST...");
displayLCDString(0,1,"DO NOT USE CTRL!");
pulseMotor(port1);
pulseMotor(port2);
pulseMotor(port3);
pulseMotor(port4);
pulseMotor(port5);
pulseMotor(port6);
pulseMotor(port7);
pulseMotor(port8);
pulseMotor(port9);
pulseMotor(port10);
boop(); beep(); boop();
//TODO: Check sensors before and after 1s delay and make sure values are the same (this will tell if a sensor is connected or not)
}
task usercontrol()
{
//startTask(accelerate); //controls flywheel acceleration so that the flywheel can accelerate concurrently with drivetrain and intake motors
clearLCDLine(0);
clearLCDLine(1);
bLCDBacklight = true;
char str[32];
bLCDBacklight = true;
// Start the flywheel control task
startTask(flywheelController);
while(true)
{
//drivetrain
motor[leftDrive] = vexRT[Ch3];
motor[rightDrive] = vexRT[Ch2];
sprintf( str, "%4d %4d %5.2f", l_target_velocity, l_motor_velocity, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", l_drive, l_drive_at_zero );
displayLCDString(1, 0, str );
//intake
if(vexRT[Btn6U] == 1)
{
motor[intake1] = 125;
motor[intake2] = 125;
}
else if(vexRT[Btn6D] == 1)
{
motor[intake1] = -125;
motor[intake2] = -125;
}
else
{
motor[intake1] = 0;
motor[intake2] = 0;
}
wait1Msec(10); //so we don't overload the CPU
}
}
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void displayEncoders()
{
//Setup the VEX LCD for displaying encoder values
clearLCDLine(0);
clearLCDLine(1);
displayLCDString(0, 0, "R: ");
displayLCDString(1, 0, "L: ");
//Clear the encoders associated with the left and right motors
nMotorEncoder[RFfly] = 0;
nMotorEncoder[LFfly] = 0;
while(1 == 1)
{
//Display the right and left motor encoder values
displayLCDNumber(0, 3, nMotorEncoder[RFfly], 6);
displayLCDNumber(1, 3, nMotorEncoder[LFfly], 6);
}
}
void batteryVoltagle(int buttonPress) {
if(buttonPress == batteryButton) {
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
if(nImmediateBatteryLevel < 6.2) {
backLightFlash();
}
//Display the Backup battery voltage
displayLCDString(1, 0, "Backup: ");
sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V'); //Build the value to be displayed
displayNextLCDString(backupBattery);
wait1Msec(100);
}
}
task usercontrol()
{
bLCDBacklight = true;
startTask( readRPM );
string debug = "";
while (true)
{
sprintf(debug,"%d",rpm);
displayLCDString(0,0,debug);
}
}
void
LcdDisplayMotorEncoders( tMotor m1, tMotor m2 = (-1), tMotor m3 = (-1), tMotor m4 = (-1) )
{
string str;
if(m2==(-1))
sprintf( str, "%7d ",nMotorEncoder[ m1 ]);
else
sprintf( str, "%7d %7d",nMotorEncoder[ m1 ], nMotorEncoder[ m2 ]);
displayLCDString(0, 0, str);
if(m3!=(-1)) {
if(m4==(-1))
sprintf( str, "%7d ",nMotorEncoder[ m3 ]);
else
sprintf( str, "%7d %7d",nMotorEncoder[ m3 ], nMotorEncoder[ m4 ]);
displayLCDString(1, 0, str);
}
}
task main()
{
char str[32];
// pointer to user parameters
flash_user *u;
bLCDBacklight = true;
while(1)
{
// use LCD as trigger to start code
displayLCDString(0, 0, "Hit button to ");
displayLCDString(1, 0, "continue... ");
WaitForKey();
// Read the user parameters
u = FlashUserRead();
displayLCDString(0, 0, "read done ");
sprintf(str, "param 0 is %d ", u->data[0]);
displayLCDString(1, 0, str);
// wait a while
wait1Msec(4000);
displayLCDString(0, 0, "Hit button to ");
displayLCDString(1, 0, "continue... ");
WaitForKey();
// increase that parameter by 1
if( u->data[0] < 255 )
u->data[0] = u->data[0] + 1;
else
u->data[0] = 1;
// now write parameters
if( FlashUserWrite( u ) )
displayLCDString(0, 0, "write ok ");
else
displayLCDString(0, 0, "write error ");
clearLCDLine(1);
// wait then repeat
wait1Msec(2000);
}
}
void calJoy(){//recalibrates joystick on-the-fly by saving values while joystick is released
halt();
clearLCDLine(1);
displayLCDString(1, 0, "calJoy: Waiting");
delay(2000);
if (abs(vexRT[Ch4]) < 30 && abs(vexRT[Ch3]) < 30){
joy1X = vexRT[Ch4];
joy1Y = vexRT[Ch3];
}
if (abs(vexRT[Ch1]) < 30 && abs(vexRT[Ch2]) < 30){
joy2X = vexRT[Ch1];
joy2Y = vexRT[Ch2];
}
clearLCDLine(1);
}//Function-variable creation end
void displayScreen(const string display){ //Standard Display screen format for LCD
displayLCDString(0,0, display);
displayLCDCenteredString(1,"<Select>");
}
void
LcdAutonomousDisplay( vexLcdMenus menu )
{
// Cleat the lcd
clearLCDLine(0);
clearLCDLine(1);
// Display the selection arrows
displayLCDString(1, 0, l_arr_str);
displayLCDString(1, 13, r_arr_str);
displayLCDString(1, 5, "CHANGE");
// Show the autonomous names
switch( menu ) {
case kMenuAlliance:
if( vAlliance == kAllianceBlue )
displayLCDString(0, 0, "Alliance - BLUE");
else
displayLCDString(0, 0, "Alliance - RED");
break;
case kMenuStartpos:
if( vPosition == kStartHanging )
displayLCDString(0, 0, "Start - Hanging");
else
displayLCDString(0, 0, "Start - Middle");
break;
case kMenuAutonSelect:
switch( vAuton ) {
case 0:
displayLCDString(0, 0, "Default");
break;
case 1:
displayLCDString(0, 0, "Special 1");
break;
default:
char str[20];
sprintf(str,"Undefined %d", vAuton );
displayLCDString(0, 0, str);
break;
}
break;
default:
displayLCDString(0, 0, "Unknown");
break;
}
}
task usercontrol()
{
while (true)
{
float x1 = vexRT[Ch4];
float y1 = vexRT[Ch3];
float x2 = vexRT[Ch1];
float y2 = vexRT[Ch2];
//Left Joystick Arcade Drive//
motor [left] = y1 + x1; //
motor [right] = y2 - x2; //
//////////////////////////////
//Arm Control/////////////////
if (vexRT[Btn6U] == 1) //
{ //
arm(127); //
} //
else if (vexRT[Btn6D] == 1 )//
{ //
arm(-127); //
} //
else //
{ //
arm(0); //
} //
//////////////////////////////
//Grabber Control/////////////
if (vexRT[Btn8U] == 1) //
{ //
grab(127); //
} //
else if (vexRT[Btn8D] == 1) //
{ //
grab(-127); //
} //
else //
{ //
grab(0); //
} //
//////////////////////////////
//LCD Display/////////////////
string mainBattery, PEBattery;
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V');
displayNextLCDString(mainBattery);
//Display the Backup battery voltage
displayLCDString(1, 0, "Count: ");
sprintf(PEBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V');
displayNextLCDString(PEBattery);
//Deadband////////////////////
int deadBand = 10;
if(x1 > deadBand || x1 < -deadBand)
{
x1 = x1;
}
else
{
x1 = 0;
}
if(y1 > deadBand || y1 < -deadBand)
{
y1 = y1;
}
else
{
y1 = 0;
}
if(x2 > deadBand || x2 < -deadBand)
{
x2 = x2;
}
else
{
x2 = 0;
}
if(y2 > deadBand || y2 < -deadBand)
{
y2 = y2;
}
else
{
y2 = 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 ");
}
}
