void teleDrive()
{
drive->setLinVelocity(oi->getDriveJoystick()->GetY());
drive->setTurnSpeed(oi->getDriveJoystick()->GetX(), oi->getDriveJoystickButton(1));//TODO: Actual button value needed
drive->drive();
drive->shift(oi->getDriveJoystickButton(1), oi->getDriveJoystickButton(1));
}
void shoot()
{
if(oi->getPIDToggle(1))
{
shooter->setTarget((oi->getManipJoystick()->GetThrottle()+1)/2);
shooter->setVelocity(shooter->pidOutput->readOutput());
shooter->shootDisc();
}
}
void teleDrive()
{
drive->setLinVelocity(-oi->getDriveJoystick()->GetY(Joystick::kRightHand));
drive->setTurnSpeed(oi->getDriveJoystick()->GetX(Joystick::kRightHand), oi->getDriveJoystickButton(1));
drive->drive();
drive->shift(oi->getDriveJoystickButton(8), oi->getDriveJoystickButton(9));
if(oi->getDriveJoystickButton(6))
{
comp599->Start();
}
else if(oi->getDriveJoystickButton(7))
{
comp599->Stop();
}
}
void intake()
{
if(oi->getManipJoystickButton(1) == 1)
{
manipulator->intakeDisc(true); // intake
manipulator->conveyer(true); // move
manipulator->loadMagazine(true, false); // load, unload
manipulator->feedShooter(true); // feed
}
else
{
manipulator->intakeDisc(false); // intake
manipulator->conveyer(false); // move
manipulator->loadMagazine(false, true); // load, unload
manipulator->feedShooter(false); // feed
}
}
