void TeleopPeriodic() {
if(tick==10) if (ds->IsSysBrownedOut()) {
ds->ReportError("[ERROR] BROWNOUT DETECTED!!");
}
if(tick == 15) if (!ds->IsNewControlData()) {
ds->ReportError(
"[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!");
}
if(tick==20) if (!ds->IsDSAttached()) {
ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!");
}
if (stick.GetRawButton(10))
zeroSanics();
if (stick.GetRawButton(8)) {
leftIRZero = 0;
rightIRZero = 0;
}
tick++;
if (liftStick.GetRawButton(2)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(canScale);
} else if (liftStick.GetRawButton(3)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(-canScale);
} else
canGrabber.SetSpeed(0);
double speed;
//Calculate scalar to use for POV/Adjusted drive
double scale = stick.GetRawAxis(3);
scale += 1;
scale = 2 - scale;
scale /= 2;
//Use pov/hat switch for movement if enabled
if (stick.GetRawButton(1) && stick.GetRawButton(2)) {
AutomaticLineup();
} else if (stick.GetRawButton(1)) {
double leftVolts = leftIR.GetAverageVoltage() - leftIRZero;
double rightVolts = rightIR.GetAverageVoltage() - leftIRZero;
if (rightVolts + VOLTAGE_TOLERANCE > leftVolts
&& rightVolts - VOLTAGE_TOLERANCE < leftVolts) {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
} else if (rightVolts > leftVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, 0.2);
else if (leftVolts > rightVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, -0.2);
} else if (stick.GetRawButton(6)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, scale);
} else if (stick.GetRawButton(5)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, -scale);
} else if (stick.GetPOV(0) != -1) {
//If POV moved, move polar (getPOV returns an angle in degrees)
robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0);
} else if (stick.GetRawButton(2)) {
//Drive with scalar
robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale,
stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale);
} else {
//Drive normally
robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(),
stick.GetZ());
}
speed = -liftStick.GetY();
//bool canGoUp = maxUp.Get();
bool canGoUp = true;
//bool canGoDown = maxDown.Get();
bool canGoDown = true;
//If at a limit switch and moving in that direction, stop
if (speed > 0 && !canGoUp)
speed = 0;
if (speed < 0 && !canGoDown)
speed = 0;
chainLift.SetSpeed(speed);
if (tick >50) {
if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) {
//Smart Dash outputs
//SmartDashboard::PutNumber("X Acceleration: ", accel.GetX());
//SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY());
//SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ());
SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get());
SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get());
SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get());
SmartDashboard::PutBoolean("Auto switch A: ",
autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ",
autoSwitch2.Get());
//SmartDashboard::PutBoolean("RobotDrive Alive?",
// robotDrive.IsAlive());
//SmartDashboard::PutBoolean("ChainLift Alive?",
// robotDrive.IsAlive());
SmartDashboard::PutNumber("Left Sensor",
leftIR.GetAverageVoltage());
SmartDashboard::PutNumber("Right Sensor",
rightIR.GetAverageVoltage());
SmartDashboard::PutNumber("Left w zero",
leftIR.GetAverageVoltage() - leftIRZero);
SmartDashboard::PutNumber("Rigt w zero",
rightIR.GetAverageVoltage() - rightIRZero);
SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14));
SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15));
}
tick = 0;
}
}
void RhsRobotBase::StartCompetition() //Robot's main function
{
DriverStation *pDS = DriverStation::GetInstance();
Init(); //Initialize the robot
while(true)
{
if(!pDS->IsNewControlData())
{
Wait(0.002);
continue;
}
//Checks the current state of the robot
if(IsDisabled())
{
currentRobotState = ROBOT_STATE_DISABLED;
}
else if(IsEnabled() && IsAutonomous())
{
currentRobotState = ROBOT_STATE_AUTONOMOUS;
}
else if(IsEnabled() && IsOperatorControl())
{
currentRobotState = ROBOT_STATE_TELEOPERATED;
}
else if(IsEnabled() && IsTest())
{
currentRobotState = ROBOT_STATE_TEST;
}
else
{
currentRobotState = ROBOT_STATE_UNKNOWN;
}
if(HasStateChanged()) //Checks for state changes
{
switch(GetCurrentRobotState())
{
case ROBOT_STATE_DISABLED:
printf("ROBOT_STATE_DISABLED\n");
robotMessage.command = COMMAND_ROBOT_STATE_DISABLED;
//robotMessage.robotMode = ROBOT_STATE_DISABLED;
break;
case ROBOT_STATE_AUTONOMOUS:
printf("ROBOT_STATE_AUTONOMOUS\n");
robotMessage.command = COMMAND_ROBOT_STATE_AUTONOMOUS;
//robotMessage.robotMode = ROBOT_STATE_AUTONOMOUS;
break;
case ROBOT_STATE_TELEOPERATED:
printf("ROBOT_STATE_TELEOPERATED\n");
robotMessage.command = COMMAND_ROBOT_STATE_TELEOPERATED;
//robotMessage.robotMode = ROBOT_STATE_TELEOPERATED;
break;
case ROBOT_STATE_TEST:
printf("ROBOT_STATE_TEST\n");
robotMessage.command = COMMAND_ROBOT_STATE_TEST;
//robotMessage.robotMode = ROBOT_STATE_TEST;
break;
case ROBOT_STATE_UNKNOWN:
printf("ROBOT_STATE_UNKNOWN\n");
robotMessage.command = COMMAND_ROBOT_STATE_UNKNOWN;
//robotMessage.robotMode = ROBOT_STATE_UNKNOWN;
break;
}
OnStateChange(); //Handles the state change
}
if(IsEnabled())
{
if((currentRobotState == ROBOT_STATE_TELEOPERATED) ||
(currentRobotState == ROBOT_STATE_TEST) ||
(currentRobotState == ROBOT_STATE_AUTONOMOUS))
{
Run(); //Robot logic
}
}
previousRobotState = currentRobotState;
++loop; //Increment the loop counter
}
}