From fa62005ef5301cb71afbc9d85005d5cc25c04b9a Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Tue, 3 Feb 2026 16:42:43 -0800 Subject: [PATCH] a --- .../frc/robot/commands/gpm/SimpleAutoShoot.java | 17 ++++++++++++++--- .../controls/PS5ControllerDriverConfig.java | 4 ++-- .../frc/robot/subsystems/turret/Turret.java | 15 ++++++++++++--- 3 files changed, 28 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java index 663b5fc..0e544c7 100644 --- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java @@ -2,6 +2,8 @@ package frc.robot.commands.gpm; import java.lang.reflect.Field; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; @@ -37,11 +39,14 @@ public class SimpleAutoShoot extends Command { private boolean turretVisionEnabled = false; private boolean SOTM = true; private Translation2d drivepose; + private double lastPos = 0; private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); private double lastTurretAngle = 0; private double lastTurretVelocity = 0; + private double lastFrameVelocity; + public SimpleAutoShoot(Turret turret, Drivetrain drivetrain, Shooter shooter) { this.turret = turret; this.drivetrain = drivetrain; @@ -129,14 +134,20 @@ public class SimpleAutoShoot extends Command { // turretAngleFilter.calculate( // new Rotation2d(Units.degreesToRadians(turretSetpoint)).minus(new Rotation2d(Units.degreesToRadians(lastTurretAngle))).getRadians() / Constants.LOOP_TIME); - double turretAcceleration = ((-drivetrain.getAngularRate(2)) - (lastTurretVelocity)) / Constants.LOOP_TIME; + double velocityAdjustment = 0; + double turretAcceleration = ((-drivetrain.getAngularRate(2)) - (lastFrameVelocity)) / Constants.LOOP_TIME; + if (Math.abs(lastTurretAngle - turretSetpoint) > 90) { + velocityAdjustment = -drivetrain.getAngularRate(2) * 1.4; + } + Logger.recordOutput("Spinny accel", drivetrain.getAngularRate(2)); + Logger.recordOutput("Original Turret Setpoint", turretSetpoint); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2)); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) - velocityAdjustment); // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3); //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity); lastTurretAngle = turretSetpoint; - lastTurretVelocity = -drivetrain.getAngularRate(2); + lastFrameVelocity = drivetrain.getAngularRate(2); } @Override diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index f7fba67..37cd9d3 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -99,11 +99,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { driver.get(PS5Button.CIRCLE).onTrue( new InstantCommand(()->{ - turret.setFieldRelativeTarget(new Rotation2d(Math.PI), 0); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0); }) ).onFalse( new InstantCommand(()->{ - turret.setFieldRelativeTarget(new Rotation2d(0), 0); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0); }) ); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 1c8c2aa..8a1630f 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -40,8 +40,8 @@ public class Turret extends SubsystemBase implements TurretIO{ private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE); private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE); - private static final double MAX_VEL_RAD_PER_SEC = 16.0; - private static final double MAX_ACCEL_RAD_PER_SEC2 = 80.0; + private static final double MAX_VEL_RAD_PER_SEC = 25; + private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0; private static final double VERSA_RATIO = 5.0; private static final double TURRET_RATIO = 140.0 / 10.0; @@ -54,6 +54,8 @@ public class Turret extends SubsystemBase implements TurretIO{ private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private double lastFrameVelocity = 0.0; + /* ---------------- Hardware ---------------- */ private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN); @@ -116,6 +118,8 @@ public class Turret extends SubsystemBase implements TurretIO{ } }); + // profile = new TrapezoidProfile(new Constraints(MAX_VEL_RAD_PER_SEC, feedForward.maxAchievableAcceleration(DCMotor.getKrakenX60(1, GEAR_RATIO), goalVelocityRadPerSec)))) + setpoint = new State(getPositionRad(), 0.0); lastGoalRad = setpoint.position; @@ -204,9 +208,12 @@ public class Turret extends SubsystemBase implements TurretIO{ // motor.setVoltage(voltage); // } else{ // in rad/sec + //double robotRotAcceleration = (Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) - lastFrameVelocity) / Constants.LOOP_TIME; + double motorSetpointPosition = (setpoint.position) * GEAR_RATIO; + targetVelocity = positionPID.calculate( motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians), - setpoint.position * GEAR_RATIO); + motorSetpointPosition); targetVelocity += Units.rotationsToRadians(motorVelRotPerSec); @@ -217,6 +224,7 @@ public class Turret extends SubsystemBase implements TurretIO{ motor.setVoltage(voltage); // } + lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()); // var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false); Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); @@ -235,6 +243,7 @@ public class Turret extends SubsystemBase implements TurretIO{ Units.radiansToDegrees(best)); SmartDashboard.putNumber("Turret SetpointDeg", Units.radiansToDegrees(setpoint.position)); + SmartDashboard.putNumber("Turret Raw Setpoint", Units.radiansToDegrees(best)); SmartDashboard.putNumber("Turret motorPosRot", Units.radiansToDegrees(motorPosRot)); SmartDashboard.putNumber("Turret motorVelRotPerSec", -- 2.39.5