From 46eea89d793fc1fee31cd8f84786695ec8897e17 Mon Sep 17 00:00:00 2001 From: zaneb Date: Fri, 4 Nov 2022 12:26:42 -0500 Subject: [PATCH 1/2] Implement OI class --- src/main/java/frc/robot/Constants.java | 50 +------- src/main/java/frc/robot/OI.java | 120 ++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 80 +++--------- .../java/frc/robot/commands/ClimbEnable.java | 51 -------- .../frc/robot/commands/ClimbPeriodic.java | 26 ++-- .../commands/DriveBaseTeleopCommand.java | 20 ++- .../frc/robot/subsystems/ClimbSubsystem.java | 91 +++++-------- .../robot/subsystems/DriveBaseSubsystem.java | 36 +----- 8 files changed, 206 insertions(+), 268 deletions(-) create mode 100644 src/main/java/frc/robot/OI.java delete mode 100644 src/main/java/frc/robot/commands/ClimbEnable.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 37097e82..c3bcfba0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -241,49 +241,8 @@ public String[] getPaths() { public static final boolean testMode = false; // if false CDS will eject balls of wrong color 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..fd0d2b35 --- /dev/null +++ b/src/main/java/frc/robot/OI.java @@ -0,0 +1,120 @@ +package frc.robot; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +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)); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aaa049cc..977edaf7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,12 +11,12 @@ 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.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; 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 +46,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 +67,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 +102,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 +117,15 @@ 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 +159,14 @@ 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,30 +177,18 @@ 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( + OI.Driver.getShootButton().whileHeld( shooterHeldLow.beforeStarting( () -> { shooterSubsystem.setAimMode(Constants.AimModes.LOW); @@ -232,15 +197,12 @@ private void configureButtonBindings() { } 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..2afc67c4 100644 --- a/src/main/java/frc/robot/commands/ClimbPeriodic.java +++ b/src/main/java/frc/robot/commands/ClimbPeriodic.java @@ -4,33 +4,37 @@ package frc.robot.commands; +import java.util.function.Supplier; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ClimbSubsystem; 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..5b197841 100644 --- a/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java +++ b/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java @@ -4,16 +4,22 @@ package frc.robot.commands; +import java.util.function.Supplier; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DriveBaseSubsystem; 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 +27,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..7547a16b 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -21,7 +21,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 +28,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 +39,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 +113,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 +223,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 +238,49 @@ 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); - // } + public void midClimb(double axisValue) { + if (climbEnabled) { + if (axisValue > 0) { + if (armHeightOne + (axisValue * Constants.armUpSpeed) >= Constants.armHeightMin) { + armHeightOne = armHeightOne + (axisValue * 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); - // } + if (armHeightTwo + (axisValue * Constants.armUpSpeed) >= Constants.armHeightMin) { + armHeightTwo = armHeightTwo + (axisValue * Constants.armUpSpeed); } } - if (armJoystickAxis < 0) { - if (armHeightOne + (armJoystickAxis * Constants.armDownSpeed) <= Constants.armHeightMax) { - armHeightOne = armHeightOne + (armJoystickAxis * Constants.armDownSpeed); + if (axisValue < 0) { + if (armHeightOne + (axisValue * Constants.armDownSpeed) <= Constants.armHeightMax) { + armHeightOne = armHeightOne + (axisValue * Constants.armDownSpeed); } - if (armHeightTwo + (armJoystickAxis * Constants.armDownSpeed) <= Constants.armHeightMax) { - armHeightTwo = armHeightTwo + (armJoystickAxis * 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); } - } - 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); + public void highArms(double axisValue) { + if (axisValue > 0) { + if (poleHeightOne + (axisValue * Constants.poleInSpeed) <= Constants.poleHeightMax) { + poleHeightOne = poleHeightOne + (axisValue * Constants.poleInSpeed); } - if (poleHeightTwo + (poleJoystickAxis * Constants.poleInSpeed) <= Constants.poleHeightMax) { - poleHeightTwo = poleHeightTwo + (poleJoystickAxis * Constants.poleInSpeed); + if (poleHeightTwo + (axisValue * Constants.poleInSpeed) <= Constants.poleHeightMax) { + poleHeightTwo = poleHeightTwo + (axisValue * Constants.poleInSpeed); } } - if (poleJoystickAxis < 0) { - if (poleHeightOne + (poleJoystickAxis * Constants.poleOutSpeed) + if (axisValue < 0) { + if (poleHeightOne + (axisValue * Constants.poleOutSpeed) >= Constants.poleHeightMin) { - poleHeightOne = poleHeightOne + (poleJoystickAxis * Constants.poleOutSpeed); + poleHeightOne = poleHeightOne + (axisValue * Constants.poleOutSpeed); } - if (poleHeightTwo + (poleJoystickAxis * Constants.poleOutSpeed) + if (poleHeightTwo + (axisValue * Constants.poleOutSpeed) >= Constants.poleHeightMin) { - poleHeightTwo = poleHeightTwo + (poleJoystickAxis * Constants.poleOutSpeed); + poleHeightTwo = poleHeightTwo + (axisValue * Constants.poleOutSpeed); } } - } poleOne.getPIDCtrl().setReference(poleHeightOne, CANSparkMax.ControlType.kPosition); - poleTwo.getPIDCtrl().setReference(poleHeightOne, CANSparkMax.ControlType.kPosition); } @@ -385,8 +358,8 @@ public boolean atFirstSetpoint() { } public void periodic() { - if (DriverStation.isDisabled() && climbEnable) { - climbEnable(); + if (DriverStation.isDisabled() && climbEnabled) { + toggleEnabled(); } if (DriverStation.isDisabled()) { @@ -396,7 +369,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 From 65f37d5a75e3ae40691d759e8577354d2398007b Mon Sep 17 00:00:00 2001 From: zaneb Date: Fri, 4 Nov 2022 16:02:28 -0500 Subject: [PATCH 2/2] spotless --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/OI.java | 185 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 32 +-- .../frc/robot/commands/ClimbPeriodic.java | 8 +- .../commands/DriveBaseTeleopCommand.java | 9 +- .../frc/robot/subsystems/ClimbSubsystem.java | 63 +++--- 6 files changed, 153 insertions(+), 148 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c3bcfba0..e96bdce0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -241,8 +241,8 @@ public String[] getPaths() { public static final boolean testMode = false; // if false CDS will eject balls of wrong color public static final double reverseStopperWheelSpeed = -0.125; - - //ShooterConstants + + // ShooterConstants public static final class Shooter { // Motor IDs public static final int shooterID = 10; // ID of the shooter diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index fd0d2b35..78c86b8b 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -1,120 +1,121 @@ package frc.robot; -import java.util.function.Supplier; - 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; + // 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); + 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 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 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); + 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 getDriveSpeedSupplier() { + return () -> kDriveSpeedCurve.calculate(-kJoystick.getRawAxis(kDriveSpeedAxis)); + } - public static Supplier getDriveRotationSupplier(){ - return () -> kDriveRotationCurve.calculate(kJoystick.getRawAxis(kDriveRotationAxis)); - } + public static Supplier getDriveRotationSupplier() { + return () -> kDriveRotationCurve.calculate(kJoystick.getRawAxis(kDriveRotationAxis)); + } - public static JoystickButton getIntakeButton(){ - return new JoystickButton(kJoystick, kIntakeButtonID); - } + public static JoystickButton getIntakeButton() { + return new JoystickButton(kJoystick, kIntakeButtonID); + } - public static JoystickButton getOuttakeButton(){ - return new JoystickButton(kJoystick, kOuttakeButtonID); - } + public static JoystickButton getOuttakeButton() { + return new JoystickButton(kJoystick, kOuttakeButtonID); + } - public static JoystickButton getShootButton(){ - return new JoystickButton(kJoystick, kShootButtonID); - } + public static JoystickButton getShootButton() { + return new JoystickButton(kJoystick, kShootButtonID); } + } - public static final class Operator{ - private static final Joystick kJoystick = new Joystick(OI.kOperatorJoystickPort); + 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 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 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); + 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 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 Supplier getClimbPoleSupplier() { + return () -> kClimbPoleCurve.calculate(-kJoystick.getRawAxis(kClimbPoleAxis)); + } - public static JoystickButton getAutoClimbButton(){ - return new JoystickButton(kJoystick, kAutoClimbButtonID); - } + public static JoystickButton getEnableClimbButton() { + return new JoystickButton(kJoystick, kEnableClimbButtonID); + } - public static JoystickButton getCDSForwardButton(){ - return new JoystickButton(kJoystick, kCDSForwardButtonID); - } + public static JoystickButton getAutoClimbButton() { + return new JoystickButton(kJoystick, kAutoClimbButtonID); + } - public static JoystickButton getOuttakeButton(){ - return new JoystickButton(kJoystick, kOuttakeButtonID); - } + 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 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)); - } + 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)); } -} \ No newline at end of file + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 977edaf7..9fd3e282 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,14 +5,11 @@ 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.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.Auton; import frc.robot.commands.AutonModes; import frc.robot.commands.CDSBallManagementCommand; @@ -123,9 +120,11 @@ private void initSubsystems() { private void initCommands() { // Initializes commands based on enabled subsystems if (driveBaseSubsystem != null) { - driveBaseTeleopCommand = new DriveBaseTeleopCommand(driveBaseSubsystem, - OI.Driver.getDriveSpeedSupplier(), - OI.Driver.getDriveRotationSupplier()); + driveBaseTeleopCommand = + new DriveBaseTeleopCommand( + driveBaseSubsystem, + OI.Driver.getDriveSpeedSupplier(), + OI.Driver.getDriveRotationSupplier()); driveBaseSubsystem.setDefaultCommand(driveBaseTeleopCommand); } if (cdsSubsystem != null && shooterSubsystem != null) { @@ -160,9 +159,11 @@ private void initCommands() { if ((climbSubsystem != null) && (driveBaseSubsystem != null)) { climbEnable = new InstantCommand(climbSubsystem::toggleEnabled, climbSubsystem); - climbPeriodic = new ClimbPeriodic(climbSubsystem, - OI.Operator.getClimbArmSupplier(), - OI.Operator.getClimbPoleSupplier()); + climbPeriodic = + new ClimbPeriodic( + climbSubsystem, + OI.Operator.getClimbArmSupplier(), + OI.Operator.getClimbPoleSupplier()); climbSequence1 = new ClimbSequence1(climbSubsystem); hookUnlock = new HookUnlock(climbSubsystem); hookLock = new HookLock(climbSubsystem); @@ -188,12 +189,13 @@ private void configureButtonBindings() { } if (shooterSubsystem != null && shooterHeldLow != null && shooterHeldAuto != null) { - OI.Driver.getShootButton().whileHeld( - shooterHeldLow.beforeStarting( - () -> { - shooterSubsystem.setAimMode(Constants.AimModes.LOW); - }, - shooterSubsystem)); + OI.Driver.getShootButton() + .whileHeld( + shooterHeldLow.beforeStarting( + () -> { + shooterSubsystem.setAimMode(Constants.AimModes.LOW); + }, + shooterSubsystem)); } if (climbSubsystem != null) { diff --git a/src/main/java/frc/robot/commands/ClimbPeriodic.java b/src/main/java/frc/robot/commands/ClimbPeriodic.java index 2afc67c4..addeccf4 100644 --- a/src/main/java/frc/robot/commands/ClimbPeriodic.java +++ b/src/main/java/frc/robot/commands/ClimbPeriodic.java @@ -4,10 +4,9 @@ package frc.robot.commands; -import java.util.function.Supplier; - 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 climbSubsystem; @@ -15,7 +14,10 @@ public class ClimbPeriodic extends CommandBase { private final Supplier climbPoleSupplier; /** Creates a new ClimbKeepDown. */ - public ClimbPeriodic(ClimbSubsystem climbSubsystem, Supplier climbArmSupplier, Supplier climbPoleSupplier) { + public ClimbPeriodic( + ClimbSubsystem climbSubsystem, + Supplier climbArmSupplier, + Supplier climbPoleSupplier) { addRequirements(climbSubsystem); this.climbSubsystem = climbSubsystem; this.climbArmSupplier = climbArmSupplier; diff --git a/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java b/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java index 5b197841..c5baf2f3 100644 --- a/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java +++ b/src/main/java/frc/robot/commands/DriveBaseTeleopCommand.java @@ -4,18 +4,21 @@ package frc.robot.commands; -import java.util.function.Supplier; - 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 driveBaseSubsystem; + private final Supplier speedSupplier; private final Supplier rotationSupplier; - public DriveBaseTeleopCommand(DriveBaseSubsystem driveBaseSubsystem, Supplier speedSupplier, Supplier rotationSupplier) { + public DriveBaseTeleopCommand( + DriveBaseSubsystem driveBaseSubsystem, + Supplier speedSupplier, + Supplier rotationSupplier) { addRequirements(driveBaseSubsystem); this.driveBaseSubsystem = driveBaseSubsystem; this.speedSupplier = speedSupplier; diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 7547a16b..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; @@ -240,46 +239,44 @@ public void toggleEnabled() { public void midClimb(double axisValue) { if (climbEnabled) { - if (axisValue > 0) { - if (armHeightOne + (axisValue * Constants.armUpSpeed) >= Constants.armHeightMin) { - armHeightOne = armHeightOne + (axisValue * Constants.armUpSpeed); - } - 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); - } - - public void highArms(double axisValue) { if (axisValue > 0) { - if (poleHeightOne + (axisValue * Constants.poleInSpeed) <= Constants.poleHeightMax) { - poleHeightOne = poleHeightOne + (axisValue * Constants.poleInSpeed); + if (armHeightOne + (axisValue * Constants.armUpSpeed) >= Constants.armHeightMin) { + armHeightOne = armHeightOne + (axisValue * Constants.armUpSpeed); } - if (poleHeightTwo + (axisValue * Constants.poleInSpeed) <= Constants.poleHeightMax) { - poleHeightTwo = poleHeightTwo + (axisValue * Constants.poleInSpeed); + if (armHeightTwo + (axisValue * Constants.armUpSpeed) >= Constants.armHeightMin) { + armHeightTwo = armHeightTwo + (axisValue * Constants.armUpSpeed); } } if (axisValue < 0) { - if (poleHeightOne + (axisValue * Constants.poleOutSpeed) - >= Constants.poleHeightMin) { - poleHeightOne = poleHeightOne + (axisValue * Constants.poleOutSpeed); + if (armHeightOne + (axisValue * Constants.armDownSpeed) <= Constants.armHeightMax) { + armHeightOne = armHeightOne + (axisValue * Constants.armDownSpeed); } - if (poleHeightTwo + (axisValue * Constants.poleOutSpeed) - >= Constants.poleHeightMin) { - poleHeightTwo = poleHeightTwo + (axisValue * Constants.poleOutSpeed); + 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); + } + + public void highArms(double axisValue) { + if (axisValue > 0) { + if (poleHeightOne + (axisValue * Constants.poleInSpeed) <= Constants.poleHeightMax) { + poleHeightOne = poleHeightOne + (axisValue * Constants.poleInSpeed); + } + 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); }