/**
* This method is called every 20ms to update the robots components during autonomous.
* There should be no blocking calls in this method (connection requests, large data collection, etc),
* failing to comply with this could result in inconsistent robot behavior
*/
void AutonomousPeriodic()
{
//In the first two seconds of auto, drive forwardat half power
if(Timer::GetFPGATimestamp() - autoTimer >= 0 && Timer::GetFPGATimestamp() < 2)
{
rd->SetLeftRightMotorOutputs(.5, .5);
}
else if(Timer::GetFPGATimestamp() - autoTimer >= 2 && Timer::GetFPGATimestamp() < 4)
{
//2 to 4 seconds, stop drivetrain and spin shooter wheels
rd->SetLeftRightMotorOutputs(0, 0);
shoot1->Set(1);
shoot2->Set(1);
}
else if(Timer::GetFPGATimestamp() - autoTimer >= 4 && Timer::GetFPGATimestamp() < 8 && !kickerSwitch->Get())
{
//4 to 8 seconds and while kicker switch is not true, spin shooter wheels and kicker
shoot1->Set(1);
shoot2->Set(1);
kicker->Set(-1);
}
else
{
//After all that, stop everything
rd->SetLeftRightMotorOutputs(0, 0);
shoot1->Set(0);
shoot2->Set(0);
kicker->Set(0);
}
}
void TeleopPeriodic()
{
while(1)
{
fps->SetLeftRightMotorOutputs(0.5*-drivercontroller->GetRawAxis(1) + drivercontroller->GetRawAxis(2), 0.5*-drivercontroller->GetRawAxis(1) - drivercontroller->GetRawAxis(2));
if(operatorcontroller->GetRawAxis(1) > 0)
{
armminipluator->Set(0.6*operatorcontroller->GetRawAxis(1));
}
else if(operatorcontroller->GetRawAxis(1) < 0)
{
armminipluator->Set(0.3*operatorcontroller->GetRawAxis(1));
}
if(operatorcontroller->GetRawButton(5))
{
shro->Set(1);
}
else if(operatorcontroller->GetRawAxis(6))
{
shro->Set(-1);
}
else
{
shro->Set(0);
}
}
}
void TeleopPeriodic(void )
{
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
//Start compressor
compressor->Start();
driveTrainValues();
deadzone();
//Drivetrain.....
//When button eight is pressed robot drives at 25% speed
printf("right: %f and left: %f\n", useright, useleft);
if (gamepad->GetRawButton(8))
{
drivetrain->TankDrive((-0.5*(useleft)), (-0.5*(useright)));
//Negative for switched wires
}
else
{
drivetrain->SetLeftRightMotorOutputs(-useleft, -useright);
//Normal driving
//Negative for switched wires
}
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
GetWatchdog().Feed();
// if(autoPilot == true)
// {
// Auto Align Disable Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 2)
// {
// Goal_Align_PID->Disable(); // Stop outputs
// Goal_Align_PID->Enable(); // Start PIDContoller up again
// Goal_Align_PID->SetSetpoint(0.0);
// autoPilot = false;
// }
// }
// else
// {
// Calculate jaguar output based on exponent we pass from SmartDashboard
double leftOutput, rightOutput;
leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2));
rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5));
m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput);
if(operatorGamepad->GetRawButton(1) && !buttonWasDown)
{
printf("LEFT ENCODER: %f\n", Front_L->GetPosition());
printf("RIGHT ENCODER: %f\n", Front_R->GetPosition());
}
buttonWasDown = operatorGamepad->GetRawButton(1);
// Auto Align Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 1)
// {
// // Turn Auto Align on if we see a goal and we know the azimuth
// if(SmartDashboard::GetBoolean(FOUND_KEY) == true)
// {
// Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY));
// autoPilot = true;
// }
// }
// }
}