From 4819e36f01f7bb91c793afe0a89623079aabb867 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 14 Feb 2026 16:16:43 -0800 Subject: [PATCH] i'm cooked --- .../robot/commands/gpm/AutoShootCommand.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 18 ++++++------ .../subsystems/turret/ShotInterpolation.java | 5 ++++ .../frc/robot/subsystems/turret/Turret.java | 28 ++++++++++--------- .../frc/robot/subsystems/turret/TurretIO.java | 1 + 5 files changed, 32 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 4f1ce7c..ec744f4 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -207,7 +207,7 @@ public class AutoShootCommand extends Command { turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); - shooter.setShooter(goalState.exitVel()); + //shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b029e1c..d12c69d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -48,10 +48,10 @@ public class Shooter extends SubsystemBase implements ShooterIO { updateInputs(); TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.1; //tune p value + config.Slot0.kP = 0.15; //tune p value config.Slot0.kI = 0; - config.Slot0.kD = 0; - config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps + config.Slot0.kD = 0.0; + config.Slot0.kV = 0.125; //Maximum rps = 100 --> 12V/100rps shooterMotorLeft.getConfigurator().apply(config); shooterMotorRight.getConfigurator().apply(config); @@ -64,7 +64,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) ); - SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(20.0))); + SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0))); } @Override @@ -73,8 +73,10 @@ public class Shooter extends SubsystemBase implements ShooterIO { powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier); SmartDashboard.putNumber("shooter power modifier", powerModifier); - shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier)); - shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier)); + shooterMotorLeft.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)))); + shooterMotorRight.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)))); + + Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed); } public void setShooter(double linearVelocityMps) { @@ -88,8 +90,8 @@ public class Shooter extends SubsystemBase implements ShooterIO { @Override public void updateInputs(){ - inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()); - inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble()); + inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; + inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())* ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; Logger.processInputs("Shooter", inputs); } diff --git a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java index c265cec..67486bd 100644 --- a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java +++ b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java @@ -7,6 +7,8 @@ public class ShotInterpolation { public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap(); public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap(); + + public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap(); static{ timeOfFlightMap.put(0.0, 0.67); @@ -17,5 +19,8 @@ public class ShotInterpolation { hoodAngleMap.put(0.0, Units.degreesToRadians(90)); hoodAngleMap.put(1.0, Units.degreesToRadians(90)); + + exitVelocityMap.put(1.0, 2.0); + exitVelocityMap.put(2.0, 4.0); } } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 2a7721d..f856892 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -56,12 +56,13 @@ public class Turret extends SubsystemBase implements TurretIO{ private static final double MAX_VEL_RAD_PER_SEC = 15; //private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now // private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0; - private static final double MAX_ACCEL_RAD_PER_SEC2 = 60.0; + private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0; private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO; - private static final PIDController positionPID = new PIDController(15, 0, 0.25); - private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0); + private static final PIDController positionPID = new PIDController(15.0, 0, 0.0); + // private static final PIDController positionPID = new PIDController(15, 0, 0.25); + private static final PIDController velocityPID = new PIDController(0.2, 0.0, 0.0); private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN); private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN); @@ -105,7 +106,7 @@ public class Turret extends SubsystemBase implements TurretIO{ private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0)); // private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010); - private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); + private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, (1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt) * 0.8, 0); private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0); /* ---------------- Constructor ---------------- */ @@ -113,10 +114,10 @@ public class Turret extends SubsystemBase implements TurretIO{ public Turret() { motor.setNeutralMode(NeutralModeValue.Coast); - motor.getConfigurator().apply( - new Slot0Configs() - .withKP(kP) - .withKD(kD)); + // motor.getConfigurator().apply( + // new Slot0Configs() + // .withKP(kP) + // .withKD(kD)); TalonFXConfiguration config = new TalonFXConfiguration(); var motionMagicConfigs = config.MotionMagic; @@ -228,17 +229,17 @@ public class Turret extends SubsystemBase implements TurretIO{ double motorSetpointPosition = (setpoint.position) * GEAR_RATIO; - targetVelocity = positionPID.calculate( + targetVelocity = + positionPID.calculate( motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians), motorSetpointPosition); - //targetVelocity += Units.rotationsToRadians(motorVelRotPerSec); // TODO: Change this back after full rotation - targetVelocity += Math.abs(setpoint.position) != Math.PI/2 ? Units.rotationsToRadians(motorVelRotPerSec) : 0; + targetVelocity += Units.rotationsToRadians(motorVelRotPerSec) * 1.0; double voltage = feedForward.calculate(targetVelocity); - double velocityCorrectionVoltage = velocityPID.calculate(Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()), targetVelocity); - voltage += velocityCorrectionVoltage; + // double velocityCorrectionVoltage = velocityPID.calculate(Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()), targetVelocity); + // voltage += velocityCorrectionVoltage; motor.setVoltage(voltage); @@ -279,6 +280,7 @@ public class Turret extends SubsystemBase implements TurretIO{ inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); inputs.encoderLeftRot = encoderLeft.getAbsolutePosition().getValueAsDouble(); inputs.encoderRightRot = encoderRight.getAbsolutePosition().getValueAsDouble(); + inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); } private double wrapUnit(double value) { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index 44c1be3..a7bd928 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -10,6 +10,7 @@ public interface TurretIO { public double motorCurrent = 0; public double encoderLeftRot = 0; public double encoderRightRot = 0; + public double motorVoltage = 0; } public void updateInputs(); -- 2.39.5