void TeleopPeriodic()
{
if(!isWait)
{
//drive
drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)), setTurnSpeed(oi->joyDrive->GetRawAxis(TURN_X_AXIS));
drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9));
//manipulator
manip->moveArm(oi->joyManip->GetRawButton(6), oi->joyManip->GetRawButton(7));
manip->intakeBall(oi->joyManip->GetRawButton(INTAKE_BUTTON), oi->joyManip->GetRawButton(OUTTAKE_BUTTON), (oi->getManipJoystick()->GetThrottle()+1)/2);
catapult->launchBall();
manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON));
}
//camera motor mount
if(oi->joyManip->GetRawButton(10))
{
bCameraLatch = true;
}
else if(oi->joyManip->GetRawButton(11))
{
bCameraLatch = false;
}
manip->toggleCameraPosition(bCameraLatch);
}
void MedicDrive::autoTurn(double target, double speed)
{
static bool init = true;
static double initTicks = 0;
static double currentTicks = 0;
static double deltaTicks = 0;
static double targetTicks = 0;
static double error = 0;
if(init)
{
initTicks = leftEncoder->Get() - rightEncoder->Get();
targetTicks = target / TICKS_PER_DEGREE;
init = false;
}
else
{
currentTicks = leftEncoder->Get() - rightEncoder->Get();
deltaTicks = currentTicks - initTicks;
error = targetTicks - deltaTicks;
if(error < 0 - AUTO_TURN_DEADZONE / TICKS_PER_DEGREE)
{
setTurnSpeed(-speed, false);
isAtTurnTarget = false;
}
else if(error > 0 + AUTO_TURN_DEADZONE / TICKS_PER_DEGREE)
{
setTurnSpeed(speed, false);
isAtTurnTarget = false;
}
else
{
setTurnSpeed(0, false);
isAtTurnTarget = true;
init = true;
}
drive();
}
}
