From 4af225649103820d57eeebc04b579a431dc9d69c Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Sun, 25 Jan 2026 06:37:46 -0800 Subject: [PATCH] Made the elevator climb + fixed error on BuildData I killed it --- simgui-ds.json | 5 + src/main/java/frc/robot/Robot.java | 36 ++--- src/main/java/frc/robot/RobotContainer.java | 5 +- .../java/frc/robot/constants/IdConstants.java | 4 + .../frc/robot/subsystems/climb/Climb.java | 142 ++++++++++++++++++ 5 files changed, 173 insertions(+), 19 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climb/Climb.java diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4919a24 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Joysticks": { + "window": { + "visible": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dfe0261..89701c6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.Constants; import frc.robot.constants.VisionConstants; import frc.robot.constants.swerve.DriveConstants; -import frc.robot.util.BuildData; +// import frc.robot.util.BuildData; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -80,23 +80,23 @@ public class Robot extends LoggedRobot { // obtain this robot's identity RobotId robotId = RobotId.getRobotId(); - // Record metadata - Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildData.GIT_SHA); - Logger.recordMetadata("GitDate", BuildData.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH); - switch (BuildData.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; - } + // // Record metadata + // Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME); + // Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE); + // Logger.recordMetadata("GitSHA", BuildData.GIT_SHA); + // Logger.recordMetadata("GitDate", BuildData.GIT_DATE); + // Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH); + // switch (BuildData.DIRTY) { + // case 0: + // Logger.recordMetadata("GitDirty", "All changes committed"); + // break; + // case 1: + // Logger.recordMetadata("GitDirty", "Uncomitted changes"); + // break; + // default: + // Logger.recordMetadata("GitDirty", "Unknown"); + // break; + // } robotContainer = new RobotContainer(robotId); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b79bffb..9773ebd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; +import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; import frc.robot.subsystems.shooter.Shooter; @@ -51,6 +52,7 @@ public class RobotContainer { private Vision vision = null; private Turret turret = null; private Shooter shooter = null; + private Climb climb = null; private Command auto = new DoNothing(); // Controllers are defined here @@ -63,6 +65,7 @@ public class RobotContainer { * Different robots may have different subsystems. */ public RobotContainer(RobotId robotId) { + climb = new Climb(); // dispatch on the robot switch (robotId) { case TestBed1: @@ -81,7 +84,7 @@ public class RobotContainer { case Vivace: case Phil: case Vertigo: - vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); + vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); turret = new Turret(); shooter = new Shooter(); diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index f293329..b9fa6db 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -26,4 +26,8 @@ public class IdConstants { public static final int SHOOTER_LEFT_ID = 22; public static final int SHOOTER_RIGHT_ID = 23; public static final int FEEDER_ID = 21; + + // Climb + public static final int CLIMB_MOTOR_LEFT = 48; + public static final int CLIMB_MOTOR_RIGHT = 49; } diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java new file mode 100644 index 0000000..c0388da --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -0,0 +1,142 @@ +package frc.robot.subsystems.climb; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; +import frc.robot.util.ClimbArmSim; + +public class Climb extends SubsystemBase { + + private double startingPosition = 0; + + //Motors + // TODO: tune better once design is finalized + private final PIDController pid = new PIDController(0.4, 4, 0.04); + + private TalonFX motorLeft = new TalonFX(IdConstants.CLIMB_MOTOR_LEFT); + private TalonFX motorRight = new TalonFX(IdConstants.CLIMB_MOTOR_RIGHT); + + private final DCMotor climbGearBox = DCMotor.getKrakenX60(1); + private TalonFXSimState encoderSim; + + //Mechism2d display + private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3); + private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5); + private final MechanismLigament2d simLigament = mechanismRoot.append( + new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite)) + ); + + private final double climbGearRatio = 54 / 10 * 60 / 18; // gear ratio of 18 + private ClimbArmSim climbSim; + private double power; + + public Climb() { + if (isSimulation()) { + encoderSim = motorLeft.getSimState(); + encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*climbGearRatio); + + climbSim = new ClimbArmSim( + climbGearBox, + climbGearRatio, + 0.1, + 0.127, + 0, //min angle + Units.degreesToRadians(90), //max angle + true, + Units.degreesToRadians(startingPosition), + 60 + ); + + climbSim.setIsClimbing(true); + } + + pid.setIZone(1); + pid.setSetpoint(Units.degreesToRadians(startingPosition)); + + motorLeft.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio); + motorRight.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio); + + SmartDashboard.putData("PID", pid); + SmartDashboard.putData("Climb Display", simulationMechanism); + SmartDashboard.putData("EXTEND", new InstantCommand(() -> extend())); + SmartDashboard.putData("CLIMB", new InstantCommand(() -> climb())); + } + + @Override + public void periodic() { + double motorPosition = motorLeft.getPosition().getValueAsDouble(); + double currentPosition = Units.rotationsToRadians(motorPosition/climbGearRatio); + + power = pid.calculate(currentPosition); + + motorLeft.set(MathUtil.clamp(power, -1, 1)); + motorRight.set(MathUtil.clamp(-power, -1, 1)); // Invert motor + + simLigament.setAngle(Units.radiansToDegrees(currentPosition)); + + SmartDashboard.putNumber("Climb Position", getAngle()); + + SmartDashboard.putNumber("Encoder Position", motorLeft.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Motor Velocity", motorLeft.getVelocity().getValueAsDouble()); + } + + @Override + public void simulationPeriodic() { + climbSim.setInput(power * Constants.ROBOT_VOLTAGE); + climbSim.update(Constants.LOOP_TIME); + + double climbRotations = Units.radiansToRotations(climbSim.getAngleRads()); + encoderSim.setRawRotorPosition(climbRotations * climbGearRatio); + } + + /** + * Sets the motor to an angle from 0-90 deg + * @param angle in degrees + */ + public void setAngle(double angle) { + pid.reset(); + pid.setSetpoint(Units.degreesToRadians(angle)); + } + + /** + * Gets the current position of the motor in degrees + * @return The angle in degrees + */ + public double getAngle() { + return Units.rotationsToDegrees(motorLeft.getPosition().getValueAsDouble() / climbGearRatio); + } + + /** + * Turns the motor to 90 degrees (extended positiion) + */ + public void extend(){ + double extendAngle = 90; + setAngle(extendAngle); + } + + /** + * Turns the motor to 0 degrees (climb position) + */ + public void climb(){ + setAngle(startingPosition); + } + + public boolean isSimulation(){ + return RobotBase.isSimulation(); + } +} \ No newline at end of file -- 2.39.5