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 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 smartDashboardPrint()
{
oi->dashboard->PutNumber("Linear Velocity", drive->getLinVelocity());
oi->dashboard->PutNumber("Left Encoder", drive->leftEncoder->Get());
oi->dashboard->PutNumber("Right Encoder", drive->rightEncoder->Get());
oi->dashboard->PutNumber("Step", step);
}
void TeleopInit()
{
drive->setLinVelocity(0);
drive->setTurnSpeed(0, false);
drive->drive();
}