From f45b21d624ffc1b835767c7e7c4d19f165a3a0fd Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 4 Mar 2022 00:16:02 -0600 Subject: [PATCH 1/3] fixed climb buttons --- .gradle/file-system.probe | Bin 8 -> 8 bytes networktables.ini | 4 ++ networktables.ini.bak | 16 ++++++++ simgui-window.json | 4 +- simgui.json | 1 + src/main/java/frc/robot/AutonSetup.java | 27 ++++++++----- src/main/java/frc/robot/Gamepads.java | 4 ++ src/main/java/frc/robot/Robot.java | 6 ++- .../frc/robot/commands/ClimberCommands.java | 5 ++- .../java/frc/robot/commands/swerve/LLAim.java | 2 +- .../frc/robot/constants/AutonConstants.java | 4 ++ .../frc/robot/constants/ClimberConstants.java | 12 +++--- .../java/frc/robot/subsystems/Climber.java | 36 +++++++++++++----- .../java/frc/robot/subsystems/VisionLL.java | 4 +- .../telemetry/shuffleboard/MainTelemetry.java | 5 ++- 15 files changed, 95 insertions(+), 35 deletions(-) diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index b3491a4eb84c368ec62c4301744791faa571cab8..5829690635e9c2fa9a1ea703a0c39d2a739ed6f4 100644 GIT binary patch literal 8 PcmZQzV5|?eIw1f61|tFH literal 8 PcmZQzV5|?kG>sJi2Xq2I diff --git a/networktables.ini b/networktables.ini index 171e179..c26ca57 100644 --- a/networktables.ini +++ b/networktables.ini @@ -1,5 +1,9 @@ [NetworkTables Storage 3.0] string "/Preferences/.type"="RobotPreferences" +double "/Preferences/LL-AIM Tolerance"=1 +double "/Preferences/LL-AIM kD"=0 +double "/Preferences/LL-AIM kI"=0 +double "/Preferences/LL-AIM kP"=0.8 double "/Preferences/Swerve: Theta kD"=0 double "/Preferences/Swerve: Theta kP"=1000 double "/Preferences/Swerve: X kD"=20 diff --git a/networktables.ini.bak b/networktables.ini.bak index 74cd3a3..171e179 100644 --- a/networktables.ini.bak +++ b/networktables.ini.bak @@ -6,7 +6,15 @@ double "/Preferences/Swerve: X kD"=20 double "/Preferences/Swerve: X kP"=2300 double "/Preferences/Swerve: Y kD"=20 double "/Preferences/Swerve: Y kP"=2300 +double "/Shuffleboard/Climber/Climber Config/Acceleration"=0 +double "/Shuffleboard/Climber/Climber Config/Closed Loop Ramp Rate"=0 +double "/Shuffleboard/Climber/Climber Config/CruiseVelocity"=0 +double "/Shuffleboard/Climber/Climber Config/Open Loop Ramp Rate"=0 +double "/Shuffleboard/Climber/Climber Config/kD"=0 double "/Shuffleboard/Climber/Climber Config/kF"=0.05 +double "/Shuffleboard/Climber/Climber Config/kI"=0 +double "/Shuffleboard/Climber/Climber Config/kIzone"=150 +double "/Shuffleboard/Climber/Climber Config/kP"=0 double "/Shuffleboard/Gamepads/Driver 0/xLeftDeadband"=0.1 double "/Shuffleboard/Gamepads/Driver 0/xLeftExpVal"=1 double "/Shuffleboard/Gamepads/Driver 0/xLeftScalar"=1 @@ -34,4 +42,12 @@ double "/Shuffleboard/Gamepads/Operator 1/yRightScalar"=1 double "/Shuffleboard/Launcher/Launcher Config/kF"=0.0609999 double "/Shuffleboard/Launcher/Launcher Config/kI"=0.000499964 double "/Shuffleboard/Launcher/Launcher Config/kP"=0.0465 +double "/Shuffleboard/Swerve/Drive Mod 0/Acceleration"=0 +double "/Shuffleboard/Swerve/Drive Mod 0/Closed Loop Ramp Rate"=0 +double "/Shuffleboard/Swerve/Drive Mod 0/CruiseVelocity"=0 +double "/Shuffleboard/Swerve/Drive Mod 0/Open Loop Ramp Rate"=0.25575 +double "/Shuffleboard/Swerve/Drive Mod 0/kD"=0 +double "/Shuffleboard/Swerve/Drive Mod 0/kF"=0 +double "/Shuffleboard/Swerve/Drive Mod 0/kI"=0 +double "/Shuffleboard/Swerve/Drive Mod 0/kIzone"=0 double "/Shuffleboard/Swerve/Drive Mod 0/kP"=0.0999999 diff --git a/simgui-window.json b/simgui-window.json index 627985a..69e4a20 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -13,8 +13,8 @@ "Window": { "###/SmartDashboard/Field": { "Collapsed": "0", - "Pos": "300,46", - "Size": "560,342" + "Pos": "274,46", + "Size": "586,378" }, "###FMS": { "Collapsed": "0", diff --git a/simgui.json b/simgui.json index 8551aef..525861c 100644 --- a/simgui.json +++ b/simgui.json @@ -17,6 +17,7 @@ "types": { "/FMSInfo": "FMSInfo", "/Shuffleboard/Main/SendableChooser[0]": "String Chooser", + "/Shuffleboard/Main/SendableChooser[1]": "String Chooser", "/SmartDashboard/Field": "Field2d" }, "windows": { diff --git a/src/main/java/frc/robot/AutonSetup.java b/src/main/java/frc/robot/AutonSetup.java index ee4586e..ca79251 100644 --- a/src/main/java/frc/robot/AutonSetup.java +++ b/src/main/java/frc/robot/AutonSetup.java @@ -5,27 +5,34 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import frc.robot.commands.auto.RightDoubleBall; import frc.robot.commands.auto.TestPathPlanner; +import frc.robot.constants.AutonConstants; public class AutonSetup { - // A simple auto routine that drives forward a specified distance, and then - // stops. - private static final Command simpleAuto = new PrintCommand("TEST SIMPLE AUTO"); + //Positions + public static final SendableChooser posChooser = new SendableChooser<>(); - // A complex auto routine that drives forward, drops a hatch, and then drives - // backward. + //AutoRoutines private static final Command complexAuto = new PrintCommand("TEST COMPLEX AUTO"); - - private static final Command testPathPlanner = new RightDoubleBall(); + private static final Command DoubleBall = new RightDoubleBall(); // A chooser for autonomous commands public static final SendableChooser chooser = new SendableChooser<>(); - public static void setupSelector() { - chooser.setDefaultOption("Simple Auto", simpleAuto); + public static void setupSelectors() { + posChooser.setDefaultOption("1. Left", AutonConstants.pos1angle); + posChooser.addOption("2. Middle", AutonConstants.pos2angle); + posChooser.addOption("3. Right", AutonConstants.pos3angle); + + chooser.setDefaultOption("DoubleBall", DoubleBall); chooser.addOption("Complex Auto", complexAuto); - chooser.addOption("Test Path Planner", testPathPlanner); } + + //Return the starting angle for each position + public static double getAutonAngle(){ + return posChooser.getSelected(); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/Gamepads.java b/src/main/java/frc/robot/Gamepads.java index e946646..eac514b 100644 --- a/src/main/java/frc/robot/Gamepads.java +++ b/src/main/java/frc/robot/Gamepads.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.gamepads.AndButton; import frc.lib.gamepads.AndNotButton; +import frc.lib.gamepads.AxisButton; import frc.lib.gamepads.XboxGamepad; import frc.lib.util.Alert; import frc.robot.Robot.RobotState; @@ -125,6 +126,9 @@ public static void operatorBindings() { //Manual climb tilt control operator.leftBumper.whileHeld(ClimberCommands.tiltUp()); + + //Return to default command when pressed + operator.leftStickButton.whileHeld(Robot.climber.defaultCommand()); } // Configure the button bindings for the driver control in Test Mode diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a6a222f..6e87c25 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -173,7 +173,11 @@ public void autonomousInit() { LiveWindow.disableAllTelemetry(); // schedule the autonomous command (example) + visionLL.limeLightLEDOn(); m_autonomousCommand = AutonSetup.getAutonomousCommand(); + double angle = AutonSetup.getAutonAngle(); + Robot.swerve.resetGyro(angle); + if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } @@ -271,7 +275,7 @@ private static String getMACaddress() { } MAC = String.join(":", hexadecimal); return MAC; - } catch (UnknownHostException | SocketException e) { + } catch (UnknownHostException | SocketException | NullPointerException e) { e.printStackTrace(); } return ""; diff --git a/src/main/java/frc/robot/commands/ClimberCommands.java b/src/main/java/frc/robot/commands/ClimberCommands.java index 44da6ca..42496d9 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands.java +++ b/src/main/java/frc/robot/commands/ClimberCommands.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot; import frc.robot.constants.ClimberConstants; @@ -17,7 +18,7 @@ public class ClimberCommands { //Put the climber fully up, used to grab the mid bar public static Command fullUp(){ - return tiltUp().alongWith(extendFull()); + return extendFull().alongWith(new WaitCommand(0.5).andThen(tiltUp())); } //Put the climber partially out and tilted up, used to grab the next rung @@ -27,7 +28,7 @@ public static Command nextRungUp(){ //pull the climber down, used to climb to mid bar, and from rung to rung public static Command climb(){ - return tiltDown().alongWith(pull()); + return pull().alongWith(new WaitCommand(0.1).andThen(tiltDown())); } public static Command extendFull(){ diff --git a/src/main/java/frc/robot/commands/swerve/LLAim.java b/src/main/java/frc/robot/commands/swerve/LLAim.java index 64db570..9075442 100644 --- a/src/main/java/frc/robot/commands/swerve/LLAim.java +++ b/src/main/java/frc/robot/commands/swerve/LLAim.java @@ -35,7 +35,7 @@ public LLAim() { @Override public void initialize() { - SmartDashboard.putNumber("/limelight/ledMode", 3); + Robot.visionLL.limeLightLEDOn(); kP = SpectrumPreferences.getNumber("LL-AIM kP", kP)/100; kI = SpectrumPreferences.getNumber("LL-AIM kI", kI)/100; kD = SpectrumPreferences.getNumber("LL-AIM kD", kD)/100; diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index 9cb6dc0..f32233b 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -3,6 +3,10 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; public final class AutonConstants { + public static final double pos1angle = 92; + public static final double pos2angle = 120; + public static final double pos3angle = 270; + public static final double kMaxSpeedMetersPerSecond = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java index 207588f..68e5f55 100644 --- a/src/main/java/frc/robot/constants/ClimberConstants.java +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -13,9 +13,9 @@ public final class ClimberConstants{ public static final String name = "Climber"; - public static final int fullExtend = 100000; - public static final int nextRungExtend = 6000; - public static final int fullRetract = -2000; + public static final int fullExtend = 112000; + public static final int nextRungExtend = 94000; + public static final int fullRetract = -500; //Physical Constants public static final double pulleyDiameterInches = 2; @@ -42,13 +42,13 @@ public final class ClimberConstants{ public static final NeutralMode kNeutralMode = NeutralMode.Brake; /* Control Loop Constants */ - public static final double kP = 0.0; + public static final double kP = 0.01; public static final double kI = 0; public static final double kD = 0; public static final double kF = 0.05; public static final double kIz = 150; - public static final double motionCruiseVelocity = 0; - public static final double motionAcceleration = 0; + public static final double motionCruiseVelocity = 10000; + public static final double motionAcceleration = 20000; /* Current Limiting */ public static final int currentLimit = 40; diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index eac2696..d9b34ed 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -8,37 +8,53 @@ import frc.robot.constants.Constants.CanIDs; import frc.robot.constants.Constants.SolenoidPorts; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; -public class Climber extends PositionSubsystem{ +public class Climber extends PositionSubsystem { public final WPI_TalonFX motorFollower; public final SolenoidSubsystem pneumatic; - public Climber(){ + public Climber() { setName(ClimberConstants.name); motorLeader = new WPI_TalonFX(CanIDs.kClimberMotorLeft, Constants.Canivorename); ClimberConstants.setupFalconLeader(motorLeader); motorFollower = new WPI_TalonFX(CanIDs.kClimberMotorRight, Constants.Canivorename); ClimberConstants.setupFalconFollower(motorFollower, motorLeader); - + resetEncoder(); + motorLeader.configForwardSoftLimitThreshold(ClimberConstants.fullExtend); + motorLeader.configForwardSoftLimitEnable(true); + + motorLeader.configReverseSoftLimitThreshold(ClimberConstants.fullRetract); + motorLeader.configReverseSoftLimitEnable(true); pneumatic = new SolenoidSubsystem("Climber Solenoid", SolenoidPorts.kclimberUp); - - setDefaultCommand(new RunCommand(() -> setManualOutput(Gamepads.getClimberJoystick()), this)); + + setDefaultCommand(defaultCommand()); } - public void follow(){ + public Command defaultCommand(){ + return new RunCommand(() -> setManualOutput(Gamepads.getClimberJoystick()), this); + } + public void follow() { + motorFollower.setNeutralMode(NeutralMode.Brake); motorFollower.follow(motorLeader); } - public void tiltUp(){ + + public void tiltUp() { + motorLeader.setNeutralMode(NeutralMode.Brake); + motorFollower.setNeutralMode(NeutralMode.Brake); pneumatic.on(); } - public void tiltDown(){ - pneumatic.off(); - } + public void tiltDown() { + motorLeader.setNeutralMode(NeutralMode.Brake); + motorFollower.setNeutralMode(NeutralMode.Brake); + pneumatic.off(); + } } diff --git a/src/main/java/frc/robot/subsystems/VisionLL.java b/src/main/java/frc/robot/subsystems/VisionLL.java index 212afd6..176ffed 100644 --- a/src/main/java/frc/robot/subsystems/VisionLL.java +++ b/src/main/java/frc/robot/subsystems/VisionLL.java @@ -44,7 +44,7 @@ public void periodic() { // This method will be called once per scheduler run // If disabled and LED-Toggle is false, than leave lights off, else they should // be on - if (Robot.s_robot_state == RobotState.DISABLED && !MainTelemetry.getLimeLightToggle() + /*if (Robot.s_robot_state == RobotState.DISABLED && !MainTelemetry.getLimeLightToggle() && !DriverStation.isFMSAttached()) { if (LEDState == true) { limeLightLEDOff(); @@ -55,7 +55,7 @@ public void periodic() { limeLightLEDOn(); LEDState = true; } - } + }*/ TargetAngle = limelight.getdegVerticalToTarget(); Distance = ((TargetHeight - LLHeight) / Math.tan(Math.toRadians(LLAngle + TargetAngle))); diff --git a/src/main/java/frc/robot/telemetry/shuffleboard/MainTelemetry.java b/src/main/java/frc/robot/telemetry/shuffleboard/MainTelemetry.java index 03fcc58..f5d2fe4 100644 --- a/src/main/java/frc/robot/telemetry/shuffleboard/MainTelemetry.java +++ b/src/main/java/frc/robot/telemetry/shuffleboard/MainTelemetry.java @@ -35,6 +35,7 @@ public class MainTelemetry { public static SimpleWidget m_limelightLEDenable; public static SimpleWidget m_enableTabsWidget; public static ComplexWidget m_autonSelectorWidget; + public static ComplexWidget m_autonPositionWidget; // --------------// // Constructor // @@ -60,9 +61,11 @@ public void initialize() { m_tab.addNumber("Target Distance", () -> Robot.visionLL.getActualDistance()).withPosition(2, 2); m_enableTabsWidget = m_tab.add("Update Enable", false).withWidget(BuiltInWidgets.kToggleButton).withPosition(3, 0).withSize(1, 1); - AutonSetup.setupSelector(); + AutonSetup.setupSelectors(); m_autonSelectorWidget = m_tab.add(AutonSetup.chooser).withPosition(4, 0).withSize(3, 1) .withProperties(Map.of("Title", "Choose Auton", "title", "Choose Auton")); + m_autonPositionWidget = m_tab.add(AutonSetup.posChooser).withPosition(4, 1).withSize(3, 1) + .withProperties(Map.of("Name", "Auton Positio", "name", " Auton Position")); } // Match Time From 49de5f4d8fba40d5073ab93dbc925b1fb2150593 Mon Sep 17 00:00:00 2001 From: Allen Gregory Date: Sat, 5 Mar 2022 12:46:54 -0600 Subject: [PATCH 2/3] morning added driver commands for single controllers, fixed some ball path and climb things --- .gradle/file-system.probe | Bin 8 -> 8 bytes src/main/java/frc/robot/Gamepads.java | 5 +- .../frc/robot/commands/ClimberCommands.java | 59 ++++++++++-------- .../commands/ballpath/BallPathCommands.java | 4 ++ .../frc/robot/constants/ClimberConstants.java | 15 +++-- .../java/frc/robot/subsystems/Climber.java | 4 +- 6 files changed, 54 insertions(+), 33 deletions(-) diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 5829690635e9c2fa9a1ea703a0c39d2a739ed6f4..55b0dadaf9dfab9edb56b23fe38fd56b373b37ed 100644 GIT binary patch literal 8 PcmZQzV62al)ye_@1qK0( literal 8 PcmZQzV5|?eIw1f61|tFH diff --git a/src/main/java/frc/robot/Gamepads.java b/src/main/java/frc/robot/Gamepads.java index eac514b..21d62f3 100644 --- a/src/main/java/frc/robot/Gamepads.java +++ b/src/main/java/frc/robot/Gamepads.java @@ -91,6 +91,9 @@ public static void driverBindings() { new AndNotButton(driver.aButton, driver.leftBumper).whileHeld(BallPathCommands.intakeBalls()); new AndNotButton(driver.yButton, driver.leftBumper).whileHeld(BallPathCommands.eject()); + new AndNotButton(driver.xButton, driver.leftBumper).whileHeld(BallPathCommands.feed()); + driver.startButton.whenPressed(BallPathCommands.tarmacShot()); + driver.selectButton.whenPressed(BallPathCommands.stopLauncher()); // Aim with limelight driver.rightBumper.whileHeld(new LLAim()); @@ -121,7 +124,7 @@ public static void operatorBindings() { // Climber Controls operator.Dpad.Up.whenPressed(ClimberCommands.fullUp()); operator.Dpad.Down.whenPressed(ClimberCommands.climb()); - operator.Dpad.Left.whenPressed(ClimberCommands.extendNextRung()); + operator.Dpad.Left.whenPressed(ClimberCommands.nextRungDown()); operator.Dpad.Right.whenPressed(ClimberCommands.nextRungUp()); //Manual climb tilt control diff --git a/src/main/java/frc/robot/commands/ClimberCommands.java b/src/main/java/frc/robot/commands/ClimberCommands.java index 42496d9..a59c401 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands.java +++ b/src/main/java/frc/robot/commands/ClimberCommands.java @@ -1,6 +1,5 @@ package frc.robot.commands; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -8,51 +7,61 @@ import frc.robot.constants.ClimberConstants; public class ClimberCommands { - /* Climber Sequence - * 1. fullUp - * 2. climb - * 3. nextRungExtend - * 4. nextRungUp - * Repeat 2-4. - */ - - //Put the climber fully up, used to grab the mid bar - public static Command fullUp(){ + /* + * Climber Sequence + * 1. fullUp + * 2. climb + * 3. nextRungExtend + * 4. nextRungUp + * Repeat 2-4. + */ + + // Put the climber fully up, used to grab the mid bar + public static Command fullUp() { return extendFull().alongWith(new WaitCommand(0.5).andThen(tiltUp())); } - //Put the climber partially out and tilted up, used to grab the next rung - public static Command nextRungUp(){ + // Put the climber partially out and tilted up, used to grab the next rung + public static Command nextRungUp() { return tiltUp().alongWith(extendNextRung()); } - //pull the climber down, used to climb to mid bar, and from rung to rung - public static Command climb(){ + public static Command nextRungDown(){ + return extendNextRung().alongWith(releaseRung()); + } + + // pull the climber down, used to climb to mid bar, and from rung to rung + public static Command climb() { return pull().alongWith(new WaitCommand(0.1).andThen(tiltDown())); } - public static Command extendFull(){ + // release rung + public static Command releaseRung() { + return tiltUp().withTimeout(0.2).andThen(tiltDown()); + } + + public static Command extendFull() { return toPosition(ClimberConstants.fullExtend); } - //Used to extend before tilting up to grab the next rung - public static Command extendNextRung(){ + // Used to extend before tilting up to grab the next rung + public static Command extendNextRung() { return toPosition(ClimberConstants.nextRungExtend); } - public static Command pull(){ + public static Command pull() { return toPosition(ClimberConstants.fullRetract); } - public static Command toPosition(double position){ - return new RunCommand(()-> Robot.climber.setMMPosition(position), Robot.climber); + public static Command toPosition(double position) { + return new RunCommand(() -> Robot.climber.setMMPosition(position), Robot.climber); } - public static Command tiltUp(){ - return new RunCommand(()-> Robot.climber.tiltUp(), Robot.climber.pneumatic); + public static Command tiltUp() { + return new RunCommand(() -> Robot.climber.tiltUp(), Robot.climber.pneumatic); } - public static Command tiltDown(){ - return new RunCommand(()-> Robot.climber.tiltDown(), Robot.climber.pneumatic); + public static Command tiltDown() { + return new RunCommand(() -> Robot.climber.tiltDown(), Robot.climber.pneumatic); } } diff --git a/src/main/java/frc/robot/commands/ballpath/BallPathCommands.java b/src/main/java/frc/robot/commands/ballpath/BallPathCommands.java index db51596..9ecb461 100644 --- a/src/main/java/frc/robot/commands/ballpath/BallPathCommands.java +++ b/src/main/java/frc/robot/commands/ballpath/BallPathCommands.java @@ -62,6 +62,10 @@ public static Command runFeeder(double speed) { return new StartEndCommand(() -> Robot.feeder.setManualOutput(speed), () -> Robot.feeder.stop(), Robot.feeder); } + public static Command stopLauncher(){ + return new RunCommand(() -> Robot.launcher.setManualOutput(0.0), Robot.launcher); + } + // Run launcher motor public static Command runLauncher(double speed) { return new RunCommand(() -> Robot.launcher.setManualOutput(speed), Robot.launcher); diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java index 68e5f55..98434bb 100644 --- a/src/main/java/frc/robot/constants/ClimberConstants.java +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.sensors.SensorInitializationStrategy; @@ -14,8 +15,8 @@ public final class ClimberConstants{ public static final String name = "Climber"; public static final int fullExtend = 112000; - public static final int nextRungExtend = 94000; - public static final int fullRetract = -500; + public static final int nextRungExtend = 98000; + public static final int fullRetract = -2000; //Physical Constants public static final double pulleyDiameterInches = 2; @@ -51,13 +52,16 @@ public final class ClimberConstants{ public static final double motionAcceleration = 20000; /* Current Limiting */ - public static final int currentLimit = 40; - public static final int tirggerThresholdLimit = 45; - public static final double PeakCurrentDuration = 0.5; + public static final int currentLimit = 30; + public static final int tirggerThresholdLimit = 35; + public static final double PeakCurrentDuration = 0.3; public static final boolean EnableCurrentLimit = true; public static final SupplyCurrentLimitConfiguration supplyLimit = new SupplyCurrentLimitConfiguration( EnableCurrentLimit, currentLimit, tirggerThresholdLimit, PeakCurrentDuration); + public static final StatorCurrentLimitConfiguration statorLimit = new StatorCurrentLimitConfiguration( + EnableCurrentLimit, currentLimit, tirggerThresholdLimit, PeakCurrentDuration); + /* Voltage Compensation */ public static final double voltageCompSaturation = 12; @@ -84,6 +88,7 @@ private ClimberConstants(){ config.motionAcceleration = motionAcceleration; config.supplyCurrLimit = supplyLimit; + config.statorCurrLimit = statorLimit; config.openloopRamp = openLoopRamp; config.closedloopRamp = closedLoopRamp; config.voltageCompSaturation = voltageCompSaturation; diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index d9b34ed..8a4bf72 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -28,10 +28,10 @@ public Climber() { resetEncoder(); motorLeader.configForwardSoftLimitThreshold(ClimberConstants.fullExtend); - motorLeader.configForwardSoftLimitEnable(true); + motorLeader.configForwardSoftLimitEnable(false); motorLeader.configReverseSoftLimitThreshold(ClimberConstants.fullRetract); - motorLeader.configReverseSoftLimitEnable(true); + motorLeader.configReverseSoftLimitEnable(false); pneumatic = new SolenoidSubsystem("Climber Solenoid", SolenoidPorts.kclimberUp); From f05f2508900b7d74463fde1363c61cc06d7c83c2 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Tue, 8 Mar 2022 16:13:17 -0600 Subject: [PATCH 3/3] updates from Dripping Springs --- .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/constants/AutonConstants.java | 2 +- .../java/frc/robot/constants/ClimberConstants.java | 13 ++++++------- src/main/java/frc/robot/subsystems/Climber.java | 2 +- 4 files changed, 8 insertions(+), 9 deletions(-) diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 55b0dadaf9dfab9edb56b23fe38fd56b373b37ed..44091ab266b515f65734985b9b840f6f4ad38f70 100644 GIT binary patch literal 8 PcmZQzV62bdzNs1j2nhnj literal 8 PcmZQzV62al)ye_@1qK0( diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index f32233b..036f890 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -3,7 +3,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; public final class AutonConstants { - public static final double pos1angle = 92; + public static final double pos1angle = 160; public static final double pos2angle = 120; public static final double pos3angle = 270; diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java index 98434bb..6c574b6 100644 --- a/src/main/java/frc/robot/constants/ClimberConstants.java +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -14,7 +14,7 @@ public final class ClimberConstants{ public static final String name = "Climber"; - public static final int fullExtend = 112000; + public static final int fullExtend = 118000; public static final int nextRungExtend = 98000; public static final int fullRetract = -2000; @@ -52,15 +52,15 @@ public final class ClimberConstants{ public static final double motionAcceleration = 20000; /* Current Limiting */ - public static final int currentLimit = 30; - public static final int tirggerThresholdLimit = 35; - public static final double PeakCurrentDuration = 0.3; + public static final int currentLimit = 40; + public static final int tirggerThresholdLimit = 40; + public static final double PeakCurrentDuration = 0.5; public static final boolean EnableCurrentLimit = true; public static final SupplyCurrentLimitConfiguration supplyLimit = new SupplyCurrentLimitConfiguration( EnableCurrentLimit, currentLimit, tirggerThresholdLimit, PeakCurrentDuration); - public static final StatorCurrentLimitConfiguration statorLimit = new StatorCurrentLimitConfiguration( - EnableCurrentLimit, currentLimit, tirggerThresholdLimit, PeakCurrentDuration); + /*public static final StatorCurrentLimitConfiguration statorLimit = new StatorCurrentLimitConfiguration( + EnableCurrentLimit, currentLimit, tirggerThresholdLimit, PeakCurrentDuration);*/ /* Voltage Compensation */ public static final double voltageCompSaturation = 12; @@ -88,7 +88,6 @@ private ClimberConstants(){ config.motionAcceleration = motionAcceleration; config.supplyCurrLimit = supplyLimit; - config.statorCurrLimit = statorLimit; config.openloopRamp = openLoopRamp; config.closedloopRamp = closedLoopRamp; config.voltageCompSaturation = voltageCompSaturation; diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 8a4bf72..26dfef7 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -22,10 +22,10 @@ public Climber() { setName(ClimberConstants.name); motorLeader = new WPI_TalonFX(CanIDs.kClimberMotorLeft, Constants.Canivorename); ClimberConstants.setupFalconLeader(motorLeader); - motorFollower = new WPI_TalonFX(CanIDs.kClimberMotorRight, Constants.Canivorename); ClimberConstants.setupFalconFollower(motorFollower, motorLeader); + resetEncoder(); motorLeader.configForwardSoftLimitThreshold(ClimberConstants.fullExtend); motorLeader.configForwardSoftLimitEnable(false);