// Turn to a point based on gyroscopic target
// Test status:
// Tested, works great.
// TODO Add in breakout time parameter, because it is constant at 2 seconds
// right now
void pLoopTurnPointRaw(int angleTarget, double p, double d, int thresh,
int threshCount) {
int error; // error in current position
int lastError = 0; // error from last iteration of the loop
int speed = 0; // speed for the motors to run at
int stopCount = 0; // Amount of time spent within threshold
int iterations = 0;
bprintf(1, "---\r\nang:%d\r\np:%f\r\nd:%f\r\nthresh%d\r\ncount:%d\r\n",
angleTarget, p, d, thresh, threshCount);
while (iterations++ < 40) { // Was 263
error = angleTarget - gyroGet(getGyro());
speed = (error * p) + ((error - lastError) * d);
speed = (abs(speed) > 127) ? (speed < 0) ? -127 : 127 : speed;
speed = (abs(speed) < 30) ? (speed < 0) ? -30 : 30 : speed;
speed = linSpeed(speed); // Linearize speed
driveRaw(-speed, -speed, speed, speed);
if (abs(error) < thresh) {
stopCount++;
if (stopCount >= threshCount)
break;
}
lastError = error;
delay(20);
}
driveRaw(speed, speed, -speed, -speed); // Slam the breaks
delay(10);
driveRaw(0, 0, 0, 0);
}
// Drive an amount of ticks as straight as possible
// Test status:
// completely untested
void pLoopDriveStraightRaw(int tickDiff, bool correctBackwards, bool correctDir,
double pSpeed, double dSpeed, double pCorrect,
int thresh, int threshCount) {
int leftInit = encoderGet(getEncoderBL()); // Initial left value
int rightInit = encoderGet(getEncoderBR()); // Initial right value
int errorL; // Error in the left side
int errorR; // Error in the right side
int error; // Averaged Error
int lastError = 0; // The Error from the Previous Iteration
int speed; // Calculated speed to drive at
int stopCount = 0; // Amount of time spent within threshold
int initGyro = gyroGet(getGyro()); // Initial value of the gyro
int speedModif = 0; // How much to modify the speed for angle
int angleOffset; // How much the robot is curving in its motion
int iterations = 0;
while (iterations++ <= (P_LOOP_DRIVE_BREAK_TIME / 20)) {
errorL = tickDiff - (encoderGet(getEncoderBL()) - leftInit);
errorR = tickDiff - (encoderGet(getEncoderBR()) - rightInit);
error = errorR; // round((errorL + errorR) / 2); // errorL;
speed = error * pSpeed + ((error - lastError) * dSpeed);
speed = (abs(speed) > 127) ? (speed < 0) ? -127 : 127 : speed;
speed = (abs(speed) < 25) ? (speed < 0) ? -25 : 25 : speed;
angleOffset = gyroGet(getGyro()) - initGyro;
speedModif = (correctDir) ? angleOffset * pCorrect : 0;
// Set motors to linearized version of input speeds
driveRaw(linSpeed(speed + speedModif), linSpeed(speed + speedModif),
linSpeed(speed - speedModif), linSpeed(speed - speedModif));
if (abs(error) < thresh) {
stopCount++;
if (stopCount >= threshCount || !correctBackwards)
break;
}
lastError = error;
delay(20);
}
driveRaw(-speed, -speed, -speed, -speed); // Slam the breaks
delay(10);
driveRaw(0, 0, 0, 0);
}
void drive(int8_t vx, int8_t vy, int8_t r, bool isFieldCentric) {
int16_t speed[4]; // one for each wheel
int16_t absRawSpeed, maxRawSpeed;
int8_t i;
if (isFieldCentric) {
float heading = -gyroGet(gyro) % 360 * M_PI / 180;
float v = hypotf(vx, vy);
vx = (int8_t) (v * sinf(heading));
vy = (int8_t) (v * cosf(heading));
}
speed[0] = vy + vx + r; // front left
speed[1] = vy - vx + r; // back left
speed[2] = -vy + vx + r; // front right
speed[3] = -vy - vx + r; // back right
maxRawSpeed = 0;
for (i = 0; i < 4; ++i) {
absRawSpeed = abs(speed[i]);
if (absRawSpeed > maxRawSpeed) {
maxRawSpeed = absRawSpeed;
}
}
if (maxRawSpeed > MAX_SPEED) { // TODO: replace MAXIMUM_SHOOTER_CAP with macro
float scale = (float) maxRawSpeed / MAX_SPEED;
for (i = 0; i < 4; ++i) {
speed[i] /= scale;
}
}
// Linear filtering for gradual acceleration and reduced motor wear
speed[0] = getfSpeed(FRONT_LEFT_MOTOR_CHANNEL, speed[0]);
speed[1] = getfSpeed(BACK_LEFT_MOTOR_CHANNEL, speed[1]);
speed[2] = getfSpeed(FRONT_RIGHT_MOTOR_CHANNEL, speed[2]);
speed[3] = getfSpeed(BACK_RIGHT_MOTOR_CHANNEL, speed[3]);
motorSet(FRONT_LEFT_MOTOR_CHANNEL, speed[0]);
motorSet(BACK_LEFT_MOTOR_CHANNEL, speed[1]);
motorSet(FRONT_RIGHT_MOTOR_CHANNEL, speed[2]);
motorSet(BACK_RIGHT_MOTOR_CHANNEL, speed[3]);
}
short getSensorValue(Sensor s) {
short retVal = -1;
switch (s.type) {
case SHAFT_ENCODER:
if (DEBUG) {
print("got encoder value");
}
retVal = (short)encoderGet(s.enc);
break;
case GYROSCOPE:
retVal = (short)gyroGet(s.gyr);
break;
case ULTRASONIC:
retVal = (short)ultrasonicGet(s.ult);
break;
case ACCELEROMETER:
retVal = (short)analogReadCalibratedHR(s.port);
break;
case BUMPER_SWITCH:
case LIMIT_SWITCH:
retVal = (short)digitalRead(s.port);
break;
case LINE_TRACKER:
case POTENTIOMETER:
retVal = (short)analogRead(s.port);
break;
case TIME:
retVal = (short)getTimer(s.port, true);
break;
case IME: ; // so apparently due to history this ";" has to be here to declare
// a variable inside a case statement?
int imeVal = -1;
imeGet(s.port, &imeVal);
retVal = (short)imeVal;
break;
}
return retVal;
}
void startIOTask(void *ignore) {
while(1) {
setDriveTrainMotors(driveTrainStyle);
motorSet(ARMLeft.port, ARMLeft.out * ARMLeft.reflected);
motorSet(ARMRight.port, ARMRight.out * ARMRight.reflected);
motorSet(ARMTopLeft.port, ARMTopLeft.out * ARMTopLeft.reflected);
motorSet(ARMTopRight.port, ARMTopRight.out * ARMTopRight.reflected);
motorSet(ARMBottomLeft.port, ARMBottomLeft.out * ARMBottomLeft.reflected);
motorSet(ARMBottomRight.port, ARMBottomRight.out * ARMBottomRight.reflected);
if(useIMEs) {
imeGet(IMELEFT, &encVals[0]);
imeGet(IMERIGHT, &encVals[1]);
} else {
encVals[0] = encoderGet(encLeft);
encVals[1] = encoderGet(encRight);
}
gyroVal = gyroGet(gyro);
delay(10);
}
}
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;
}
}
}
void GyroSensor::pollSensor()
{
if(sensorport != 13)
curval = gyroGet(gyro);
}
