unsigned char getNumConnectedJoysticks() {
unsigned char joysticksConnected = 0;
if (isJoystickConnected(1)) {
joysticksConnected++;
}
if (isJoystickConnected(2)) {
joysticksConnected++;
}
return joysticksConnected;
}
/*
* Runs the user operator control 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 operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
twoJoysticks = isJoystickConnected(2);//check for second joystick
taskResume(leftFlywheel_task);
taskResume(shooter_task);
taskResume(velocity_task);
taskResume(powerListener_task);
taskResume(joystick_task);
taskResume(drive_task);
while (1) {
delay(20);
}
}
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 operator control 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 operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
int leftPower,rightPower;
setTeamName("Team1");
while (1) {
if (isJoystickConnected(CONTROLLER)){
leftPower = joystickGetAnalog(CONTROLLER,3);
rightPower = joystickGetAnalog(CONTROLLER,2);
motorSet(LEFTMOTOR,-leftPower);
motorSet(RIGHTMOTOR,-rightPower);
motorSet(ARM,-30*(joystickGetDigital(CONTROLLER,5,JOY_UP)-joystickGetDigital(CONTROLLER,5,JOY_DOWN)));
motorSet(CLAW,45*(joystickGetDigital(CONTROLLER,6,JOY_UP)-joystickGetDigital(CONTROLLER,6,JOY_DOWN)));
}
}
}
void operatorControl() {
//autonomous();
unsigned long prevWakeupTime = millis();
// PID controller variable
struct pid_dat arm_pid;
initPID(&arm_pid, 2, 0.5, 0.02);
while (1) {
/* Stores which joysticks are connected
* 0 - None
* 1 - Only Joystick 1
* 2 - Only Joystick 2
* 3 - Both Josticks 1 and 2
*/
int joystickStatus = 0;
if (isJoystickConnected(1))
joystickStatus |= 1;
if (isJoystickConnected(2))
joystickStatus |= 2;
// Local Variables
int moveX = 0, moveY = 0, rotate = 0, arm = 0, liftSpeed = 0;
int L, R, C;
bool liftUp = 0, liftDw = 0, supUp = 0, supDw = 0, liftCenter = 0, liftLower = 0;
// Subteam Definition
int team1 = digitalRead(1);
// Get Joystick Values based on Status
if (joystickStatus == 3) {
// Driving
moveX = joystickGetAnalog(1, JOY_LX); // Movement
moveY = joystickGetAnalog(1, JOY_LY); // Movement
rotate = joystickGetAnalog(1, JOY_RX); // Rotate
arm = joystickGetAnalog(2, JOY_RY);
// Support
supUp = joystickGetDigital(1, JOY_LBUM, JOY_DOWN);
supDw = joystickGetDigital(1, JOY_LBUM, JOY_UP);
// Both joysticks connected, reference as 1 or 2
liftUp = joystickGetDigital(1, JOY_RBUM, JOY_DOWN);
liftDw = joystickGetDigital(1, JOY_RBUM, JOY_UP);
liftCenter = joystickGetDigital(2, JOY_RBUM, JOY_UP);
liftLower = joystickGetDigital(2, JOY_RBUM, JOY_DOWN);
} else {
// Driving
moveX = joystickGetAnalog(joystickStatus, JOY_LX); // Movement
moveY = joystickGetAnalog(joystickStatus, JOY_LY); // Movement
rotate = joystickGetAnalog(joystickStatus, JOY_RX); // Rotate
// Support
supUp = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_DOWN);
supDw = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_UP);
// Lift
liftUp = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_DOWN);
liftDw = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_UP);
}
// Driving
L = CAP(moveY + rotate, RANGE_MAX);
R = CAP(moveY - rotate, RANGE_MAX);
C = CAP(moveX, RANGE_MAX);
motorSet(MC_WHEEL_L, team1 == ON ? L : L*2/3);
motorSet(MC_WHEEL_R, team1 == ON ? -R : -R*2/3);
motorSet(MC_WHEEL_M, team1 == ON ? -C : -C*2/3);
// Support
if (supDw)
motorSet(MC_SUPPORT, team1 == ON ? -32 : 32);
else if (supUp)
motorSet(MC_SUPPORT, team1 == ON ? 32 : -32);
else
motorSet(MC_SUPPORT, 0);
if (team1 == ON) {
double loc = ((MAP_INPUT(arm) + 1.0) / 2.0) * 0.9 - 0.8;
if(liftCenter) {
loc = -0.35;
} else if(liftLower) {
loc = -0.45;
}
liftSpeed = -MAP_OUTPUT(computePID(loc, MAP_POT(analogRead(1)), &arm_pid ));
}
if (liftUp)
liftSpeed = LIFTSPEED;
else if (liftDw)
liftSpeed = -LIFTSPEED;
if(team1 != ON) {
liftSpeed = -liftSpeed;
}
// Lift
motorSet(MC_LIFT_BL, liftSpeed);
motorSet(MC_LIFT_ML, liftSpeed);
motorSet(MC_LIFT_TL, liftSpeed);
motorSet(MC_LIFT_BR, -liftSpeed);
motorSet(MC_LIFT_MR, -liftSpeed);
motorSet(MC_LIFT_TR, -liftSpeed);
// Motors can only be updated once every 20ms, therefore updating at twice the rate for better response time
taskDelayUntil(&prevWakeupTime, 10);
}
}
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;
}
}
}
