diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 37097e82..e96bdce0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -242,48 +242,7 @@ public String[] getPaths() { public static final double reverseStopperWheelSpeed = -0.125; - // spotless:off - // Controller Constants { - public static final int portNumber0 = 0; - public static final int portNumber1 = 1; - - // Buttons not in use - public static final int XButton = 1; - public static final int AButton = 2; - public static final int BButton = 3; - public static final int YButton = 4; - - // Intake Subsystem - public static final int LBumper = 5; // Intake forward - public static final int RBumper = 6; // Intake reverse - - // CDS Subsystem - public static final int LTriggerButton = 7; // CDS forward - public static final int RTriggerButton = 8; // CDS reverse - - // Shooter Prime/LimeLight - public static final int backButton = 9; // Button starts ShooterPrime - public static final int startButton = 10; // Button to align LimeLight - - // Shooter Subsystem mode change - public static final int LJoystickButton = 11; // Button for Shooter mode - public static final int RJoystickButton = 12; // BUtton for Shooter mode - - // POV's not in use - public static final int POVup = 0; - public static final int POVdown = 180; - public static final int POVright = 90; - public static final int POVleft = 270; - - // Joystick - - // Unused but will easily be accidentally activated if used - public static final int leftJoystickX = 0; - public static final int leftJoystickY = 1; // arcade forward / tank left turning - public static final int rightJoystickX = 2; // arcade turning - public static final int rightJoystickY = 3; // tank right turning - - // Shooter Constants + // ShooterConstants public static final class Shooter { // Motor IDs public static final int shooterID = 10; // ID of the shooter @@ -365,11 +324,6 @@ public static final class Shooter { public static final double climbServoSetPoint = 0.5; - public static final double driveSpeedHigh = 1; - public static final double driveSpeedLow = 0.6; - - public static final double controllerDeadZone = 0.1; - public static final double climbSequence1Timeout = 20.0; public static final double armUpSpeed = -1; diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java new file mode 100644 index 00000000..78c86b8b --- /dev/null +++ b/src/main/java/frc/robot/OI.java @@ -0,0 +1,121 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.Supplier; + +public class OI { + // Operator Interface (OI) class containing all control information + private static final int kDriverJoystickPort = 0; + private static final int kOperatorJoystickPort = 1; + + public static final class Driver { + private static final Joystick kJoystick = new Joystick(OI.kDriverJoystickPort); + + private static final int kIntakeButtonID = 8; // Run the CDS and intake forwards + private static final int kOuttakeButtonID = 6; // Run the CDS and intake backwards + private static final int kShootButtonID = 7; // Fire the shooter + + private static final int kDriveSpeedAxis = 1; // This axis is inverted + private static final int kDriveRotationAxis = 2; + + private static final ControlCurve kDriveSpeedCurve = new ControlCurve(0.9, 0, 0, 0); + private static final ControlCurve kDriveRotationCurve = new ControlCurve(0.85, 0, 0, 0); + + public static Supplier getDriveSpeedSupplier() { + return () -> kDriveSpeedCurve.calculate(-kJoystick.getRawAxis(kDriveSpeedAxis)); + } + + public static Supplier getDriveRotationSupplier() { + return () -> kDriveRotationCurve.calculate(kJoystick.getRawAxis(kDriveRotationAxis)); + } + + public static JoystickButton getIntakeButton() { + return new JoystickButton(kJoystick, kIntakeButtonID); + } + + public static JoystickButton getOuttakeButton() { + return new JoystickButton(kJoystick, kOuttakeButtonID); + } + + public static JoystickButton getShootButton() { + return new JoystickButton(kJoystick, kShootButtonID); + } + } + + public static final class Operator { + private static final Joystick kJoystick = new Joystick(OI.kOperatorJoystickPort); + + private static final int kEnableClimbButtonID = + 10; // Toggle the ability to manipulate climb arms + private static final int kAutoClimbButtonID = 1; // Automatically deploy climb arms + private static final int kCDSForwardButtonID = 8; // Run the CDS forward + private static final int kOuttakeButtonID = 6; // Run the CDS and intake backwards + + private static final int kClimbArmAxis = 1; // This axis is inverted + private static final int kClimbPoleAxis = 3; // This axis is inverted + + private static final ControlCurve kClimbArmCurve = new ControlCurve(1, 0, 0, 0.1); + private static final ControlCurve kClimbPoleCurve = new ControlCurve(1, 0, 0, 0.1); + + public static Supplier getClimbArmSupplier() { + return () -> kClimbArmCurve.calculate(-kJoystick.getRawAxis(kClimbArmAxis)); + } + + public static Supplier getClimbPoleSupplier() { + return () -> kClimbPoleCurve.calculate(-kJoystick.getRawAxis(kClimbPoleAxis)); + } + + public static JoystickButton getEnableClimbButton() { + return new JoystickButton(kJoystick, kEnableClimbButtonID); + } + + public static JoystickButton getAutoClimbButton() { + return new JoystickButton(kJoystick, kAutoClimbButtonID); + } + + public static JoystickButton getCDSForwardButton() { + return new JoystickButton(kJoystick, kCDSForwardButtonID); + } + + public static JoystickButton getOuttakeButton() { + return new JoystickButton(kJoystick, kOuttakeButtonID); + } + } + + public static class ControlCurve { + private double ySaturation; // Maximum output, in percentage of possible output + private double yIntercept; // Minimum output, in percentage of saturation + private double curvature; // Curvature shift between linear and cubic + private double deadzone; // Range of input that will always return zero output + + public ControlCurve(double ySaturation, double yIntercept, double curvature, double deadzone) { + this.ySaturation = ySaturation; + this.yIntercept = yIntercept; + this.curvature = curvature; + this.deadzone = deadzone; + } + + public double calculate(double input) { + /* Two equations, separated by a ternary + The first is the deadzone + y = 0 {|x| < d} + The second is the curve + y = a(sign(x) * b + (1 - b) * (c * x^3 + (1 - c) * x)) {|x| >= d} + Where + x = input + y = output + a = ySaturation + b = yIntercept + c = curvature + d = deadzone + and 0 <= a,b,c,d < 1 + */ + return Math.abs(input) < deadzone + ? 0 + : ySaturation + * (Math.signum(input) * yIntercept + + (1 - yIntercept) * (curvature * Math.pow(input, 3) + (1 - curvature) * input)); + } + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aaa049cc..9fd3e282 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,18 +5,15 @@ package frc.robot; import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Constants.Auton; import frc.robot.commands.AutonModes; import frc.robot.commands.CDSBallManagementCommand; import frc.robot.commands.CDSForwardCommand; -import frc.robot.commands.ClimbEnable; import frc.robot.commands.ClimbPeriodic; import frc.robot.commands.ClimbSequence1; import frc.robot.commands.CombinedIntakeCDSForwardCommand; @@ -46,10 +43,6 @@ public class RobotContainer { public static ShuffleboardTab controllerDetection; // The robot's subsystems and commands are defined here... - private static final Joystick driverJoystick = new Joystick(Constants.portNumber0); - private static final Joystick operatorJoystick = new Joystick(Constants.portNumber1); - private JoystickButton[] buttons = new JoystickButton[13]; - private JoystickButton[] buttons2 = new JoystickButton[13]; // subsystems private static ClimbSubsystem climbSubsystem; @@ -71,9 +64,9 @@ public class RobotContainer { private OuttakeCommand outtakeCommand; private LimelightAlign limelightAlign; // ----------climb--------- - private ClimbEnable climbEnabling; + private InstantCommand climbEnable; private ClimbSequence1 climbSequence1; - private ClimbPeriodic ClimbPeriodic; + private ClimbPeriodic climbPeriodic; private Command HaDeploy; private Command hookUnlock; private Command hookLock; @@ -106,35 +99,12 @@ public RobotContainer() { initSubsystems(); initCommands(); - // initialize the button bindings - for (int i = 1; i < buttons.length; i++) { - buttons[i] = new JoystickButton(driverJoystick, i); - buttons2[i] = new JoystickButton(operatorJoystick, i); - } configureButtonBindings(); } - private void controllerCheck() { - axisCount0 = DriverStation.getStickAxisCount(Constants.portNumber0); - buttonCount0 = DriverStation.getStickButtonCount(Constants.portNumber0); - sbaxisCount0.setDouble(axisCount0); - sbbuttonCount0.setDouble(buttonCount0); - - axisCount1 = DriverStation.getStickAxisCount(Constants.portNumber1); - buttonCount1 = DriverStation.getStickButtonCount(Constants.portNumber1); - sbaxisCount1.setDouble(axisCount1); - sbbuttonCount1.setDouble(buttonCount1); - - System.out.printf( - "axisCount0 %d buttonCount0 %d axisCount1 %d buttonCount1 %d\n ", - axisCount0, buttonCount0, axisCount1, buttonCount1); - } - private void initSubsystems() { - // subsystems - controllerCheck(); - driveBaseSubsystem = new DriveBaseSubsystem(driverJoystick, Constants.usingExternal); + driveBaseSubsystem = new DriveBaseSubsystem(Constants.usingExternal); cdsSubsystem = new CDSSubsystem(); @@ -144,13 +114,17 @@ private void initSubsystems() { limelightSubsystem = new LimelightSubsystem(); - climbSubsystem = new ClimbSubsystem(operatorJoystick); + climbSubsystem = new ClimbSubsystem(); } private void initCommands() { // Initializes commands based on enabled subsystems if (driveBaseSubsystem != null) { - driveBaseTeleopCommand = new DriveBaseTeleopCommand(driveBaseSubsystem); + driveBaseTeleopCommand = + new DriveBaseTeleopCommand( + driveBaseSubsystem, + OI.Driver.getDriveSpeedSupplier(), + OI.Driver.getDriveRotationSupplier()); driveBaseSubsystem.setDefaultCommand(driveBaseTeleopCommand); } if (cdsSubsystem != null && shooterSubsystem != null) { @@ -184,12 +158,16 @@ private void initCommands() { } if ((climbSubsystem != null) && (driveBaseSubsystem != null)) { - climbEnabling = new ClimbEnable(climbSubsystem, driveBaseSubsystem); - ClimbPeriodic = new ClimbPeriodic(climbSubsystem); + climbEnable = new InstantCommand(climbSubsystem::toggleEnabled, climbSubsystem); + climbPeriodic = + new ClimbPeriodic( + climbSubsystem, + OI.Operator.getClimbArmSupplier(), + OI.Operator.getClimbPoleSupplier()); climbSequence1 = new ClimbSequence1(climbSubsystem); hookUnlock = new HookUnlock(climbSubsystem); hookLock = new HookLock(climbSubsystem); - climbSubsystem.setDefaultCommand(ClimbPeriodic); + climbSubsystem.setDefaultCommand(climbPeriodic); } } @@ -200,47 +178,33 @@ private void initCommands() { // it to a {@link // edu.wpi.first.wpilibj2.command.button.JoystickButton}. private void configureButtonBindings() { - controllerCheck(); // Intake / CDS if (outtakeCommand != null) { - // spits ball out - buttons[Constants.RBumper].whileHeld(outtakeCommand); + OI.Driver.getOuttakeButton().whileHeld(outtakeCommand); } if (combinedIntakeCDS != null) { - buttons[Constants.RTriggerButton].whileHeld(combinedIntakeCDS); - } /*else { - buttons[Constants.RTriggerButton].whileHeld(intakeForwardCommand); - }*/ + OI.Driver.getIntakeButton().whileHeld(combinedIntakeCDS); + } if (shooterSubsystem != null && shooterHeldLow != null && shooterHeldAuto != null) { - // Auto Aim Shot - buttons[Constants.LTriggerButton].whileHeld( - shooterHeldAuto.beforeStarting( - () -> { - shooterSubsystem.setAimMode(Constants.AimModes.TARMAC); - }, - shooterSubsystem)); - // Fender Shot - buttons[Constants.LBumper].whileHeld( - shooterHeldLow.beforeStarting( - () -> { - shooterSubsystem.setAimMode(Constants.AimModes.LOW); - }, - shooterSubsystem)); + OI.Driver.getShootButton() + .whileHeld( + shooterHeldLow.beforeStarting( + () -> { + shooterSubsystem.setAimMode(Constants.AimModes.LOW); + }, + shooterSubsystem)); } if (climbSubsystem != null) { - // enable climb and spool out arms - buttons2[Constants.startButton].whenPressed(climbEnabling); - buttons2[Constants.XButton].whileHeld(climbSequence1); - - // whenHeld button for ClimbSequence2 + OI.Operator.getEnableClimbButton().whenPressed(climbEnable); + OI.Operator.getAutoClimbButton().whileHeld(climbSequence1); } - if (outtakeCommand != null && intakeForwardCommand != null) { - buttons2[Constants.RTriggerButton].whileHeld(intakeForwardCommand); + if (outtakeCommand != null && CDSForwardCommand != null) { + OI.Operator.getCDSForwardButton().whileHeld(CDSForwardCommand); } } diff --git a/src/main/java/frc/robot/commands/ClimbEnable.java b/src/main/java/frc/robot/commands/ClimbEnable.java deleted file mode 100644 index 7bc05971..00000000 --- a/src/main/java/frc/robot/commands/ClimbEnable.java +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; -import frc.robot.subsystems.ClimbSubsystem; -import frc.robot.subsystems.DriveBaseSubsystem; - -public class ClimbEnable extends CommandBase { - private final ClimbSubsystem m_subsystem; - private final DriveBaseSubsystem m_drivesubsystem; - - /** Creates a new ClimbEnable. */ - public ClimbEnable(ClimbSubsystem s, DriveBaseSubsystem d) { - addRequirements(s); - addRequirements(d); - m_subsystem = s; - m_drivesubsystem = d; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_subsystem.climbEnable(); - m_subsystem.resetClimbHeights(); - - if (m_subsystem.getclimbingenable()) { - m_drivesubsystem.setDriveBaseSpeed(Constants.driveSpeedLow); - } else { - m_drivesubsystem.setDriveBaseSpeed(Constants.driveSpeedHigh); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/commands/ClimbPeriodic.java b/src/main/java/frc/robot/commands/ClimbPeriodic.java index 632bc48b..addeccf4 100644 --- a/src/main/java/frc/robot/commands/ClimbPeriodic.java +++ b/src/main/java/frc/robot/commands/ClimbPeriodic.java @@ -6,31 +6,37 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ClimbSubsystem; +import java.util.function.Supplier; public class ClimbPeriodic extends CommandBase { - private final ClimbSubsystem m_subsystem; + private final ClimbSubsystem climbSubsystem; + private final Supplier climbArmSupplier; + private final Supplier climbPoleSupplier; /** Creates a new ClimbKeepDown. */ - public ClimbPeriodic(ClimbSubsystem s) { - addRequirements(s); - - m_subsystem = s; - // Use addRequirements() here to declare subsystem dependencies. + public ClimbPeriodic( + ClimbSubsystem climbSubsystem, + Supplier climbArmSupplier, + Supplier climbPoleSupplier) { + addRequirements(climbSubsystem); + this.climbSubsystem = climbSubsystem; + this.climbArmSupplier = climbArmSupplier; + this.climbPoleSupplier = climbPoleSupplier; } // Called when the command is initially scheduled. @Override public void initialize() { - m_subsystem.resetClimbHeights(); - m_subsystem.climbKeepDownFunction(); + climbSubsystem.resetClimbHeights(); + climbSubsystem.climbKeepDownFunction(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_subsystem.midClimb(); - m_subsystem.highArms(); - m_subsystem.periodic(); + climbSubsystem.midClimb(climbArmSupplier.get()); + climbSubsystem.highArms(climbPoleSupplier.get()); + climbSubsystem.periodic(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java b/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java index 27f760a8..c5baf2f3 100644 --- a/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java +++ b/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java @@ -6,14 +6,23 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DriveBaseSubsystem; +import java.util.function.Supplier; public class DriveBaseTeleopCommand extends CommandBase { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final DriveBaseSubsystem subsystem; - - public DriveBaseTeleopCommand(DriveBaseSubsystem s) { - addRequirements(s); - subsystem = s; + private final DriveBaseSubsystem driveBaseSubsystem; + + private final Supplier speedSupplier; + private final Supplier rotationSupplier; + + public DriveBaseTeleopCommand( + DriveBaseSubsystem driveBaseSubsystem, + Supplier speedSupplier, + Supplier rotationSupplier) { + addRequirements(driveBaseSubsystem); + this.driveBaseSubsystem = driveBaseSubsystem; + this.speedSupplier = speedSupplier; + this.rotationSupplier = rotationSupplier; } @Override @@ -21,12 +30,12 @@ public void initialize() {} @Override public void execute() { - subsystem.arcadeDrive(); + driveBaseSubsystem.arcadeDrive(speedSupplier.get(), rotationSupplier.get()); } @Override public void end(boolean interrupted) { - subsystem.stopDriveMotors(); + driveBaseSubsystem.stopDriveMotors(); } @Override diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index de85ba26..b744ed09 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -8,7 +8,6 @@ import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -21,7 +20,6 @@ public class ClimbSubsystem extends SubsystemBase { // 1 = Right Side, 2 = Left Side - private Joystick climbJoystick; private MotorController armOne; private MotorController armTwo; private MotorController poleOne; @@ -29,7 +27,7 @@ public class ClimbSubsystem extends SubsystemBase { private Servo servoOne; private Servo servoTwo; - private boolean climbEnable, hookLocked; + private boolean climbEnabled, hookLocked; private double armHeightOne; private double armHeightTwo; @@ -40,9 +38,6 @@ public class ClimbSubsystem extends SubsystemBase { private double poleEncoderHeightOne; private double poleEncoderHeightTwo; - private double armJoystickAxis; - private double poleJoystickAxis; - // 1 = Right Side, 2 = Left Side private ShuffleboardTab climbTab; @@ -117,9 +112,8 @@ public class ClimbSubsystem extends SubsystemBase { .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); - public ClimbSubsystem(Joystick joystick) { - climbJoystick = joystick; - climbEnable = false; + public ClimbSubsystem() { + climbEnabled = false; // Shuffle Board Widgets climbTab = Shuffleboard.getTab("ClimbBase"); @@ -228,9 +222,9 @@ public void climbKeepDownFunction() { poleTwo.getPIDCtrl().setIMaxAccum(Constants.poleSetIMaxAccum, 0); } - public void climbEnable() { - climbEnable = !climbEnable; - if (climbEnable) { + public void toggleEnabled() { + climbEnabled = !climbEnabled; + if (climbEnabled) { armOne.setSmartCurrentLimit(Constants.climbArmHighCurrent); armTwo.setSmartCurrentLimit(Constants.climbArmHighCurrent); poleOne.setSmartCurrentLimit(Constants.climbPoleHighCurrent); @@ -243,71 +237,47 @@ public void climbEnable() { } } - public boolean getclimbingenable() { - return climbEnable; - } - - public void midClimb() { - if (climbEnable) { - armJoystickAxis = -climbJoystick.getRawAxis(Constants.leftJoystickY); - if (armJoystickAxis > Constants.controllerDeadZone - || armJoystickAxis < -Constants.controllerDeadZone) { - if (armJoystickAxis > 0) { - /*if (armHeightOne + (armJoystickAxis * (Constants.ArmUpSpeed/2)) >= Constants.ArmHeightFeather) { - armHeightOne = armHeightOne + (armJoystickAxis * Constants.ArmUpSpeed/2); - } else{*/ - if (armHeightOne + (armJoystickAxis * Constants.armUpSpeed) >= Constants.armHeightMin) { - armHeightOne = armHeightOne + (armJoystickAxis * Constants.armUpSpeed); - // } - } - /*if (armHeightTwo + (armJoystickAxis * (Constants.ArmUpSpeed/2)) >= Constants.ArmHeightFeather) { - armHeightTwo = armHeightTwo + (armJoystickAxis * Constants.ArmUpSpeed/2); - } else{*/ - if (armHeightTwo + (armJoystickAxis * Constants.armUpSpeed) >= Constants.armHeightMin) { - armHeightTwo = armHeightTwo + (armJoystickAxis * Constants.armUpSpeed); - // } - } + public void midClimb(double axisValue) { + if (climbEnabled) { + if (axisValue > 0) { + if (armHeightOne + (axisValue * Constants.armUpSpeed) >= Constants.armHeightMin) { + armHeightOne = armHeightOne + (axisValue * Constants.armUpSpeed); } - if (armJoystickAxis < 0) { - if (armHeightOne + (armJoystickAxis * Constants.armDownSpeed) <= Constants.armHeightMax) { - armHeightOne = armHeightOne + (armJoystickAxis * Constants.armDownSpeed); - } - if (armHeightTwo + (armJoystickAxis * Constants.armDownSpeed) <= Constants.armHeightMax) { - armHeightTwo = armHeightTwo + (armJoystickAxis * Constants.armDownSpeed); - } + if (armHeightTwo + (axisValue * Constants.armUpSpeed) >= Constants.armHeightMin) { + armHeightTwo = armHeightTwo + (axisValue * Constants.armUpSpeed); + } + } + if (axisValue < 0) { + if (armHeightOne + (axisValue * Constants.armDownSpeed) <= Constants.armHeightMax) { + armHeightOne = armHeightOne + (axisValue * Constants.armDownSpeed); + } + if (armHeightTwo + (axisValue * Constants.armDownSpeed) <= Constants.armHeightMax) { + armHeightTwo = armHeightTwo + (axisValue * Constants.armDownSpeed); } } - armOne.getPIDCtrl().setReference(armHeightOne, CANSparkMax.ControlType.kPosition); - - armTwo.getPIDCtrl().setReference(armHeightTwo, CANSparkMax.ControlType.kPosition); } + armOne.getPIDCtrl().setReference(armHeightOne, CANSparkMax.ControlType.kPosition); + armTwo.getPIDCtrl().setReference(armHeightTwo, CANSparkMax.ControlType.kPosition); } - public void highArms() { - poleJoystickAxis = -climbJoystick.getRawAxis(Constants.rightJoystickY); - if (poleJoystickAxis > Constants.controllerDeadZone - || poleJoystickAxis < -Constants.controllerDeadZone) { - if (poleJoystickAxis > 0) { - if (poleHeightOne + (poleJoystickAxis * Constants.poleInSpeed) <= Constants.poleHeightMax) { - poleHeightOne = poleHeightOne + (poleJoystickAxis * Constants.poleInSpeed); - } - if (poleHeightTwo + (poleJoystickAxis * Constants.poleInSpeed) <= Constants.poleHeightMax) { - poleHeightTwo = poleHeightTwo + (poleJoystickAxis * Constants.poleInSpeed); - } + public void highArms(double axisValue) { + if (axisValue > 0) { + if (poleHeightOne + (axisValue * Constants.poleInSpeed) <= Constants.poleHeightMax) { + poleHeightOne = poleHeightOne + (axisValue * Constants.poleInSpeed); } - if (poleJoystickAxis < 0) { - if (poleHeightOne + (poleJoystickAxis * Constants.poleOutSpeed) - >= Constants.poleHeightMin) { - poleHeightOne = poleHeightOne + (poleJoystickAxis * Constants.poleOutSpeed); - } - if (poleHeightTwo + (poleJoystickAxis * Constants.poleOutSpeed) - >= Constants.poleHeightMin) { - poleHeightTwo = poleHeightTwo + (poleJoystickAxis * Constants.poleOutSpeed); - } + if (poleHeightTwo + (axisValue * Constants.poleInSpeed) <= Constants.poleHeightMax) { + poleHeightTwo = poleHeightTwo + (axisValue * Constants.poleInSpeed); + } + } + if (axisValue < 0) { + if (poleHeightOne + (axisValue * Constants.poleOutSpeed) >= Constants.poleHeightMin) { + poleHeightOne = poleHeightOne + (axisValue * Constants.poleOutSpeed); + } + if (poleHeightTwo + (axisValue * Constants.poleOutSpeed) >= Constants.poleHeightMin) { + poleHeightTwo = poleHeightTwo + (axisValue * Constants.poleOutSpeed); } } poleOne.getPIDCtrl().setReference(poleHeightOne, CANSparkMax.ControlType.kPosition); - poleTwo.getPIDCtrl().setReference(poleHeightOne, CANSparkMax.ControlType.kPosition); } @@ -385,8 +355,8 @@ public boolean atFirstSetpoint() { } public void periodic() { - if (DriverStation.isDisabled() && climbEnable) { - climbEnable(); + if (DriverStation.isDisabled() && climbEnabled) { + toggleEnabled(); } if (DriverStation.isDisabled()) { @@ -396,7 +366,7 @@ public void periodic() { lockHooks(); } - BClimbEnabled.setBoolean(climbEnable); + BClimbEnabled.setBoolean(climbEnabled); DClimbHeight1.setNumber(armOne.getEncoder().getPosition()); DClimbHeight2.setNumber(armTwo.getEncoder().getPosition()); DClimbHeight3.setNumber(poleOne.getEncoder().getPosition()); diff --git a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java index 85827564..dab5b9b0 100644 --- a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj.ADIS16448_IMU; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -34,8 +33,6 @@ import frc.robot.common.hardware.MotorController; public class DriveBaseSubsystem extends SubsystemBase { - private double driveBaseSpeed; - private final Joystick driverJoystick; private final MotorController[] motorControllers; private final DifferentialDrive differentialDrive; private DifferentialDrivetrainSim differentialDrivetrainSim; @@ -72,9 +69,7 @@ public class DriveBaseSubsystem extends SubsystemBase { private NetworkTableEntry sbRightPosition; private NetworkTableEntry sbGyroInfo; - public DriveBaseSubsystem(Joystick joystick, boolean usingExternal) { - driveBaseSpeed = 1; - driverJoystick = joystick; + public DriveBaseSubsystem(boolean usingExternal) { motorControllers = new MotorController[4]; gyro = new AHRS(SPI.Port.kMXP); @@ -247,33 +242,8 @@ public void periodic() { motorControllers[Constants.driveRightFrontIndex].updateSmartDashboard(); } - public void setDriveBaseSpeed(double driveBaseSpeed) { - this.driveBaseSpeed = driveBaseSpeed; - } - - // Normal Arcade Drive - public void arcadeDrive() { - differentialDrive.arcadeDrive( - driverJoystick.getRawAxis(Constants.leftJoystickY) * -driveBaseSpeed, - driverJoystick.getRawAxis(Constants.rightJoystickX) * Constants.driveBaseTurnRate, - true); - // joystick has y-axis flipped so up is negative, multiply by negative to accomodate this - } - - // Arcade Drive where you can only move forwards and backwards for testing - public void arcadeDrive(double rotation) { - differentialDrive.arcadeDrive( - driverJoystick.getRawAxis(Constants.leftJoystickY) * -driveBaseSpeed, rotation); - } - - // TODO: Make a command to switch modes (extra) - - // tank drive, not used but good to have - // TODO: check tankdrive if joystick axes are working - public void tankDrive() { - differentialDrive.tankDrive( - -1 * driverJoystick.getRawAxis(Constants.leftJoystickY), - -1 * driverJoystick.getRawAxis(Constants.rightJoystickY)); + public void arcadeDrive(double speed, double rotation) { + differentialDrive.arcadeDrive(speed, rotation); } @Override