From 39c67e789475aa16541552900336a88813625077 Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Wed, 4 Feb 2026 17:43:55 +0800 Subject: [PATCH 1/8] Climber-develope --- .../frc/robot/commands/ClimberCommand.java | 27 +++++++++++ .../subsystems/climber/ClimberConstants.java | 15 ++++++ .../subsystems/climber/ClimberSubsystem.java | 48 +++++++++++++++++++ 3 files changed, 90 insertions(+) create mode 100644 src/main/java/frc/robot/commands/ClimberCommand.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberConstants.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java new file mode 100644 index 0000000..a93057a --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.climber.ClimberSubsystem; + +public class ClimberCommand { + private final ClimberSubsystem climberSubsystem; + private final double climberPosition; + + public ClimberCommand(ClimberSubsystem climberSubsystem, double climberPosition) { + this.climberSubsystem = climberSubsystem; + this.climberPosition = climberPosition; + } + public void initialize() { + climberSubsystem.resetClimberPosition(); + climberSubsystem.setClimberTargetPosition(climberPosition); + } + public void execute() { + climberSubsystem.runClimber(); + SmartDashboard.putNumber("climberMotor", climberSubsystem.getClimberPosition()); + SmartDashboard.putNumber("climberTarget", climberPosition); + } + public void end(boolean interrupted) { + climberSubsystem.resetClimberPosition(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..c966d7f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.climber; + +public class ClimberConstants { + public static final int leftMotorID = 20; + public static final int rightMotorID = 21; + + public static final double climberKP = 0.1; + public static final double climberKI = 0.0; + public static final double climberKD = 0.0; + + + + + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java new file mode 100644 index 0000000..a0d838d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; + +import edu.wpi.first.math.controller.PIDController; +import frc.robot.subsystems.climber.ClimberConstants; + +public class ClimberSubsystem { + private TalonFX leftMotor = new TalonFX(ClimberConstants.leftMotorID,"GTX7130"); + private TalonFX rightMotor = new TalonFX(ClimberConstants.rightMotorID,"GTX7130"); + + private PIDController climberPID = new PIDController(ClimberConstants.climberKP, ClimberConstants.climberKI, ClimberConstants.climberKD); + + public ClimberSubsystem() { + + MotorOutputConfigs leftConfigs = new MotorOutputConfigs(); + MotorOutputConfigs rightConfigs = new MotorOutputConfigs(); + leftConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + rightConfigs.Inverted = InvertedValue.Clockwise_Positive; + leftMotor.setControl(new Follower(ClimberConstants.leftMotorID , MotorAlignmentValue.Aligned)); + + // TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + // motorConfig.MotorOutput + // .withInverted(InvertedValue.CounterClockwise_Positive) + + } + public double getClimberPosition() { + return (leftMotor.getPosition().getValueAsDouble()+rightMotor.getPosition().getValueAsDouble())/2; + + + } + public void resetClimberPosition() { + leftMotor.set(0); + rightMotor.set(0); + } + public void setClimberTargetPosition(double position) { + climberPID.setSetpoint(position); + } + public void runClimber() { + leftMotor.setVoltage(climberPID.calculate((leftMotor.getPosition().getValueAsDouble() + + rightMotor.getPosition().getValueAsDouble())/2)); + } +} From 1b5391eb66bdc67f350a708990ba0cf17baa47ac Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Wed, 4 Feb 2026 18:14:06 +0800 Subject: [PATCH 2/8] climber-moveFile --- .../robot/{commands => subsystems/climber}/ClimberCommand.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) rename src/main/java/frc/robot/{commands => subsystems/climber}/ClimberCommand.java (91%) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/subsystems/climber/ClimberCommand.java similarity index 91% rename from src/main/java/frc/robot/commands/ClimberCommand.java rename to src/main/java/frc/robot/subsystems/climber/ClimberCommand.java index a93057a..08797d3 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberCommand.java @@ -1,7 +1,6 @@ -package frc.robot.commands; +package frc.robot.subsystems.climber; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.climber.ClimberSubsystem; public class ClimberCommand { private final ClimberSubsystem climberSubsystem; From efc47354be430a5b8d3f4fedeaddb9fc64a65c34 Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Wed, 4 Feb 2026 18:58:48 +0800 Subject: [PATCH 3/8] climber-moveFile --- .../robot/{subsystems/climber => commands}/ClimberCommand.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/frc/robot/{subsystems/climber => commands}/ClimberCommand.java (100%) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java similarity index 100% rename from src/main/java/frc/robot/subsystems/climber/ClimberCommand.java rename to src/main/java/frc/robot/commands/ClimberCommand.java From b55e080cc86386d595c444a9fe8eeda0c54685f0 Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Wed, 4 Feb 2026 22:57:25 +0800 Subject: [PATCH 4/8] climber-fix --- src/main/java/frc/robot/commands/ClimberCommand.java | 12 ++++++++---- .../robot/subsystems/climber/ClimberSubsystem.java | 2 -- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index 08797d3..1777a1d 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -1,8 +1,10 @@ -package frc.robot.subsystems.climber; +package frc.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climber.ClimberSubsystem; -public class ClimberCommand { +public class ClimberCommand extends Command{ private final ClimberSubsystem climberSubsystem; private final double climberPosition; @@ -11,7 +13,7 @@ public ClimberCommand(ClimberSubsystem climberSubsystem, double climberPosition) this.climberPosition = climberPosition; } public void initialize() { - climberSubsystem.resetClimberPosition(); + climberSubsystem.setClimberTargetPosition(climberPosition); } public void execute() { @@ -22,5 +24,7 @@ public void execute() { public void end(boolean interrupted) { climberSubsystem.resetClimberPosition(); } - + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index a0d838d..7188224 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -35,8 +35,6 @@ public double getClimberPosition() { } public void resetClimberPosition() { - leftMotor.set(0); - rightMotor.set(0); } public void setClimberTargetPosition(double position) { climberPID.setSetpoint(position); From d0235eebd1af94192095856918d2aa37ac79d9fc Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Thu, 5 Feb 2026 10:08:00 +0800 Subject: [PATCH 5/8] climber-fix --- .../subsystems/climber/ClimberConstants.java | 9 ++---- .../subsystems/climber/ClimberSubsystem.java | 32 ++++++++++++------- 2 files changed, 22 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java index c966d7f..ab3ae1b 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -6,10 +6,5 @@ public class ClimberConstants { public static final double climberKP = 0.1; public static final double climberKI = 0.0; - public static final double climberKD = 0.0; - - - - - -} + public static final double climberKD = 0.0; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 7188224..6e62cd7 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.climber; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; @@ -11,34 +13,40 @@ import frc.robot.subsystems.climber.ClimberConstants; public class ClimberSubsystem { - private TalonFX leftMotor = new TalonFX(ClimberConstants.leftMotorID,"GTX7130"); - private TalonFX rightMotor = new TalonFX(ClimberConstants.rightMotorID,"GTX7130"); + final CANBus leftBus = new CANBus("GTX7130"); + final CANBus rightBus = new CANBus("GTX7130"); + private TalonFX leftMotor = new TalonFX(ClimberConstants.leftMotorID,leftBus); + private TalonFX rightMotor = new TalonFX(ClimberConstants.rightMotorID,rightBus); - private PIDController climberPID = new PIDController(ClimberConstants.climberKP, ClimberConstants.climberKI, ClimberConstants.climberKD); + private PIDController climberPID = new PIDController(ClimberConstants.climberKP, + ClimberConstants.climberKI, ClimberConstants.climberKD); public ClimberSubsystem() { - + TalonFXConfiguration leftConfiguration = new TalonFXConfiguration(); + TalonFXConfiguration rightConfiguration = new TalonFXConfiguration(); + leftConfiguration.Feedback.withSensorToMechanismRatio(12); + rightConfiguration.Feedback.withSensorToMechanismRatio(12); + MotorOutputConfigs leftConfigs = new MotorOutputConfigs(); MotorOutputConfigs rightConfigs = new MotorOutputConfigs(); leftConfigs.Inverted = InvertedValue.CounterClockwise_Positive; rightConfigs.Inverted = InvertedValue.Clockwise_Positive; - leftMotor.setControl(new Follower(ClimberConstants.leftMotorID , MotorAlignmentValue.Aligned)); - - // TalonFXConfiguration motorConfig = new TalonFXConfiguration(); - // motorConfig.MotorOutput - // .withInverted(InvertedValue.CounterClockwise_Positive) - + leftMotor.setControl(new Follower(ClimberConstants.rightMotorID , MotorAlignmentValue.Aligned)); + rightMotor.setPosition(0); } + public double getClimberPosition() { return (leftMotor.getPosition().getValueAsDouble()+rightMotor.getPosition().getValueAsDouble())/2; - - } + public void resetClimberPosition() { + rightMotor.setPosition(0); } + public void setClimberTargetPosition(double position) { climberPID.setSetpoint(position); } + public void runClimber() { leftMotor.setVoltage(climberPID.calculate((leftMotor.getPosition().getValueAsDouble() + rightMotor.getPosition().getValueAsDouble())/2)); From 02f21ecdde2e3a9c2f199cae1fa5d37068d8c808 Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Thu, 5 Feb 2026 13:14:34 +0800 Subject: [PATCH 6/8] intake-save --- src/main/java/frc/robot/commands/IntakeCommand.java | 0 src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java | 0 src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java | 0 3 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IntakeCommand.java create mode 100644 src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java new file mode 100644 index 0000000..e69de29 From 6485ca464352bbe8a47c9f12228a696d1bc0a8f3 Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Thu, 5 Feb 2026 13:50:38 +0800 Subject: [PATCH 7/8] climber-fix --- src/main/java/frc/robot/commands/IntakeCommand.java | 0 src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java | 0 src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java | 0 3 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/IntakeCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/Intake/IntakeSubsystem.java deleted file mode 100644 index e69de29..0000000 From 6b0c3b3e842fd07f39e45ab704febbde302ba867 Mon Sep 17 00:00:00 2001 From: "@gjaswskfd" Date: Thu, 5 Feb 2026 16:46:18 +0800 Subject: [PATCH 8/8] climber-update --- .../frc/robot/commands/ClimberCommand.java | 11 +++---- .../subsystems/climber/ClimberConstants.java | 3 ++ .../subsystems/climber/ClimberSubsystem.java | 29 +++++++++---------- 3 files changed, 20 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index 1777a1d..4e44963 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -6,25 +6,22 @@ public class ClimberCommand extends Command{ private final ClimberSubsystem climberSubsystem; - private final double climberPosition; + private final double climberTarget; public ClimberCommand(ClimberSubsystem climberSubsystem, double climberPosition) { this.climberSubsystem = climberSubsystem; - this.climberPosition = climberPosition; + this.climberTarget = climberPosition; } public void initialize() { - climberSubsystem.setClimberTargetPosition(climberPosition); + climberSubsystem.setClimberTargetPosition(climberTarget); } public void execute() { climberSubsystem.runClimber(); SmartDashboard.putNumber("climberMotor", climberSubsystem.getClimberPosition()); - SmartDashboard.putNumber("climberTarget", climberPosition); + SmartDashboard.putNumber("climberTarget", climberTarget); } public void end(boolean interrupted) { climberSubsystem.resetClimberPosition(); } - public boolean isFinished() { - return false; - } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java index ab3ae1b..13dbfb3 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.climber; +import com.ctre.phoenix6.CANBus; + public class ClimberConstants { + public static final CANBus climberBus = new CANBus("GTX7130"); public static final int leftMotorID = 20; public static final int rightMotorID = 21; diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 6e62cd7..f38ff9e 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -13,26 +13,23 @@ import frc.robot.subsystems.climber.ClimberConstants; public class ClimberSubsystem { - final CANBus leftBus = new CANBus("GTX7130"); - final CANBus rightBus = new CANBus("GTX7130"); - private TalonFX leftMotor = new TalonFX(ClimberConstants.leftMotorID,leftBus); - private TalonFX rightMotor = new TalonFX(ClimberConstants.rightMotorID,rightBus); + private TalonFX leftMotor = new TalonFX(ClimberConstants.leftMotorID,ClimberConstants.climberBus); + private TalonFX rightMotor = new TalonFX(ClimberConstants.rightMotorID,ClimberConstants.climberBus); - private PIDController climberPID = new PIDController(ClimberConstants.climberKP, - ClimberConstants.climberKI, ClimberConstants.climberKD); + private PIDController climberPID = new PIDController(ClimberConstants.climberKP,ClimberConstants.climberKI, ClimberConstants.climberKD); public ClimberSubsystem() { - TalonFXConfiguration leftConfiguration = new TalonFXConfiguration(); - TalonFXConfiguration rightConfiguration = new TalonFXConfiguration(); - leftConfiguration.Feedback.withSensorToMechanismRatio(12); - rightConfiguration.Feedback.withSensorToMechanismRatio(12); + TalonFXConfiguration leftConfiguration = new TalonFXConfiguration(); + TalonFXConfiguration rightConfiguration = new TalonFXConfiguration(); + leftConfiguration.Feedback.withSensorToMechanismRatio(12); + rightConfiguration.Feedback.withSensorToMechanismRatio(12); - MotorOutputConfigs leftConfigs = new MotorOutputConfigs(); - MotorOutputConfigs rightConfigs = new MotorOutputConfigs(); - leftConfigs.Inverted = InvertedValue.CounterClockwise_Positive; - rightConfigs.Inverted = InvertedValue.Clockwise_Positive; - leftMotor.setControl(new Follower(ClimberConstants.rightMotorID , MotorAlignmentValue.Aligned)); - rightMotor.setPosition(0); + MotorOutputConfigs leftConfigs = new MotorOutputConfigs(); + MotorOutputConfigs rightConfigs = new MotorOutputConfigs(); + leftConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + rightConfigs.Inverted = InvertedValue.Clockwise_Positive; + leftMotor.setControl(new Follower(ClimberConstants.rightMotorID , MotorAlignmentValue.Aligned)); + rightMotor.setPosition(0); } public double getClimberPosition() {