From a9407405dfcdbd75d3483a311dfdfed738f65a73 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 15:48:47 -0800 Subject: [PATCH 01/10] Add configurations for turret pivot, hood, and ball shooter motors --- src/main/java/frc/robot/RobotMap.java | 9 ++ .../java/frc/robot/constants/ConstMotion.java | 95 +++++++++++++++++++ .../java/frc/robot/constants/ConstRotors.java | 16 +++- .../java/frc/robot/subsystems/Motion.java | 95 ++++++++++++++++++- .../java/frc/robot/subsystems/Rotors.java | 28 +++++- 5 files changed, 239 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 9056a8f..4f3d49a 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -31,4 +31,13 @@ public static class mapDrivetrain { public static final int BACK_RIGHT_STEER_CAN = 7; public static final int BACK_RIGHT_ABSOLUTE_ENCODER_CAN = 3; } + + public static class mapRotors { + public static final int BALL_SHOOTER_CAN = 10; + } + + public static class mapMotion { + public static final int TURRET_PIVOT_CAN = 20; + public static final int HOOD_CAN = 21; + } } diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index a5caec0..171c5a1 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -4,6 +4,101 @@ package frc.robot.constants; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; + /** Add your docs here. */ public class ConstMotion { + public static TalonFXConfiguration TURRET_PIVOT_CONFIG = new TalonFXConfiguration(); + + static { + // elevator motor config + // TODO: tune pid values + TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + TURRET_PIVOT_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); + TURRET_PIVOT_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; + TURRET_PIVOT_CONFIG.Slot0.kP = 0; + TURRET_PIVOT_CONFIG.Slot0.kI = 0; + TURRET_PIVOT_CONFIG.Slot0.kD = 0; + TURRET_PIVOT_CONFIG.Slot0.kS = 0; + TURRET_PIVOT_CONFIG.Slot0.kG = 0; + + TURRET_PIVOT_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio + TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; + TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicAcceleration = 0; + TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; + TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicExpo_kA = 0; + TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; + TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits + TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits + TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; + + // elevator pivot motor config + TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + TURRET_PIVOT_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); + + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); + } + + public static TalonFXConfiguration HOOD_CONFIG = new TalonFXConfiguration(); + + static { + // elevator motor config + // TODO: tune pid values + HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); + HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); + HOOD_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; + HOOD_CONFIG.Slot0.kP = 0; + HOOD_CONFIG.Slot0.kI = 0; + HOOD_CONFIG.Slot0.kD = 0; + HOOD_CONFIG.Slot0.kS = 0; + HOOD_CONFIG.Slot0.kG = 0; + + HOOD_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio + HOOD_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; + HOOD_CONFIG.MotionMagic.MotionMagicAcceleration = 0; + HOOD_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; + HOOD_CONFIG.MotionMagic.MotionMagicExpo_kA = 0; + HOOD_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; + HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits + HOOD_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits + HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; + + // elevator pivot motor config + HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); + } + + public static final double POSITION_TOLERANCE = Units.Degrees.of(1).in(Units.Rotations); + + public static class MechanismPositionGroup { + public Angle turretPivotMotorAngle; + public Angle hoodMotorAngle; + } } diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 15bd784..9a3ee88 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -4,6 +4,20 @@ package frc.robot.constants; -/** Add your docs here. */ +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + public class ConstRotors { + public static TalonFXConfiguration BALL_SHOOTER_CONFIG = new TalonFXConfiguration(); + + static { + BALL_SHOOTER_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + BALL_SHOOTER_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; + BALL_SHOOTER_CONFIG.CurrentLimits.SupplyCurrentLimit = 85; // TODO: tune current limits + BALL_SHOOTER_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 60; // TODO: tune current limits + BALL_SHOOTER_CONFIG.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } + + public static final double BALL_SHOOTER_SPEED = 0.2; // TODO: Replace with actual speed } diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 05fe2f8..7dfa92a 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -4,14 +4,105 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degrees; + +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.*; +import frc.robot.constants.ConstMotion.MechanismPositionGroup; +import frc.robot.Robot; +import frc.robot.RobotMap.*; +@Logged public class Motion extends SubsystemBase { /** Creates a new Motion. */ - public Motion() {} + TalonFX turretPivotMotor; + TalonFX hoodMotor; + + private Angle turretPivotLastDesiredAngle = Degrees.zero(); + private Angle hoodLastDesiredAngle = Degrees.zero(); + MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0); + + public Motion() { + turretPivotMotor = new TalonFX(mapMotion.TURRET_PIVOT_CAN); + hoodMotor = new TalonFX(mapMotion.HOOD_CAN); + } + + private void setHoodPivotAngle(Angle angle, int slot) { + hoodMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); + hoodLastDesiredAngle = angle; + } + + private final void setTurretPivotAngle(Angle angle, int slot) { + turretPivotMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); + turretPivotLastDesiredAngle = angle; + } + + public void setHoodCoastMode(boolean coastMode) { + if (coastMode) { + ConstMotion.HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Coast; + hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG); + } else { + ConstMotion.HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG); + } + } + + public void setTurretCoastMode(boolean coastMode) { + if (coastMode) { + ConstMotion.TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Coast; + turretPivotMotor.getConfigurator().apply(ConstMotion.TURRET_PIVOT_CONFIG); + } else { + ConstMotion.TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turretPivotMotor.getConfigurator().apply(ConstMotion.TURRET_PIVOT_CONFIG); + } + } + + public Angle getHoodAngle() { + if (Robot.isSimulation()) { + return hoodLastDesiredAngle; + } + return hoodMotor.getPosition().getValue(); + } + + public Angle getTurretPivotAngle() { + if (Robot.isSimulation()) { + return turretPivotLastDesiredAngle; + } + return turretPivotMotor.getPosition().getValue(); + } + + public AngularVelocity getTurretPivotVelocity() { + return turretPivotMotor.getRotorVelocity().getValue(); + } + + public AngularVelocity getHoodVelocity() { + return hoodMotor.getRotorVelocity().getValue(); + } + + public boolean isTurretPivotVelocityZero() { + return getTurretPivotVelocity().isNear(Units.RotationsPerSecond.zero(), 0.01); + } + + public boolean isHoodVelocityZero() { + return getHoodVelocity().isNear(Units.RotationsPerSecond.zero(), 0.01); + } + + public boolean arePositionsAtSetPoint(MechanismPositionGroup positionGroup) { + return (getTurretPivotAngle().isNear(positionGroup.turretPivotMotorAngle, ConstMotion.POSITION_TOLERANCE) + && getHoodAngle().isNear(positionGroup.hoodMotorAngle, ConstMotion.POSITION_TOLERANCE)); + } @Override public void periodic() { - // This method will be called once per scheduler run + // + } } diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 0d92298..1e9560e 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -4,11 +4,37 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap.mapRotors; +import frc.robot.constants.*; +@Logged public class Rotors extends SubsystemBase { /** Creates a new Rotors. */ - public Rotors() {} + + TalonFX ballShooterMotor; + + public Rotors() { + ballShooterMotor = new TalonFX(mapRotors.BALL_SHOOTER_CAN); + + ballShooterMotor.getConfigurator().apply(ConstRotors.BALL_SHOOTER_CONFIG); + } + + public void setBallShooterMotorSpeed(double speed) { + ballShooterMotor.set(speed); + } + + public double getBallShooterMotorSpeed() { + return ballShooterMotor.getRotorVelocity().getValue().in(Units.RPM); + } + + public void stopBallShooterMotor() { + ballShooterMotor.set(0); + } @Override public void periodic() { From 6760aff92c1501b0369535bed0751c251a3b6092 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 16:09:07 -0800 Subject: [PATCH 02/10] Refactor turret and hood motor configurations for clarity and consistency --- .../java/frc/robot/constants/ConstMotion.java | 144 +++++++++--------- .../java/frc/robot/subsystems/Motion.java | 28 ++-- .../java/frc/robot/subsystems/Rotors.java | 2 +- 3 files changed, 87 insertions(+), 87 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 171c5a1..a2419a0 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -14,85 +14,85 @@ /** Add your docs here. */ public class ConstMotion { - public static TalonFXConfiguration TURRET_PIVOT_CONFIG = new TalonFXConfiguration(); + public static TalonFXConfiguration TURRET_CONFIG = new TalonFXConfiguration(); static { - // elevator motor config - // TODO: tune pid values - TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - TURRET_PIVOT_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); - TURRET_PIVOT_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; - TURRET_PIVOT_CONFIG.Slot0.kP = 0; - TURRET_PIVOT_CONFIG.Slot0.kI = 0; - TURRET_PIVOT_CONFIG.Slot0.kD = 0; - TURRET_PIVOT_CONFIG.Slot0.kS = 0; - TURRET_PIVOT_CONFIG.Slot0.kG = 0; - - TURRET_PIVOT_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio - TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; - TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicAcceleration = 0; - TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; - TURRET_PIVOT_CONFIG.MotionMagic.MotionMagicExpo_kA = 0; - TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits - TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits - TURRET_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; - - // elevator pivot motor config - TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - TURRET_PIVOT_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); - - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - TURRET_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); + // elevator motor config + // TODO: tune pid values + TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + TURRET_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); + TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); + TURRET_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; + TURRET_CONFIG.Slot0.kP = 0; + TURRET_CONFIG.Slot0.kI = 0; + TURRET_CONFIG.Slot0.kD = 0; + TURRET_CONFIG.Slot0.kS = 0; + TURRET_CONFIG.Slot0.kG = 0; + + TURRET_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio + TURRET_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; + TURRET_CONFIG.MotionMagic.MotionMagicAcceleration = 0; + TURRET_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; + TURRET_CONFIG.MotionMagic.MotionMagicExpo_kA = 0; + TURRET_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; + TURRET_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits + TURRET_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits + TURRET_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; + + // elevator pivot motor config + TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + TURRET_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); + TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); + + TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); } public static TalonFXConfiguration HOOD_CONFIG = new TalonFXConfiguration(); static { - // elevator motor config - // TODO: tune pid values - HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); - HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); - HOOD_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; - HOOD_CONFIG.Slot0.kP = 0; - HOOD_CONFIG.Slot0.kI = 0; - HOOD_CONFIG.Slot0.kD = 0; - HOOD_CONFIG.Slot0.kS = 0; - HOOD_CONFIG.Slot0.kG = 0; - - HOOD_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio - HOOD_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; - HOOD_CONFIG.MotionMagic.MotionMagicAcceleration = 0; - HOOD_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; - HOOD_CONFIG.MotionMagic.MotionMagicExpo_kA = 0; - HOOD_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits - HOOD_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits - HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; - - // elevator pivot motor config - HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); + // elevator motor config + // TODO: tune pid values + HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); + HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); + HOOD_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; + HOOD_CONFIG.Slot0.kP = 0; + HOOD_CONFIG.Slot0.kI = 0; + HOOD_CONFIG.Slot0.kD = 0; + HOOD_CONFIG.Slot0.kS = 0; + HOOD_CONFIG.Slot0.kG = 0; + + HOOD_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio + HOOD_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; + HOOD_CONFIG.MotionMagic.MotionMagicAcceleration = 0; + HOOD_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; + HOOD_CONFIG.MotionMagic.MotionMagicExpo_kA = 0; + HOOD_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; + HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits + HOOD_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits + HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; + + // elevator pivot motor config + HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) + .in(Units.Degrees); } public static final double POSITION_TOLERANCE = Units.Degrees.of(1).in(Units.Rotations); diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 7dfa92a..444e4c7 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -23,26 +23,26 @@ @Logged public class Motion extends SubsystemBase { /** Creates a new Motion. */ - TalonFX turretPivotMotor; + TalonFX turretMotor; TalonFX hoodMotor; - private Angle turretPivotLastDesiredAngle = Degrees.zero(); + private Angle turretLastDesiredAngle = Degrees.zero(); private Angle hoodLastDesiredAngle = Degrees.zero(); MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0); public Motion() { - turretPivotMotor = new TalonFX(mapMotion.TURRET_PIVOT_CAN); + turretMotor = new TalonFX(mapMotion.TURRET_PIVOT_CAN); hoodMotor = new TalonFX(mapMotion.HOOD_CAN); } - private void setHoodPivotAngle(Angle angle, int slot) { + private void setHoodAngle(Angle angle, int slot) { hoodMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); hoodLastDesiredAngle = angle; } - private final void setTurretPivotAngle(Angle angle, int slot) { - turretPivotMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); - turretPivotLastDesiredAngle = angle; + private final void setTurretAngle(Angle angle, int slot) { + turretMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); + turretLastDesiredAngle = angle; } public void setHoodCoastMode(boolean coastMode) { @@ -57,11 +57,11 @@ public void setHoodCoastMode(boolean coastMode) { public void setTurretCoastMode(boolean coastMode) { if (coastMode) { - ConstMotion.TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Coast; - turretPivotMotor.getConfigurator().apply(ConstMotion.TURRET_PIVOT_CONFIG); + ConstMotion.TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Coast; + turretMotor.getConfigurator().apply(ConstMotion.TURRET_CONFIG); } else { - ConstMotion.TURRET_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - turretPivotMotor.getConfigurator().apply(ConstMotion.TURRET_PIVOT_CONFIG); + ConstMotion.TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turretMotor.getConfigurator().apply(ConstMotion.TURRET_CONFIG); } } @@ -74,13 +74,13 @@ public Angle getHoodAngle() { public Angle getTurretPivotAngle() { if (Robot.isSimulation()) { - return turretPivotLastDesiredAngle; + return turretLastDesiredAngle; } - return turretPivotMotor.getPosition().getValue(); + return turretMotor.getPosition().getValue(); } public AngularVelocity getTurretPivotVelocity() { - return turretPivotMotor.getRotorVelocity().getValue(); + return turretMotor.getRotorVelocity().getValue(); } public AngularVelocity getHoodVelocity() { diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 1e9560e..442c93f 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -28,7 +28,7 @@ public void setBallShooterMotorSpeed(double speed) { ballShooterMotor.set(speed); } - public double getBallShooterMotorSpeed() { + public double getBallShooterMotorVelocity() { return ballShooterMotor.getRotorVelocity().getValue().in(Units.RPM); } From 28017239378a2f9cbbb365b75c8f241492dea8d4 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 16:27:21 -0800 Subject: [PATCH 03/10] Apply turret and hood motor configurations during initialization --- src/main/java/frc/robot/subsystems/Motion.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 444e4c7..1a0dbef 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -33,6 +33,9 @@ public class Motion extends SubsystemBase { public Motion() { turretMotor = new TalonFX(mapMotion.TURRET_PIVOT_CAN); hoodMotor = new TalonFX(mapMotion.HOOD_CAN); + + turretMotor.getConfigurator().apply(ConstMotion.TURRET_CONFIG); + hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG); } private void setHoodAngle(Angle angle, int slot) { From fc6eddba8ba15a822e3b86e1b22644917f87aef3 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 16:27:38 -0800 Subject: [PATCH 04/10] Update src/main/java/frc/robot/constants/ConstMotion.java Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/main/java/frc/robot/constants/ConstMotion.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index a2419a0..b59cd2b 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -17,7 +17,7 @@ public class ConstMotion { public static TalonFXConfiguration TURRET_CONFIG = new TalonFXConfiguration(); static { - // elevator motor config + // turret motor config // TODO: tune pid values TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; TURRET_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; From 8d5fadc120a8a04284b1ad5e072459b4d42d94e1 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 16:30:53 -0800 Subject: [PATCH 05/10] Removed Redundent Code --- src/main/java/frc/robot/constants/ConstMotion.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index a2419a0..e97ab4b 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -85,14 +85,6 @@ public class ConstMotion { HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits HOOD_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; - - // elevator pivot motor config - HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); } public static final double POSITION_TOLERANCE = Units.Degrees.of(1).in(Units.Rotations); From 7ae0e4d4ca9f8d58bf9f6f41810b20beddf2f039 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 16:33:20 -0800 Subject: [PATCH 06/10] Removed More Renundent Code --- .../java/frc/robot/constants/ConstMotion.java | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index fe72f10..0124112 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -41,21 +41,6 @@ public class ConstMotion { TURRET_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits TURRET_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits TURRET_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1; - - // elevator pivot motor config - TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - TURRET_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); - TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); - - TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0) - .in(Units.Degrees); } public static TalonFXConfiguration HOOD_CONFIG = new TalonFXConfiguration(); From c065cc136029c9afb225747a528a6719ab7e8c76 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 16:37:04 -0800 Subject: [PATCH 07/10] Update soft limit thresholds to use degrees instead of rotations for turret and hood configurations --- src/main/java/frc/robot/constants/ConstMotion.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 0124112..8ad1da3 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -22,9 +22,9 @@ public class ConstMotion { TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; TURRET_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); + TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Degrees.of(60).in(Units.Rotations); TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); + TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Degrees.of(0).in(Units.Rotations); TURRET_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; TURRET_CONFIG.Slot0.kP = 0; TURRET_CONFIG.Slot0.kI = 0; @@ -51,9 +51,9 @@ public class ConstMotion { HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(60).in(Units.Rotations); + HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Degrees.of(60).in(Units.Rotations); HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(0).in(Units.Rotations); + HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Degrees.of(0).in(Units.Rotations); HOOD_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static; HOOD_CONFIG.Slot0.kP = 0; HOOD_CONFIG.Slot0.kI = 0; From 82e4e24179e6d3274f0d8fb4e966044541428ff1 Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 18:20:42 -0800 Subject: [PATCH 08/10] Refactor ConstMotion and Motion classes for clarity --- src/main/java/frc/robot/constants/ConstMotion.java | 11 ++++++++--- src/main/java/frc/robot/subsystems/Motion.java | 4 ++-- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 8ad1da3..316e970 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -32,7 +32,7 @@ public class ConstMotion { TURRET_CONFIG.Slot0.kS = 0; TURRET_CONFIG.Slot0.kG = 0; - TURRET_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio + TURRET_CONFIG.Feedback.SensorToMechanismRatio = 0; // TODO: replace with actual ratio TURRET_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; TURRET_CONFIG.MotionMagic.MotionMagicAcceleration = 0; TURRET_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; @@ -46,7 +46,7 @@ public class ConstMotion { public static TalonFXConfiguration HOOD_CONFIG = new TalonFXConfiguration(); static { - // elevator motor config + // Hood motor config // TODO: tune pid values HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; @@ -61,7 +61,7 @@ public class ConstMotion { HOOD_CONFIG.Slot0.kS = 0; HOOD_CONFIG.Slot0.kG = 0; - HOOD_CONFIG.Feedback.SensorToMechanismRatio = (0); // TODO: replace with actual ratio + HOOD_CONFIG.Feedback.SensorToMechanismRatio = 0; // TODO: replace with actual ratio HOOD_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0; HOOD_CONFIG.MotionMagic.MotionMagicAcceleration = 0; HOOD_CONFIG.MotionMagic.MotionMagicExpo_kV = 0; @@ -77,5 +77,10 @@ public class ConstMotion { public static class MechanismPositionGroup { public Angle turretPivotMotorAngle; public Angle hoodMotorAngle; + + public MechanismPositionGroup(Angle turretPivotMotorAngle, Angle hoodMotorAngle) { + this.turretPivotMotorAngle = turretPivotMotorAngle; + this.hoodMotorAngle = hoodMotorAngle; + } } } diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 1a0dbef..afdc7d3 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -38,12 +38,12 @@ public Motion() { hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG); } - private void setHoodAngle(Angle angle, int slot) { + public void setHoodAngle(Angle angle, int slot) { hoodMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); hoodLastDesiredAngle = angle; } - private final void setTurretAngle(Angle angle, int slot) { + public final void setTurretAngle(Angle angle, int slot) { turretMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); turretLastDesiredAngle = angle; } From 6b3f59ad8b841ed437f4366c5ba72291248e63bd Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 18:34:00 -0800 Subject: [PATCH 09/10] Refactor Motion class to initialize motors as final and improve method visibility --- src/main/java/frc/robot/subsystems/Motion.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index afdc7d3..96e80a9 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -23,22 +23,19 @@ @Logged public class Motion extends SubsystemBase { /** Creates a new Motion. */ - TalonFX turretMotor; - TalonFX hoodMotor; + final TalonFX turretMotor = new TalonFX(mapMotion.TURRET_PIVOT_CAN); + TalonFX hoodMotor = new TalonFX(mapMotion.HOOD_CAN); private Angle turretLastDesiredAngle = Degrees.zero(); private Angle hoodLastDesiredAngle = Degrees.zero(); MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0); public Motion() { - turretMotor = new TalonFX(mapMotion.TURRET_PIVOT_CAN); - hoodMotor = new TalonFX(mapMotion.HOOD_CAN); - turretMotor.getConfigurator().apply(ConstMotion.TURRET_CONFIG); hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG); } - public void setHoodAngle(Angle angle, int slot) { + public final void setHoodAngle(Angle angle, int slot) { hoodMotor.setControl(positionRequest.withPosition(angle).withSlot(slot)); hoodLastDesiredAngle = angle; } From 2356ef4731faa5d6909afb17e22ec2943c56859a Mon Sep 17 00:00:00 2001 From: Evan Grinnell Date: Sat, 15 Nov 2025 18:55:31 -0800 Subject: [PATCH 10/10] Remove redundant constructor from MechanismPositionGroup in ConstMotion class --- src/main/java/frc/robot/constants/ConstMotion.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 316e970..a77bb6c 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -77,10 +77,5 @@ public class ConstMotion { public static class MechanismPositionGroup { public Angle turretPivotMotorAngle; public Angle hoodMotorAngle; - - public MechanismPositionGroup(Angle turretPivotMotorAngle, Angle hoodMotorAngle) { - this.turretPivotMotorAngle = turretPivotMotorAngle; - this.hoodMotorAngle = hoodMotorAngle; - } } }