void OperatorControl(void)
{
myRobot->SetSafetyEnabled(false);
LEDLights (true); //turn camera lights on
shooterspeedTask->Start((UINT32)this); //start counting shooter speed
kickerTask->Start((UINT32)this); //turns on the kicker task
kicker_in_motion = false;
while (IsOperatorControl() && !IsDisabled())
{
if (ControllBox->GetDigital(3)) //turn tracking on with switch 3 on controll box
{
tracking(ControllBox->GetDigital(7));
}
else
{
myRobot->TankDrive(leftstick, rightstick); //if tracking is off, enable human drivers
Wait(0.005); // wait for a motor update time
}
Shooter_onoff=ControllBox->GetDigital(4); //shoot if switch 4 is on
ballgatherer(ControllBox->GetDigital(5), rightstick->GetRawButton(10));
kicker_onoff=lonelystick->GetRawButton(1);
bridgeboot(ControllBox->GetDigital(6));
kicker_cancel=lonelystick->GetRawButton(2);
//kicker_down=rightstick->GetRawButton(11));
}
LEDLights (false);
shooterspeedTask->Stop();
kickerTask->Stop();
ballgatherer(false, false);
kickermotor->Set(Relay::kOff);
}
/**
* Periodic code for disabled mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in disabled mode.
*/
void DisabledPeriodic()
{
// Keep watching wheel speeds during spin-down
RunWheels();
// respond to log dump request even when disabled
if (!eio->GetDigital(13))
{
if (!dump)
{
LogSave("/ni-rt/system/k9.csv");
}
dump = true;
}
else
{
dump = false;
}
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void TeleopPeriodic()
{
if (!eio->GetDigital(1))
{
StartWheels();
}
else if (!eio->GetDigital(2))
{
StopWheels();
}
RunWheels();
#ifdef HAVE_LEGS
if (!eio->GetDigital(3))
{
legs->Set(true);
}
else if (!eio->GetDigital(4))
{
legs->Set(false);
}
#endif
#ifdef HAVE_INJECTOR
if (!eio->GetDigital(5))
{
injectorL->Set(DoubleSolenoid::kForward);
injectorR->Set(DoubleSolenoid::kForward);
}
else if (!eio->GetDigital(6))
{
injectorL->Set(DoubleSolenoid::kReverse);
injectorR->Set(DoubleSolenoid::kReverse);
}
else
{
injectorL->Set(DoubleSolenoid::kOff);
injectorR->Set(DoubleSolenoid::kOff);
}
#endif
#ifdef HAVE_EJECTOR
if (!eio->GetDigital(7))
{
ejector->Set(true);
}
else if (!eio->GetDigital(8))
{
ejector->Set(false);
}
#endif
if (!eio->GetDigital(13))
{
if (!dump)
{
LogSave("/ni-rt/system/k9.csv");
}
dump = true;
}
else
{
dump = false;
}
}
