void imeReinitialize(){
print("Starting IMEs....\n\r");
imeShutdown();
imeReset(0);
imeReset(1);
imeInitializeAll();
print("Done!\n\r");
}
void driveBackSlow(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, -40);
motorSet(MOTOR2, -40);
motorSet(MOTOR10, -40);
motorSet(MOTOR9, -40);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, 10); // no inertia
motorSet(MOTOR2, 10);
motorSet(MOTOR10, 10);
motorSet(MOTOR9, 10);
delay(45);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
void scrapeRight(int x) {
int counts = 0;
imeReset(1); // rest left I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 90);
motorSet(MOTOR9, 90);
imeGet(1, &counts); // keep getting the value
}
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, -10);
motorSet(MOTOR9, -10);
delay(55);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
void driveBack(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, 7) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, 7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 7) ;
delay (65);
motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
delay (200);
}
void driveForward(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet(MOTOR_DRIVE_RIGHT_LINE_BACK, MOTOR_MAX);
motorSet(MOTOR_DRIVE_RIGHT_LINE_FRONT, MOTOR_MAX);
motorSet(MOTOR_DRIVE_LEFT_LINE_BACK, MOTOR_MAX);
motorSet(MOTOR_DRIVE_LEFT_LINE_FRONT, MOTOR_MAX);
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, -7) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, -7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, -7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, -7) ;
delay (65);
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, 0) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, 0) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 0) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 0) ;
delay(200);
}
void scrapeRightBack(int x) {
int counts = 0;
imeReset(1); // rest rightLine I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, -90);
motorSet(MOTOR9, -90);
imeGet(1, &counts); // keep getting the value
}
motorSet(MOTOR1, 0); // no intertia
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 10);
motorSet(MOTOR9, 10);
delay(55);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
void driveBack(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, REVERSE_VELOCITY);
motorSet(MOTOR2, REVERSE_VELOCITY);
motorSet(MOTOR10, REVERSE_VELOCITY);
motorSet(MOTOR9, REVERSE_VELOCITY);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, INERTIA_CANCELLATION_FACTOR); // no inertia
motorSet(MOTOR2, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR10, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR9, INERTIA_CANCELLATION_FACTOR);
delay(65);
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void turnRight(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, -64);
motorSet(MOTOR2, -64);
motorSet(MOTOR10, 64);
motorSet(MOTOR9, 64);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, 10); // no inertia
motorSet(MOTOR2, 10);
motorSet(MOTOR10, -10);
motorSet(MOTOR9, -10);
delay(45);
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void driveForward(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 127) ;
motorSet (motor2, 127) ;
motorSet (motor10, 127) ;
motorSet (motor9, 127) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, -7) ; // no inertia
motorSet (motor2, -7) ;
motorSet (motor10, -7) ;
motorSet (motor9, -7) ;
delay (65);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0) ;
motorSet (motor9, 0) ;
delay(200);
}
void driveBack(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, -100) ;
motorSet (motor2, -100) ;
motorSet (motor10, -100) ;
motorSet (motor9, -100) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, 7) ; // no inertia
motorSet (motor2, 7) ;
motorSet (motor10, 7) ;
motorSet (motor9, 7) ;
delay (65);
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void turnLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 64) ;
motorSet (motor2, 64) ;
motorSet (motor10, -64) ;
motorSet (motor9, -64) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, -10) ; // no inertia
motorSet (motor2, -10) ;
motorSet (motor10, 10) ;
motorSet (motor9, 10) ;
delay (45);
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void scrapeLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 90) ;
motorSet (motor2,90) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, -10) ; // no intertia
motorSet (motor2, -10) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (55);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (200);
}
void turnLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, -MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, -MOTOR_TURN_SPEED) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, -10) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, -10) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 10) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 10) ;
delay (45);
motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
delay (200);
}
void driveForward(int x) {
int imeAccumulator = 0;
imeReset(0); // rest rightLine I.M.E
//Read decoder into counts
imeGet(0, &imeAccumulator);
//Move forward at max speed until desired x
while (abs(imeAccumulator) < x) {
motorSet(MOTOR1, FORWARD_VELOCITY);
motorSet(MOTOR2, FORWARD_VELOCITY);
motorSet(MOTOR10, FORWARD_VELOCITY);
motorSet(MOTOR9, FORWARD_VELOCITY);
imeGet(0, &imeAccumulator); // keep getting the value
}
//Cancel forward inertia
motorSet(MOTOR1, -INERTIA_CANCELLATION_FACTOR); // no inertia
motorSet(MOTOR2, -INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR10, -INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR9, -INERTIA_CANCELLATION_FACTOR);
delay(65);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
/*
* Call this from the default Initialize function.
* After, be sure to reinitialize each motor you will be using on your robot.
*/
void riceBotInitialize() {
Motor *MOTDTFrontRight = newMotor();
Motor *MOTDTFrontMidRight = newMotor();
Motor *MOTDTMidRight = newMotor();
Motor *MOTDTBackRight = newMotor();
Motor *MOTDTFrontLeft = newMotor();
Motor *MOTDTFrontMidLeft = newMotor();
Motor *MOTDTMidLeft = newMotor();
Motor *MOTDTBackLeft = newMotor();
Motor *MOTARMFront = newMotor();
Motor *MOTARMRight = newMotor();
Motor *MOTARMLeft = newMotor();
Motor *MOTARMTopRight = newMotor();
Motor *MOTARMBottomRight = newMotor();
Motor *MOTARMTopLeft = newMotor();
Motor *MOTARMBottomLeft = newMotor();
Motor *MOTCOL = newMotor();
printf("%d\n\r", imeInitializeAll());
imeReset(IMEARMLEFT);
imeReset(IMEARMRIGHT);
encDTLeft = encoderInit(0, 0, false);
encDTRight = encoderInit(0, 0, false);
encARMLeft = encoderInit(0, 0, false);
encARMRight = encoderInit(0, 0, false);
Ricencoder *ENCDT = newEncoder();
Ricencoder *ENCARM = newEncoder();
// gyro = gyroInit(0, 196);
// gyroVal = gyroGet(gyro);
analogCalibrate(POTARMLeft);
analogCalibrate(POTARMRight);
analogCalibrate(POTARMFront);
Pid *PidARMLeft = newPid();
Pid *PidARMRight = newPid();
Pid *PidARMFront = newPid();
}
void driveBackSlow(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, -40) ;
motorSet (motor2, -40) ;
motorSet (motor10, -40);
motorSet (motor9, -40) ;
imeGet(0, &counts); // keep getting the value
}
delay (200);
}
void scrapeLeftBack (int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 0) ;
motorSet (motor2,0) ;
motorSet (motor10, 90);
motorSet (motor9, 90) ;
imeGet(0, &counts); // keep getting the value
}
delay (200);
}
void scrapeLeftBack (int x)
{
int counts = 0;
imeReset(0); // rest left ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, -90) ;
motorSet (motor2,-90) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, 10) ; // no intertia
motorSet (motor2, 10) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (55);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (200);
/*motorSet (motor1, 90) ;
motorSet (motor2, 90) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay(x);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
*/
}
void turnRightArc(int x) {
int counts = 1;
imeReset(1); // rest rightLine I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 127);
motorSet(MOTOR9, 127);
imeGet(0, &counts); // keep getting the value
}
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
}
void turnLeftArc(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 127);
motorSet(MOTOR2, 127);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
imeGet(0, &counts); // keep getting the value
}
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void driveForward(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 100) ;
motorSet (motor2, 100) ;
motorSet (motor10, 100) ;
motorSet (motor9, 100) ;
imeGet(0, &counts); // keep getting the value
}
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void turnLeftArc(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 127) ;
motorSet (motor2, 127) ;
motorSet (motor10, 0) ;
motorSet (motor9, 0) ;
imeGet(0, &counts); // keep getting the value
}
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void turnRightArc(int x)
{
int counts = 1;
imeReset(1); // rest rightLine ime
imeGet(1, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 127) ;
motorSet (motor9, 127) ;
imeGet(0, &counts); // keep getting the value
}
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
}
void startSensor(Sensor *s) {
switch (s->type) {
case SHAFT_ENCODER:
encoderReset(s->enc);
if (DEBUG) {
print("reset encoder");
}
break;
case GYROSCOPE:
gyroReset(s->gyr);
break;
case TIME:
//resetTimer(s->port, true);
startTimer(s->port, true);
break;
case IME:
imeReset(s->port);
break;
default:
break;
}
}
void classic15Blue()
{
int armMin = 290;
int wallHeight = 1000;
int goalHeight = 1700;
int dead1 = 1200;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead(8);
//encoder values
int encoder1 = 900; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 325; //back up
int encoder5 = 1350; //back up across the barrier again
int encoder6 = 80; // turn towards center large ball
int encoder7 = 600; // hit 1st ball off the barrier
int encoder8 = 350; // drive back
int encoder9 = 200; // turn towards other large ball
int encoder10 = 400; // hit 2nd ball off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 290; // turn to barf
int encoder13 = 800; // drive to barf + pickup
int encoder14 = 400;
int encoder15 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
// driveforward (some curve)////////////////////////////////////////////////////
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < encoder1)
{
motorSet(MOTOR_DRIVE_RIGHT_BACK, 110); // slight curve cuz friction
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 110);
motorSet(MOTOR_ARM_LEFT_BACK, 127);
motorSet(MOTOR_ARM_LEFT_FRONT, 127);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
///////////////////////////////////////////////////////////////////////////////
//driveForward(encoder1); // drive to goal
armUp(goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead(); //drive slow, adjust to 90 degrees
delay(1000);
outtake(1700); // outtake
driveBack(encoder4); // back up
delay(300);
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
delay(300);
turnLeft(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
delay(500);
driveBack(encoder8); // drive back
delay(300);
turnRight(encoder9); // turn towards other large ball
driveForward(encoder10); // hit 2nd ball off the barrier
delay(500);
driveBack(encoder11); // drive back to square
delay(600);
turnLeft(encoder12); // turn to barf
delay(300);
armDown(armMin);
intakeDead(); // pick up barf
driveForward(encoder13); // drive towards barf + intake it
delay(500);
//end of routine
stopAll();
delay(60000); ///////////////////////////////////////////////////////////////////////////////////
}
void followLine(int dylanSexxydistance) {
int lightDegree = 2000;
int aliSexxySpeed = 40;
int aliSpeed2 = 35;
int leftLine = analogRead(1);
int midLine = analogRead(2);
int rightLine = analogRead(3);
int counts = 0;
imeReset(0); // reset right I.M.E
imeGet(0, &counts);
printf("%d\r\n", leftLine);
printf("%d\r\n", rightLine);
printf("%d\r\n", midLine);
while (abs(counts) < dylanSexxydistance) {
double leftLine = analogRead(1);
double midLine = analogRead(2);
double rightLine = analogRead(3);
//printf("%d\r\n ", leftLine);
//printf("%d\r\n ", rightLine);
//printf("%d\r\n \n ", midLine);
//printf("\r\n ");
//printf("\r\n ");
//printf("\r\n ");
//printf("\r\n ");
//printf("\r\n ");
//if(leftLine>1300 && rightLine>1300 && midLine <1300)
//{
//}
imeGet(0, &counts); // keep getting the value
if (leftLine < 2000 || rightLine < 2000 || midLine < 2000) //(midLine>100) // Mid is black
{
printf("%f \t",
aliSexxySpeed * ((((rightLine) / (leftLine)) + 5) / 10)); //right
printf("%f \n",
aliSexxySpeed * ((((leftLine) / (rightLine)) + 5) / 10)); //left
motorSet(MOTOR1,
aliSexxySpeed * ((((rightLine) / (leftLine)) + 5) / 10));
motorSet(MOTOR2,
aliSexxySpeed * ((((rightLine) / (leftLine)) + 5) / 10));
motorSet(MOTOR9,
aliSexxySpeed * ((((leftLine) / (rightLine)) + 5) / 10));
motorSet(MOTOR10,
aliSexxySpeed * ((((leftLine) / (rightLine)) + 5) / 10));
}
/*else
{
printf("%f\t", aliSexxySpeed*((((rightLine)/(leftLine*1.5))+2)/5));//right
printf("%f\t", aliSexxySpeed*((((leftLine)/(rightLine*1.5))+2)/5));//left
motorSet (motor1, aliSexxySpeed*((((rightLine)/(leftLine*2))+2)/5));
motorSet (motor2, aliSexxySpeed*((((rightLine)/(leftLine*2))+2)/5));
motorSet (motor9, aliSexxySpeed*((((leftLine)/(rightLine*2))+2)/5));
motorSet (motor10, aliSexxySpeed*((((leftLine)/(rightLine*2))+2)/5));
}*/
/*
if (leftLine < 2000) // leftLine white
{
motorSet (motor1, 40);
motorSet (motor2, 40);
motorSet (motor9, 0);
motorSet (motor10, 0);
}
if (rightLine < 2000) // rightLine white
{
motorSet (motor1, 0);
motorSet (motor2, 0);
motorSet (motor9, 40);
motorSet (motor10, 40);
}
if (midLine < 2000)
{
motorSet (motor1, 40);
motorSet (motor2, 40);
motorSet (motor9, 40);
motorSet (motor10, 40);
}
*/
}
stopDrive();
delay(200);
}
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 integratedEncoderResetAll() {
for (int i = 0; i < connectedIntegratedMotorEncoders; i++) {
imeReset(i);
}
}
void integratedEncoderReset(IntegratedEncoder encoder) {
imeReset(encoder.imeAddress);
}
