From 7d041c61d5f6faf1d08dad2566ffe70056db05ff Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Sat, 17 Jan 2026 15:25:38 -0800 Subject: [PATCH] stuff --- gradlew | 0 simgui-ds.json | 92 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 3 + .../frc/robot/subsystems/turret/Turret.java | 38 +++++--- 4 files changed, 120 insertions(+), 13 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 simgui-ds.json diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 49efa97..52c3f12 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; +import frc.robot.subsystems.turret.Turret; import frc.robot.util.PathGroupLoader; import frc.robot.util.Vision.DetectedObject; import frc.robot.util.Vision.Vision; @@ -41,6 +42,7 @@ public class RobotContainer { // The robot's subsystems are defined here... private Drivetrain drive = null; private Vision vision = null; + private Turret turret = null; private Command auto = new DoNothing(); // Controllers are defined here @@ -53,6 +55,7 @@ public class RobotContainer { * Different robots may have different subsystems. */ public RobotContainer(RobotId robotId) { + turret = new Turret(); // dispatch on the robot switch (robotId) { case TestBed1: diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 25c68e7..01f49d6 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -42,7 +42,7 @@ public class Turret extends SubsystemBase { private SingleJointedArmSim turretSim; private static final DCMotor turretMotorSim = DCMotor.getKrakenX60(1); private TalonFXSimState encoderSim; - private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(Units.degreesToRotations(0) * 1); // 1 is placeholder for gear ratio + private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(Units.degreesToRotations(0) * gearRatio); // gear ratio private double setpoint = 0; Mechanism2d mechanism2d = new Mechanism2d(100, 100); MechanismRoot2d mechanismRoot = mechanism2d.getRoot("pivot", 50, 50); @@ -70,7 +70,7 @@ public class Turret extends SubsystemBase { TalonFXConfiguration config = new TalonFXConfiguration(); CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = 40; + limitConfig.StatorCurrentLimit = 200; limitConfig.StatorCurrentLimitEnable = true; motor.getConfigurator().apply(limitConfig); @@ -86,21 +86,30 @@ public class Turret extends SubsystemBase { config.Slot0.kD = Units.radiansToRotations(0.00 * 12); // Derivative term (used to dampen oscillations) MotionMagicConfigs motionMagicConfigs = config.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = Units.degreesToRotations(0.1) * gearRatio; // max velocity * gear ratio - motionMagicConfigs.MotionMagicAcceleration = Units.degreesToRotations(0.1) * gearRatio; // max Acceleration * gear ratio + motionMagicConfigs.MotionMagicCruiseVelocity = Units.degreesToRotations(90) * gearRatio; // max velocity * gear ratio + motionMagicConfigs.MotionMagicAcceleration = Units.degreesToRotations(90) * gearRatio; // max Acceleration * gear ratio config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; motor.getConfigurator().apply(config); - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + // config.ClosedLoopGeneral.ContinuousWrap = true; + + motor.getConfigurator().apply(config); + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.degreesToRotations(360) * gearRatio; // max angle * gear ratio - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.degreesToRotations(0) * gearRatio; // min angle * gear ratio SmartDashboard.putData("turret", mechanism2d); SmartDashboard.putData("PID", pid); + SmartDashboard.putData("Set to 0 degrees", new InstantCommand(() -> setSetpoint(0))); + SmartDashboard.putData("Set to 90 degrees", new InstantCommand(( )-> setSetpoint(90))); + SmartDashboard.putData("Set to 180 degrees", new InstantCommand(() -> setSetpoint(180))); + SmartDashboard.putData("Set to 270 degrees", new InstantCommand(() -> setSetpoint(270))); + SmartDashboard.putData("Set to 1,1", new InstantCommand(() -> setTarget(1,1))); SmartDashboard.putData("Set to -1,1", new InstantCommand(( )-> setTarget(-1,1))); SmartDashboard.putData("Set to -1,-1", new InstantCommand(() -> setTarget(-1,-1))); @@ -124,13 +133,17 @@ public class Turret extends SubsystemBase { return motor.getPosition().getValueAsDouble() / gearRatio; // Gear ratio } - public void setSetpoint(double setpointDegrees) { - // If you want to handle the wrap-around ( -180 to 180) - double clampedSetpoint = MathUtil.inputModulus(setpointDegrees, -180, 180); - this.setpoint = clampedSetpoint; + // public void setSetpoint(double setpointDegrees) { + // double clampedSetpoint = MathUtil.inputModulus(setpointDegrees, 0, 360); + // this.setpoint = clampedSetpoint; - // Convert mechanism degrees to motor rotations: (Degrees / 360) * GearRatio - double motorTargetRotations = (clampedSetpoint / 360.0) * gearRatio; + // // Convert mechanism degrees to motor rotations: (Degrees / 360) * GearRatio + // double motorTargetRotations = (clampedSetpoint / 360.0) * gearRatio; + // motor.setControl(voltageRequest.withPosition(motorTargetRotations)); + // } + + public void setSetpoint(double setpointDegrees) { + double motorTargetRotations = (setpointDegrees / 360.0) * gearRatio; motor.setControl(voltageRequest.withPosition(motorTargetRotations)); } @@ -147,7 +160,6 @@ public class Turret extends SubsystemBase { // SmartDashboard.putNumber("Turret Position", position); // align(); ligament2d.setAngle(position); - } public double getAppliedVoltage() { -- 2.39.5