@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();
    }
@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).scale(mSuperstructure.isDrivingReversed() ? -1 : 1;
                mDrive.setOpenLoop(command);
               
               
               
               
                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);
                }
               
            }
            else if (mControlBoard.getReturnToDriverControl())
            {
                mSuperstructure.setWantedState(Superstructure.WantedState.DRIVER_CONTROL);
            }
        }
        catch (Throwable t)
        {
            Logger.logThrowableCrash(t);
            throw t;
        }
        outputToSmartDashboard();
    }
@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).scale(msuperstructure.isdrivingreversed() ? -1 : 1; mdrive.setopenloop(command); 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); } } else if (mcontrolboard.getreturntodrivercontrol()) { msuperstructure.setwantedstate(superstructure.wantedstate.driver_control); } } catch (throwable t) { logger.logthrowablecrash(t); throw t; } outputtosmartdashboard(); }