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);
}
}
/*
* Runs the user autonomous code. This function will be started in its own task with the default
* priority and stack size whenever the robot is enabled via the Field Management System or the
* VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is
* lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart
* the task, not re-start it from where it left off.
*
* Code running in the autonomous task cannot access information from the VEX Joystick. However,
* the autonomous function can be invoked from another task if a VEX Competition Switch is not
* available, and it can access joystick information if called in this way.
*
* The autonomous task may exit, unlike operatorControl() which should never exit. If it does
* so, the robot will await a switch to another mode or disable/enable cycle.
*/
void autonomous() {
//lfilterClear();
int8_t shooterSpeed = shooterSpeedPresets[2];
int8_t n = 0;
while (true) {
if (abs(motorGet(SHOOTER_MOTOR_CHANNEL)) == shooterSpeed && abs(motorGet(SHOOTER_MOTOR_CHANNEL2)) == shooterSpeed) {
if (n < 75) {
++n;
} else {
lifter(60);
takeInInternal(60);
}
}
shooter(shooterSpeed);
delay(20);
}
}
bool inMotion() {
for (int i = 1; i <= 10; i++) {
// printf("DEBUG: %d %d\n\r", i, motorGet(i));
if (motorGet(i) != 0) {
return true;
}
}
return false;
}
/*void lcdAutonRecorder(){
if ((lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD == LCD_BTN_CENTER) || menuStay == LCD_BTN_CENTER){
lcdClear(uart1);
menuStay == LCD_BTN_CENTER;
arbitraryVariable = 1;
lcdSetText(uart1, 1, "CONFIRM");
if (((lcdReadButtons(uart1) == LCD_BTN_RIGHT && previousLCD == LCD_BTN_RIGHT) || endResultMenu == LCD_BTN_RIGHT) && arbitraryValueTwo = 0){
for (int j = 1; j < 4; j ++){
lcdClear(uart1);
lcdSetText(uart1, 1, "Recording in...");
lcdSetText(uart1, 2, j);
}
lcdClear(uart1);
lcdSetText(uart1, 1, "Recording");
record();
lcdClear(uart1);
lcdSetText(uart1, 1, "Finished");
delay(1000);
lcdClear(uart1);
lcdSetText(uart1, 1, "Save?");
lcdSetText(uart1, 2, "No Yes");
if((lcdReadButtons(uart1) == LCD_BTN_RIGHT && previousLCD == LCD_BTN_RIGHT) || endResult_Menu_Inner == LCD_BTN_RIGHT){
endResult_Menu_Inner = LCD_BTN_RIGHT;
arbitraryValueTwo = 1;
if (lcdReadButtons(uart1) == LCD_BTN_LEFT && previousLCD != LCD_BTN_LEFT){// if clicked now, and not clicked 100 ms before
autonRecorderMenuCount--;
}
else if (lcdReadButtons(uart1) == LCD_BTN_RIGHT && previousLCD != LCD_BTN_RIGHT){
autonRecorderMenuCount++;
}
if (autonRecorderMenuCount < 0){
autonRecorderMenuCount = autonRecorderMaxNumberMenus;
}
else if (autonRecorderMenuCount > autonRecorderMaxNumberMenus){
autonRecorderMenuCount = 0;
}
switch(autonRecorderMenuCount){
case 0:
lcdClear(uart1);
lcdSetText(uart1, 1, "Auton Left");
if(lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
saveAuton(Auton_one, "Auton_1");
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
endResult_Menu_Inner = 0;
arbitraryValueTwo = 0;
}
break;
case 1:
lcdClear(uart1);
lcdSetText(uart1, 1, "Auton Right");
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
saveAuton(Auton_two, "Auton_2");
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
endResult_Menu_Inner = 0;
arbitraryValueTwo = 0;
}
break;
}
}
else if((lcdReadButtons(uart1) == LCD_BTN_LEFT && previousLCD == LCD_BTN_LEFT) && arbitraryValueTwo = 0){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
}
else if ((lcdReadButtons(uart1) == LCD_BTN_LEFT && previousLCD == LCD_BTN_LEFT)){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
}
}
*/
void lcdMotor(){// code for the motor sub menu and displays
if ((lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD == LCD_BTN_CENTER) || menuStay == LCD_BTN_CENTER){
lcdClear(uart1);
arbitraryVariable = 1;
menuStay = LCD_BTN_CENTER;
// motor menu!!
if (lcdReadButtons(uart1) == LCD_BTN_LEFT && previousLCD != LCD_BTN_LEFT){// if clicked now, and not clicked 100 ms before
motorMenuCount--;
}
else if (lcdReadButtons(uart1) == LCD_BTN_RIGHT && previousLCD != LCD_BTN_RIGHT){
motorMenuCount++;
}
if (motorMenuCount < 0){
motorMenuCount = motorMaxNumberMenus;
}
else if (motorMenuCount > motorMaxNumberMenus){
motorMenuCount = 0;
}
switch(motorMenuCount){
case 0:
lcdClear(uart1);
lcdSetText(uart1, 1, "Top Left Drive");
snprintf(front_left_driveLCD, 17, "%d", (motorGet(front_left_drive)));
lcdSetText(uart1, 2, front_left_driveLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 1:
lcdClear(uart1);
lcdSetText(uart1, 1, "Top Right Drive");
snprintf(front_right_driveLCD, 17, "%d", (motorGet(front_right_drive)));
lcdSetText(uart1, 2, front_right_driveLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 2:
lcdClear(uart1);
lcdSetText(uart1, 1, "Back Left Drive");
snprintf(back_left_driveLCD, 17, "%d", (motorGet(back_left_drive)));
lcdSetText(uart1, 2, back_left_driveLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 3:
lcdClear(uart1);
lcdSetText(uart1, 1, "Back Right Drive");
snprintf(back_right_driveLCD, 17, "%d", (motorGet(back_right_drive)));
lcdSetText(uart1, 2, back_right_driveLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 4:
lcdClear(uart1);
lcdSetText(uart1, 1, "Lift 1");
snprintf(left_lift_Motor1LCD, 17, "%d", (motorGet(left_lift_Motor1)));
lcdSetText(uart1, 2, left_lift_Motor1LCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 5:
lcdClear(uart1);
lcdSetText(uart1, 1, "Lift 2");
snprintf(left_lift_Motor2LCD, 17, "%d", (motorGet(left_lift_Motor2)));
lcdSetText(uart1, 2, left_lift_Motor2LCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 6:
lcdClear(uart1);
lcdSetText(uart1, 1, "Lift 3");
snprintf(right_lift_Motor3LCD, 17, "%d", (motorGet(right_lift_Motor3)));
lcdSetText(uart1, 2, right_lift_Motor3LCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 7:
lcdClear(uart1);
lcdSetText(uart1, 1, "Lift 4");
snprintf(right_lift_Motor4LCD, 17, "%d", (motorGet(right_lift_Motor4)));
lcdSetText(uart1, 2, right_lift_Motor4LCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 8:
lcdClear(uart1);
lcdSetText(uart1, 1, "Pincer Left");
snprintf(pincer_leftLCD, 17, "%d", (motorGet(pincer_left)));
lcdSetText(uart1, 2, pincer_leftLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 9:
lcdClear(uart1);
lcdSetText(uart1, 1, "Pincer Right");
snprintf(pincer_rightLCD, 17, "%d", (motorGet(pincer_right)));
lcdSetText(uart1, 2, pincer_rightLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
}
}
}
void handleLcdInput(unsigned int input)
{
switch(currentPage)
{
case(startLink)://Link Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startFPS;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startBattery;
break;
}
case(LCD_BTN_CENTER):
{
//check link status
//if connected go to connected
//else go to not connected
if (isJoystickConnected(1))
{
currentPage = startLink + 1;
}
else
{
currentPage = startLink + 2;
}
break;
}
}
break;
}
case(startBattery)://Battery Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startLink;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startSensor;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery + 1;
break;
}
}
break;
}
case(startSensor)://Sensor Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startBattery;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor;
break;
}
case(LCD_BTN_CENTER):
{
quadNum = 0;
imeNum = 0;
sensIndex = 1;
currentPage = startSensor + 1;
break;
}
}
break;
}
case(startMotor)://Motor Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startSensor;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startAuton;
break;
}
case(LCD_BTN_CENTER):
{
currentMotorNum = 1;
currentPage = startMotor + 1;
break;
}
}
break;
}
case(startAuton)://Auton Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startMotor;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMode;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startAuton + 1;
break;
}
}
break;
}
case(startMode)://Op Mode status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startAuton;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startFPS;;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMode + 1;
break;
}
}
break;
}
case(startFPS):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startMode;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startLink;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startFPS + 1;
break;
}
}
break;
}
case(startLink + 1)://Link Status - Link Est.
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
//returns to top of branch
currentPage = startLink;
break;
}
}
break;
}
case(startLink + 2)://Link Status - Link Not Est.
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
//return to top of branch
currentPage = startLink;
break;
}
}
break;
}
case(startBattery + 1)://Battery Status - Battery Selector
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startBattery + 2;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startBattery + 3;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery;
break;
}
}
break;
}
case(startBattery + 2)://Battery Status - Battery Selector - Prims Battery Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery;
break;
}
}
break;
}
case(startBattery + 3)://Battery Status - Battery Selector - Backup
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery;
break;
}
}
break;
}
case(startSensor + 1):
{
/*
* SENSOR SELECTION MENU
*
* Menu for selecting individual types of sensors such
* which can then have individual sensors. This screen
* will display the name of the Sensor Group along with
* the Buttons to select the sensor group and iterate
* between individual sensor groups.
*
*/
switch(input)
{
/*
* LEFT BUTTON: Previous Sensor Group
* CENTER BUTTON: Select Current Sensor Group
* RIGHT BUTTON: Next Sensor Group
*
*/
case(LCD_BTN_LEFT):
{
sensIndex--;
if(sensIndex < 1){
sensIndex = 4;
}
break;
}
case(LCD_BTN_RIGHT):
{
sensIndex++;
if(sensIndex > 4){
sensIndex = 1;
}
break;
}
case(LCD_BTN_CENTER):
{
//the +1 must be added to go beyond this page(startSensor + 1)
currentPage = startSensor + sensIndex + 1;
break;
}
}
break;
}
case(startSensor + 2):
{
/*
* QUADRATURE: Button Control
*
* There can be multiple Quadratures meaning the
* case has functions to iterate between the
* individual Quadratures. The screen will display
* both the data of the current Quadrature and
* the buttons to iterate between the other
* individual Quadratures.
*
*/
switch(input)
{
/*
* LEFT BUTTON: Previous Quadrature
* CENTER BUTTON: Return to Main Page
* RIGHT BUTTON: Next Quadrature
*
*/
case(LCD_BTN_LEFT):
{
quadNum--;
if(quadNum < 0)
quadNum = 3;
break;
}
case(LCD_BTN_RIGHT):
{
quadNum++;
if(quadNum > 3)
quadNum = 0;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startSensor + 3):
{
/*
* IME: Button Control
*
* There can be multiple IME's meaning the IME
* case has functions to iterate between the
* individual IME's. The screen will display
* both the data and the buttons to iterate
* between the individual IME's.
*
*/
switch(input)
{
/*
* LEFT BUTTON: Previous IME
* CENTER BUTTON: Return to Main Page
* RIGHT BUTTON: Next IME
*
*/
case(LCD_BTN_LEFT):
{
imeNum--;
if(imeNum < 0)
imeNum = 1;
break;
}
case(LCD_BTN_RIGHT):
{
imeNum++;
if(imeNum > 1)
imeNum = 0;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startSensor + 4):
{
/*
* GYROSCOPE: Button Control
*
* There will only be one Gyroscope at all times,
* meaning the side buttons will have to do nothing.
* This results in the only usage of the middle
* button to be returning to the main Sensor screen.
*
*/
switch(input)
{
/*
* LEFT BUTTON: No Function
* CENTER BUTTON: Return to aMain Page
* RIGHT BUTTON: No Function
*
*/
case(LCD_BTN_LEFT):
{
break;
}
case(LCD_BTN_RIGHT):
{
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startSensor + 5):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
break;
}
case(LCD_BTN_RIGHT):
{
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startMotor + 1):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentGroupNum = 0;
currentPage = startMotor + 2;
break;
}
case(LCD_BTN_RIGHT):
{
currentMotorNum = 1;
currentPage = startMotor + 9;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor;
break;
}
}
break;
}
case(startMotor + 2):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentGroupNum--;
if(currentGroupNum < 0)
{
currentGroupNum = NUM_GROUPS - 1;
}
//currentPage = startMotor + 2;
//stay on to this page
break;
}
case(LCD_BTN_RIGHT):
{
currentGroupNum++;
if(currentGroupNum == NUM_GROUPS)
{
currentGroupNum = 0;
}
//currentPage = startMotor + 2;
//stay on to this page
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 3;
break;
}
}
break;
}
case(startMotor + 3):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
/*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;
}
currentGroupSpeed = avg;*/
currentPage = startMotor + 4;
break;
}
case(LCD_BTN_RIGHT):
{
currentGroupIndex = 0;
currentPage = startMotor + 6;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 1;
break;
}
}
break;
}
case(startMotor + 4):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
motorStop(groups[currentGroupNum].motors[i]);
}
//currentGroupSpeed = 0;
//currentPage = startMotor + 4;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor + 5;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 3;
break;
}
}
break;
}
case(startMotor + 5):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
motorSet(groups[currentGroupNum].motors[i], -127);
}
//currentGroupSpeed = -127;
currentPage = startMotor + 4;
break;
}
case(LCD_BTN_RIGHT):
{
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
motorSet(groups[currentGroupNum].motors[i], 127);
}
//currentGroupSpeed = 127;
currentPage = startMotor + 4;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 4;
break;
}
}
break;
}
case(startMotor + 6):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentGroupIndex--;
if (currentGroupIndex < 0)
{
currentGroupIndex = groups[currentGroupNum].validMotors - 1;
}
//currentPage = startMotor + 6;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentGroupIndex++;
if (currentGroupIndex == groups[currentGroupNum].validMotors)
{
currentGroupIndex = 0;
}
//currentPage = startMotor + 6;
//stay on this page
break;
}
case(LCD_BTN_CENTER):
{
//currentGroupSpeed = 0;
currentPage = startMotor + 7;
break;
}
}
break;
}
case(startMotor + 7):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
motorStop(groups[currentGroupNum].motors[currentGroupIndex]);
//currentGroupSpeed = 0;
//currentPage = startMotor + 7;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor + 8;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 3;
break;
}
}
break;
}
case(startMotor + 8):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
motorSet(groups[currentGroupNum].motors[currentGroupIndex], -127);
currentPage = startMotor + 7;
break;
}
case(LCD_BTN_RIGHT):
{
motorSet(groups[currentGroupNum].motors[currentGroupIndex], 127);
currentPage = startMotor + 7;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 7;
break;
}
}
break;
}
case(startMotor + 9):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentMotorNum--;
if(currentMotorNum < 1)
{
currentMotorNum = 10;
}
//currentPage = startMotor + 9
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentMotorNum++;
if(currentMotorNum > 10)
{
currentMotorNum = 1;
}
//currentPage = startMotor + 9
//stay on this page
break;
}
case(LCD_BTN_CENTER):
{
currentMotorSpeed = motorGet(currentMotorNum);
currentPage = startMotor + 10;
break;
}
}
break;
}
case(startMotor + 10):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
motorStop(currentMotorNum);
//currentMotorSpeed = 0;
//currentPage = startMotor + 10;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor + 11;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 1;
break;
}
}
break;
}
case(startMotor + 11):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//currentMotorSpeed = -127;
motorSet(currentMotorNum, -127);
currentPage = startMotor + 10;
break;
}
case(LCD_BTN_RIGHT):
{
//currentMotorSpeed = 127;
motorSet(currentMotorNum, 127);
currentPage = startMotor + 10;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 10;
break;
}
}
break;
}
case(startAuton + 1):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
autonMode--;
if(autonMode < 0)
autonMode = 1;
break;
}
case(LCD_BTN_RIGHT):
{
autonMode++;
if(autonMode > 1)
autonMode = 0;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startAuton;
break;
}
}
break;
}
case(startMode + 1):
{
if (input == LCD_BTN_LEFT || input == LCD_BTN_RIGHT) {
if (opMode == 1) {
imeReset(IME_RIGHT_CHAIN_NUMBER);
imeReset(IME_LEFT_CHAIN_NUMBER);
}
else if (opMode == 2) {
setShooter(0);
}
}
switch(input)
{
case(LCD_BTN_LEFT):
{
opMode--;
if (opMode == -1) {
opMode = 2;
}
break;
}
case(LCD_BTN_RIGHT):
{
opMode++;
if (opMode == 3) {
opMode = 0;
}
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMode;
break;
}
}
break;
}
case(startFPS + 1):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
break;
}
case(LCD_BTN_RIGHT):
{
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startFPS;
break;
}
}
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 runNode(void *data) {
MoveNode *currNode = (MoveNode *)data;
MoveNode *currChild = currNode->child;
if (currNode->nodeId == NULL) {
return;
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
Sensor *sensor = &pairMap[currNode->nPairId].sensors[i];
// createSensor(sensor);
/*if (sensor->type == SHAFT_ENCODER) {
sensor->enc = encoderInit(sensor->port, sensor->port + 1, false);
}*/
startSensor(sensor);
sensor = NULL;
}
signed char *saveState = NULL;
printDebug("Started node.");
// printf("Started node %d.\n\r", currNode->nodeId);
// printf("Node's child: %d\n\r", currChild->nodeId);
int nextPoint = 0;
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
while (nextPoint < currNode->numPoints) {
// printf("DEBUG: %d\n\r", nextPoint);
while (joystickGetDigital(1, 5, JOY_UP)) { // pause
if (saveState == NULL) {
saveState = malloc(pairMap[currNode->nPairId].numPorts * sizeof(*(saveState)));
if (saveState == NULL) {
return; // break completely
}
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
saveState[i] = motorGet(pairMap[currNode->nPairId].motorPorts[i]);
motorStop(pairMap[currNode->nPairId].motorPorts[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
pauseTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
printDebug("Paused!");
}
if (joystickGetDigital(1, 8, JOY_DOWN)) { // stop it entirely
printDebug("Stopping!");
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
return;
}
delay(20);
}
if (outOfMemory) {
return;
}
if (saveState != NULL) {
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
motorSet(pairMap[currNode->nPairId].motorPorts[i], saveState[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
}
if (reachedPoint(currNode, currNode->points[nextPoint].endSensorVal, currNode->points[nextPoint - 1].endSensorVal)) {
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
}
if (currChild != NULL) {
// printf("DEBUG: %d %d %d\n\r", currChild->nodeId, nextPoint, currChild->startPoint);
if (nextPoint + 1 >= currChild->startPoint && needToStart(currNode, currChild)) {
void *param = (void *)currChild;
taskCreate(runNode, TASK_DEFAULT_STACK_SIZE / 2, param, TASK_PRIORITY_DEFAULT);
currChild = currChild->sibling;
}
}
delay(5);
}
printDebug("Finished node.");
// printf("Finished node %d.", currNode->nodeId);
if (findParent(currNode)->nodeId == rootNode->nodeId) {
while (inMotion()) {
delay(20);
}
if (currNode->sibling != NULL) {
delay(currNode->sibling->startVal[0]);
runNode(currNode->sibling);
} else {
printDebug("Done.");
delay(500);
}
}
}
