void robotStatusMenu() {
int screen = 0;
int maxScreen = 6;
int minScreen = 0;
lcdSetBacklight(uart1, true);
while (button != LCD_BTN_CENTER) {
switch (screen) {
case 0:
lcdSetText(uart1, 1, " Main Battery");
lcdPrint(uart1, 2, "%d mV", powerLevelMain());
break;
case 1:
lcdSetText(uart1, 1, " Backup Battery");
lcdPrint(uart1, 2, "%d mV", powerLevelBackup());
break;
case 2:
lcdSetText(uart1, 1, " Joystick");
lcdSetText(uart1, 2, isJoystickConnected(1) ? " Connected" : " Not Connected");
break;
case 3:
lcdSetText(uart1, 1, "Field Controller");
lcdSetText(uart1, 2, isOnline() ? " Connected" : " Not Connected");
break;
case 4:
lcdSetText(uart1, 1, " IMEs Connected");
lcdSetText(uart1, 2, "xxxxxxxxxxxxxxxx");
break;
case 5:
lcdPrint(uart1, 1, "Left Pot %d", analogRead(potLiftLeft));
lcdPrint(uart1, 2, "Left Mtr %d", motorGet(liftLeft));
break;
case 6:
lcdPrint(uart1, 1, "Right Pot %d", analogRead(potLiftRight));
lcdPrint(uart1, 2, "Right Mtr %d", motorGet(liftRight));
break;
}
button = getLcdButtons();
if (button == LCD_BTN_RIGHT) {
if (++screen > maxScreen)
screen = minScreen;
} else if (button == LCD_BTN_LEFT) {
if (--screen < minScreen)
screen = maxScreen;
}
taskDelay(10);
}
}
void lcdBattery(){// code for the battery sub display in lcd
if ((lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD == LCD_BTN_CENTER) || menuStay == LCD_BTN_CENTER){
arbitraryVariable = 1;
menuStay = LCD_BTN_CENTER;
// battery menu!!
if (lcdReadButtons(uart1) == LCD_BTN_LEFT && previousLCD != LCD_BTN_LEFT){// if clicked now, and not clicked 100 ms before
batteryMenuCount--;
}
else if (lcdReadButtons(uart1) == LCD_BTN_RIGHT && previousLCD != LCD_BTN_RIGHT){
batteryMenuCount++;
}
if (batteryMenuCount < 0){
batteryMenuCount = batteryMaxNumberMenus;
}
else if (batteryMenuCount > batteryMaxNumberMenus){
batteryMenuCount = 0;
}
switch(batteryMenuCount){
case 0:
snprintf(battery, 17, "Battery: %f", ((double)powerLevelMain()/1000));
lcdClear(uart1);
lcdSetText(uart1, 1, battery);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){// going back in the menu
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
}
break;
case 1:
snprintf(backupBattery, 17, "%f", ((double)powerLevelBackup()/1000));// prints extra battery for spacing issues on the lcd
lcdSetText(uart1, 1, "Backup Battery:");
lcdSetText(uart1, 2, backupBattery);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){// going back in the menu
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
}
break;
}
}
}
void handleLcdUpdateExceptions()
{
switch(currentPage)
{
case(startLink + 1):
case(startLink + 2):
{
currentPage = startLink + (isJoystickConnected(1) ? 1 : 2);
break;
}
case(startBattery + 2):
{
lcdPrint(uart1, 1, "Main mVolt: Secn");
lcdPrint(uart1, 2, "%u Rtn %d", powerLevelMain(), (int)((analogRead(BATTERY_SECOND_PORT) * 1000)/280.0f));//70.0f * 4.0f
break;
}
case(startBattery + 3):
{
lcdPrint(uart1, 1, "Back mVolt: %u", powerLevelBackup());
lcdPrint(uart1, 2, " Rtn");
break;
}
case(startSensor + 2):
{
/*
* QUADRATURE: Rapid Updating
*
* Reads the data recieved from the Encoder.
* It then is able to handle any exceptions that
* are thrown by the Encoder. It prints the
* index of the Encoder (it's location) followed
* by the data that it is recieving. On the
* second line it prints buttons that are to
* iterate between the Encoders.
*
* *( See handleLcdInput() )*
*
*/
int quad = 0;
switch(quadNum)
{
case(0):
{
quad = encoderGet(encoderBaseLeft);
lcdPrint(uart1, 1, "Quad bL : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(1):
{
quad = encoderGet(encoderBaseRight);
lcdPrint(uart1, 1, "Quad bR : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(2):
{
quad = encoderGet(encoderShooterLeft);
lcdPrint(uart1, 1, "Quad sL : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(3):
{
quad = encoderGet(encoderShooterRight);
lcdPrint(uart1, 1, "Quad sR : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
}
break;
}
case(startSensor + 3):
{
/*
* IME: Rapid Updating
*
* Reads the data from the IME. It then is able to
* handle any exceptions thrown by the IME. It then
* prints the IME's index (location) and the data
* recieved by the IME. On the second line it prints
* the buttons needed to iterate between the IME's.
*
* *(See handleLcdUpdating() )*
*
*/
int ime;
switch(imeNum){
case(0):
{
ime = imeLMem;
ime *= IME_LEFT_MULTIPLIER;
lcdPrint(uart1, 1, "IME bL : %d", ime );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(1):
{
ime = imeRMem;
ime *= IME_RIGHT_MULTIPLIER;
lcdPrint(uart1, 1, "IME bR : %d", ime );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
}
break;
}
case(startSensor + 4):
{
/*
* GYROSCOPE: Rapid Updating
*
* Reads the data recieved from the Gyroscope.
* It then is able to handle any exceptions that
* are thrown by the Gyroscope. It prints "Gyro"
* followed by the data that it is recieving. On the
* second line it prints the button to return back
* to the Sensor Selection.
*
* *( See handleLcdInput() )*
*
*/
int gy = gyroGet(gyroscope) % 360;
lcdPrint(uart1, 1, "Gyroscope : %d", gy);
lcdPrint(uart1, 2, " Rtn");
break;
}
case(startSensor + 5):
{
/*
* Shooting Information: Rapid Updating
*
* Reads the data from the rotor IME's and from the shooter shaft encoder.
* It then processes that information to give all the relavent information
* for shooting on one screen. Does not have room to display the return
* button text. Does not have any action for the left and right buttons.
* The center button returns to the startSensor page.
*/
int iL = imeLMem;
int iR = imeRMem;
iL *= IME_LEFT_MULTIPLIER;
iR *= IME_RIGHT_MULTIPLIER;
float sL = avgLeftShooterSpd * (50 / 6.0f) * 5.0f;//25.0f;
float sR = avgRightShooterSpd * (50 / 6.0f) * 5.0f;//25.0f;
lcdPrint(uart1, 1, "l:%5d, r:%5d", (int)sL, (int)sR);
lcdPrint(uart1, 2, "L%6d R%6d", iL, iR);
break;
}
case(startMotor + 4):
{
int avg = 0;
int counter = 0;
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
avg += motorGet(groups[currentGroupNum].motors[i]);
counter++;
}
if (counter != 0)
{
avg /= counter;
}
if (currentGroupSpeed != avg)
{
currentGroupSpeed = avg;
lcdPrint(uart1, 1, "Grp Avg Spd:%d", currentGroupSpeed);
}
break;
}
case(startMotor + 7):
{
int temp = motorGet(groups[currentGroupNum].motors[currentGroupIndex]);
if (currentGroupSpeed != temp)
{
currentGroupSpeed = temp;
lcdPrint(uart1, 1, "Spd-GrIn-%s:%d",
groups[currentGroupNum].groupName,
currentGroupSpeed);
}
break;
}
case(startMotor + 10):
{
int temp = motorGet(currentMotorNum);
if (currentMotorSpeed != temp)
{
currentMotorSpeed = temp;
lcdPrint(uart1, 1, "Spd-Ind-%d:%d",
currentMotorNum,
currentMotorSpeed);
}
break;
}
case(startAuton + 1):
{
char* temp1 = "15S";
char* temp2 = "1Min";
char* s1 = (void*)"";
char* s2 = (void*)"";
if (autonMode == 0) {
s1 = (void*)temp1;
s2 = (void*)temp2;
}
else {
s1 = (void*)temp2;
s2 = (void*)temp1;
}
lcdPrint(uart1, 1, " AutonMode: %s", s1);
lcdPrint(uart1, 2, "%s Rtn %s", s2, s2);
break;
}
case(startMode + 1):
{
char* temp1 = "comp";
char* temp2 = "rotor";
char* temp3 = "shoot";//max 6 characters
char* s1 = "";
char* s2 = "";
char* s3 = "";
if (opMode == 0)
{
s1 = (void*)temp1;
s2 = (void*)temp2;
s3 = (void*)temp3;
}
else if (opMode == 1)
{
s1 = (void*)temp2;
s2 = (void*)temp3;
s3 = (void*)temp1;
}
else if (opMode == 2)
{
s1 = (void*)temp3;
s2 = (void*)temp1;
s3 = (void*)temp2;
}
lcdPrint(uart1, 1, "OpMode: %s", s1);
lcdPrint(uart1, 2, "%s Rtn %s", s3, s2);
break;
}
case(startFPS + 1):
{
lcdPrint(uart1, 1, "x:%5d, y:%5d", position.x, position.y);
lcdPrint(uart1, 2, "g:%4d, a:%d",
//getLocalAngle(gyroscope), getGlobalAngle(FPSBase.correction, gyroscope));
getGlobalAngle(FPSBase.correction, gyroscope), FPSBase.axis);
break;
}
}
}
void handleLcdScreen()
{
switch(currentPage)
{
case(startLink)://Link Status
{
updateLcdScreen(" Link Status", "< -- >");
break;
}
case(startBattery)://Battery Status
{
updateLcdScreen(" Battery Status", "< -- >");
break;
}
case(startSensor)://Sensor Status
{
updateLcdScreen(" Sensor Status", "< -- >");
break;
}
case(startMotor)://Motor Status
{
updateLcdScreen(" Motor Status", "< -- >");
break;
}
case(startAuton)://Auton Status
{
updateLcdScreen(" Auton Status", "< -- >");
break;
}
case(startMode)://OpMode status
{
updateLcdScreen(" OpMode Status", "< -- >");
break;
}
case(startFPS):
{
updateLcdScreen(" FPS Status", "< -- >");
break;
}
case(startLink + 1)://Link Status - Link Est.
{
updateLcdScreen(" Link Est.", " Rtn");
break;
}
case(startLink + 2)://Link Status - Link Not Est.
{
updateLcdScreen(" Link Not Est.", " Rtn");
break;
}
case(startBattery + 1)://Battery Status - Battery Selector
{
updateLcdScreen("Prims --- Backup", "--- Rtn ---");
break;
}
case(startBattery + 2)://Battery Status - Battery Selector - Prims Battery Status
{
lcdPrint(uart1, 1, "Main mVolt: Secn");
lcdPrint(uart1, 2, "%u Rtn %d", powerLevelMain(), (int)((analogRead(BATTERY_SECOND_PORT) * 1000)/280.0f));//70.0f * 4.0f
break;
}
case(startBattery + 3)://Battery Status - Battery Selector - Backup
{
lcdPrint(uart1, 1, "Back mVolt: %u", powerLevelBackup());
lcdPrint(uart1, 2, " Rtn");
break;
}
case(startSensor + 1):
{
char* name = (void*)"";
switch(sensIndex)
{
case(1):
{
name = (void*)"Quads";
break;
}
case(2):
{
name = (void*)"IMEs";
break;
}
case(3):
{
name = (void*)"Gyro";
break;
}
case(4):
{
name = (void*)"S-R Info";
break;
}
}
lcdPrint(uart1, 1, " Group %s", name);
lcdPrint(uart1, 2, "Last -- Next");
break;
}
case(startSensor + 2):
{
/*
* QUADRATURE: Rapid Updating
*
* Reads the data recieved from the Encoder.
* It then is able to handle any exceptions that
* are thrown by the Encoder. It prints the
* index of the Encoder (it's location) followed
* by the data that it is recieving. On the
* second line it prints buttons that are to
* iterate between the Encoders.
*
* *( See handleLcdInput() )*
*
*/
int quad = 0;
switch(quadNum)
{
case(0):
{
quad = encoderGet(encoderBaseLeft);
lcdPrint(uart1, 1, "Quad bL : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(1):
{
quad = encoderGet(encoderBaseRight);
lcdPrint(uart1, 1, "Quad bR : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(2):
{
quad = encoderGet(encoderShooterLeft);
lcdPrint(uart1, 1, "Quad sL : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(3):
{
quad = encoderGet(encoderShooterRight);
lcdPrint(uart1, 1, "Quad sR : %d", quad );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
}
break;
}
case(startSensor + 3):
{
/*
* IME: Rapid Updating
*
* Reads the data from the IME. It then is able to
* handle any exceptions thrown by the IME. It then
* prints the IME's index (location) and the data
* recieved by the IME. On the second line it prints
* the buttons needed to iterate between the IME's.
*
* *(See handleLcdUpdating() )*
*
*/
int ime;
switch(imeNum){
case(0):
{
ime = imeLMem;
ime *= IME_LEFT_MULTIPLIER;
lcdPrint(uart1, 1, "IME bL : %d", ime );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
case(1):
{
ime = imeRMem;
ime *= IME_RIGHT_MULTIPLIER;
lcdPrint(uart1, 1, "IME bR : %d", ime );
lcdPrint(uart1, 2, "Prev Rtn Next");
break;
}
}
break;
}
case(startSensor + 4):
{
/*
* GYROSCOPE: Rapid Updating
*
* Reads the data recieved from the Gyroscope.
* It then is able to handle any exceptions that
* are thrown by the Gyroscope. It prints "Gyro"
* followed by the data that it is recieving. On the
* second line it prints the button to return back
* to the Sensor Selection.
*
* *( See handleLcdInput() )*
*
*/
int gy = gyroGet(gyroscope) % 360;
lcdPrint(uart1, 1, "Gyroscope : %d", gy);
lcdPrint(uart1, 2, " Rtn");
break;
}
case(startSensor + 5):
{
/*
* Shooting Information: Rapid Updating
*
* Reads the data from the rotor IME's and from the shooter shaft encoder.
* It then processes that information to give all the relavent information
* for shooting on one screen. Does not have room to display the return
* button text. Does not have any action for the left and right buttons.
* The center button returns to the startSensor page.
*/
int iL = imeLMem;
int iR = imeRMem;
iL *= IME_LEFT_MULTIPLIER;
iR *= IME_RIGHT_MULTIPLIER;
float sL = avgLeftShooterSpd * (50 / 6.0f) * 5.0f;//25.0f;
float sR = avgRightShooterSpd * (50 / 6.0f) * 5.0f;//25.0f;
lcdPrint(uart1, 1, "l:%5d, r:%5d", (int)sL, (int)sR);
lcdPrint(uart1, 2, "L%6d R%6d", iL, iR);
break;
}
case(startMotor + 1):
{
updateLcdScreen("Grouping Select", "Group Rtn Indiv");
break;
}
case(startMotor + 2):
{
int lastNum;
int nextNum;
if(currentGroupNum == 0)
{
lastNum = NUM_GROUPS - 1;
nextNum = currentGroupNum + 1;
}
else if(currentGroupNum == NUM_GROUPS - 1)
{
lastNum = currentGroupNum - 1;
nextNum = 0;
}
else
{
lastNum = currentGroupNum - 1;
nextNum = currentGroupNum + 1;
}
lcdPrint(uart1, 1, "%d Grp:%s %d",
lastNum,
groups[currentGroupNum].groupName,
nextNum);
lcdPrint(uart1, 2, "Last -- Next");
break;
}
case(startMotor + 3):
{
switch(groups[currentGroupNum].validMotors)
{
case(1):
{
lcdPrint(uart1, 1, "%s:%d",
groups[currentGroupNum].groupName,
groups[currentGroupNum].motors[0]);
break;
}
case(2):
{
lcdPrint(uart1, 1, "%s:%d,%d",
groups[currentGroupNum].groupName,
groups[currentGroupNum].motors[0],
groups[currentGroupNum].motors[1]);
break;
}
case(3):
{
lcdPrint(uart1, 1, "%s:%d,%d,%d",
groups[currentGroupNum].groupName,
groups[currentGroupNum].motors[0],
groups[currentGroupNum].motors[1],
groups[currentGroupNum].motors[2]);
break;
}
case(4):
{
lcdPrint(uart1, 1, "%s:%d,%d,%d,%d",
groups[currentGroupNum].groupName,
groups[currentGroupNum].motors[0],
groups[currentGroupNum].motors[1],
groups[currentGroupNum].motors[2],
groups[currentGroupNum].motors[3]);
break;
}
case(5):
{
lcdPrint(uart1, 1, "%s:%d,%d,%d,%d,%d",
groups[currentGroupNum].groupName,
groups[currentGroupNum].motors[0],
groups[currentGroupNum].motors[1],
groups[currentGroupNum].motors[2],
groups[currentGroupNum].motors[3],
groups[currentGroupNum].motors[4]);
break;
}
}
lcdPrint(uart1, 2, "Speed Rtn Indiv");
break;
}
case(startMotor + 4):
{
lcdPrint(uart1, 1, "Grp Avg Spd:%d", currentGroupSpeed);
lcdPrint(uart1, 2, "Stop Rtn Max");
break;
}
case(startMotor + 5):
{
lcdPrint(uart1, 1, "Grp St Max:%s",
groups[currentGroupNum].groupName);
lcdPrint(uart1, 2, "-127 Grp-Spd 127");
break;
}
case(startMotor + 6):
{
int lastNum;
int nextNum;
int temp = groups[currentGroupNum].validMotors;
if (currentGroupIndex == 0)
{
lastNum = groups[currentGroupNum].motors[temp - 1];
nextNum = groups[currentGroupNum].motors[currentGroupIndex + 1];
}
else if (currentGroupIndex == temp - 1)
{
lastNum = groups[currentGroupNum].motors[currentGroupIndex - 1];
nextNum = groups[currentGroupNum].motors[0];
}
else
{
lastNum = groups[currentGroupNum].motors[currentGroupIndex - 1];
nextNum = groups[currentGroupNum].motors[currentGroupIndex + 1];
}
lcdPrint(uart1, 1, "%d Grp Mtr: %d %d",
lastNum,
groups[currentGroupNum].motors[currentGroupIndex],
nextNum);
lcdPrint(uart1, 2, "< -- >");
break;
}
case(startMotor + 7):
{
lcdPrint(uart1, 1, "Spd-GrIn-%s:%d",
groups[currentGroupNum].groupName,
currentGroupSpeed);
lcdPrint(uart1, 2, "Stop Rtn-Grp Max");
break;
}
case(startMotor + 8):
{
lcdPrint(uart1, 1, "Grp Ind Max:%d",
groups[currentGroupNum].motors[currentGroupIndex]);
lcdPrint(uart1, 2, "-127 Ind-Spd 127");
break;
}
case(startMotor + 9):
{
int lastNum;
int nextNum;
if (currentMotorNum == 1)
{
lastNum = 10;
nextNum = currentMotorNum + 1;
}
else if (currentMotorNum == 10)
{
lastNum = currentMotorNum - 1;
nextNum = 1;
}
else
{
lastNum = currentMotorNum - 1;
nextNum = currentMotorNum + 1;
}
lcdPrint(uart1, 1, "%d Ind Mtr: %d %d",
lastNum,
currentMotorNum,
nextNum);
lcdPrint(uart1, 2, "< -- >");
break;
}
case(startMotor + 10):
{
lcdPrint(uart1, 1, "Spd-Ind-%d: %d",
currentMotorNum,
currentMotorSpeed);
lcdPrint(uart1, 2, "Stop Rtn Max");
break;
}
case(startMotor + 11):
{
lcdPrint(uart1, 1, "Indiv Max: %d",
currentMotorNum);
lcdPrint(uart1, 2, "-127 Speed 127");
break;
}
case(startAuton + 1):
{
char* temp1 = "15S";
char* temp2 = "1Min";
char* s1 = (void*)"";
char* s2 = (void*)"";
if (autonMode == 0) {
s1 = (void*)temp1;
s2 = (void*)temp2;
}
else {
s1 = (void*)temp2;
s2 = (void*)temp1;
}
lcdPrint(uart1, 1, " AutonMode: %s", s1);
lcdPrint(uart1, 2, "%s Rtn %s", s2, s2);
break;
}
case(startMode + 1):
{
char* temp1 = "comp";
char* temp2 = "rotor";
char* temp3 = "shoot";//max 6 characters
char* s1 = "";
char* s2 = "";
char* s3 = "";
if (opMode == 0)
{
s1 = (void*)temp1;
s2 = (void*)temp2;
s3 = (void*)temp3;
}
else if (opMode == 1)
{
s1 = (void*)temp2;
s2 = (void*)temp3;
s3 = (void*)temp1;
}
else if (opMode == 2)
{
s1 = (void*)temp3;
s2 = (void*)temp1;
s3 = (void*)temp2;
}
lcdPrint(uart1, 1, "OpMode: %s", s1);
lcdPrint(uart1, 2, "%s Rtn %s", s3, s2);
break;
}
case(startFPS + 1):
{
lcdPrint(uart1, 1, "x:%5d, y:%5d", position.x, position.y);
lcdPrint(uart1, 2, "g:%4d, a:%d",
//getLocalAngle(gyroscope), getGlobalAngle(FPSBase.correction, gyroscope));
getGlobalAngle(FPSBase.correction, gyroscope), FPSBase.axis);
break;
}
}
}