/* TODO: Decide squared inputs or not */
@Override
    public void teleopPeriodic()
    {
        SmartDashboard.putString("Robot/GamePhase", "TELEOP");
        double timestamp = Timer.getFPGATimestamp();
        double throttle = mControlBoard.getThrottle();
        double turn = mControlBoard.getTurn();
        try
        {
            if (mSuperstructure.isDriverControlled())
            {
                DriveSignal command = ArcadeDriveHelper.arcadeDrive(mControlBoard.getThrottle(), mControlBoard.getTurn(),
                    true /* TODO: Decide squared inputs or not */).scale(mSuperstructure.isDrivingReversed() ? -1 : 1)/*.scale(6)*/;
                mDrive.setOpenLoop(command);
                // mDrive.setVelocity(command, new DriveSignal(
                //     command.scale(Constants.kDriveLeftKv).getLeft() + Math.copySign(Constants.kDriveLeftVIntercept, command.getLeft()),
                //     command.scale(Constants.kDriveLeftKv).getRight() + Math.copySign(Constants.kDriveLeftVIntercept, command.getRight())
                // ));
                if(mControlBoard.getEjectPanel())
                    mPanelHandler.setWantedState(PanelHandler.WantedState.EJECT);
                if(mControlBoard.getIntake())
                    mCargoIntake.setWantedState(CargoIntake.WantedState.INTAKE);
                if(mControlBoard.getTestButtonOne())
                    mCargoIntake.setWantedState(CargoIntake.WantedState.HOLD);
                if(mControlBoard.getEjectCargo())
                    mCargoIntake.setWantedState(CargoIntake.WantedState.EJECT);
                if(mControlBoard.getTestButtonTwo())
                    mPanelHandler.checkSystem("variant");
                if (mControlBoard.getReverseDirection())
                {
                    mSuperstructure.reverseDrivingDirection();
                }
                else if (mControlBoard.getDriveToSelectedTarget())
                {
                    mSuperstructure.setWantedState(Superstructure.WantedState.ALIGN_AND_INTAKE_CARGO);
                }
                else if (mControlBoard.getClimb()) 
                {
                    mSuperstructure.setWantedState(Superstructure.WantedState.CLIMB);
                }
                // TODO (for button person): add buttons for all superstructure wanted states
            }
            else if (mControlBoard.getReturnToDriverControl())
            {
                mSuperstructure.setWantedState(Superstructure.WantedState.DRIVER_CONTROL);
            }
        }
        catch (Throwable t)
        {
            Logger.logThrowableCrash(t);
            throw t;
        }
        outputToSmartDashboard();
    }
{
DriveSignal command = ArcadeDriveHelper.arcadeDrive(mControlBoard.getThrottle(), mControlBoard.getTurn(),
true /* TODO: Decide squared inputs or not */).scale(mSuperstructure.isDrivingReversed() ? -1 : 1)/*.scale(6)*/;
mDrive.setOpenLoop(command);
// mDrive.setVelocity(command, new DriveSignal(
{
SmartDashboard.putString("Robot/GamePhase", "TELEOP");
double timestamp = Timer.getFPGATimestamp();
double throttle = mControlBoard.getThrottle();
double turn = mControlBoard.getTurn();
try
{
if (mSuperstructure.isDriverControlled())
{
DriveSignal command = ArcadeDriveHelper.arcadeDrive(mControlBoard.getThrottle(), mControlBoard.getTurn(),
true /* TODO: Decide squared inputs or not */).scale(mSuperstructure.isDrivingReversed() ? -1 : 1)/*.scale(6)*/;
mDrive.setOpenLoop(command);
// mDrive.setVelocity(command, new DriveSignal(
// command.scale(Constants.kDriveLeftKv).getLeft() + Math.copySign(Constants.kDriveLeftVIntercept, command.getLeft()),
// command.scale(Constants.kDriveLeftKv).getRight() + Math.copySign(Constants.kDriveLeftVIntercept, command.getRight())
// ));
if(mControlBoard.getEjectPanel())
mPanelHandler.setWantedState(PanelHandler.WantedState.EJECT);
if(mControlBoard.getIntake())
mCargoIntake.setWantedState(CargoIntake.WantedState.INTAKE);
if(mControlBoard.getTestButtonOne())
@Override
public void teleopPeriodic()
{
SmartDashboard.putString("Robot/GamePhase", "TELEOP");
double timestamp = Timer.getFPGATimestamp();
double throttle = mControlBoard.getThrottle();
double turn = mControlBoard.getTurn();
try
{
if (mSuperstructure.isDriverControlled())
{
DriveSignal command = ArcadeDriveHelper.arcadeDrive(mControlBoard.getThrottle(), mControlBoard.getTurn(),
true /* TODO: Decide squared inputs or not */).scale(mSuperstructure.isDrivingReversed() ? -1 : 1)/*.scale(6)*/;
mDrive.setOpenLoop(command);
// mDrive.setVelocity(command, new DriveSignal(
// command.scale(Constants.kDriveLeftKv).getLeft() + Math.copySign(Constants.kDriveLeftVIntercept, command.getLeft()),
// command.scale(Constants.kDriveLeftKv).getRight() + Math.copySign(Constants.kDriveLeftVIntercept, command.getRight())
// ));
if(mControlBoard.getEjectPanel())
mPanelHandler.setWantedState(PanelHandler.WantedState.EJECT);
if(mControlBoard.getIntake())
mCargoIntake.setWantedState(CargoIntake.WantedState.INTAKE);
if(mControlBoard.getTestButtonOne())
mCargoIntake.setWantedState(CargoIntake.WantedState.HOLD);
if(mControlBoard.getEjectCargo())
mCargoIntake.setWantedState(CargoIntake.WantedState.EJECT);
if(mControlBoard.getTestButtonTwo())
mPanelHandler.checkSystem("variant");
if (mControlBoard.getReverseDirection())
{
mSuperstructure.reverseDrivingDirection();
}
else if (mControlBoard.getDriveToSelectedTarget())