a

diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
index 663b5fcb1c94a2c056539ab2deeb5ba2744da0d4..0e544c7aa9591f28cb9240db07ee5fa5d20d8a80 100644 (file)
--- 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 f7fba67bea3f55897bf1c5448a3dd70a55e9244b..37cd9d3fb42e5d2bfe6756c2dd7af492810ae91f 100644 (file)
--- 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 1c8c2aa464212e31d13afabed818af960dcd5a0c..8a1630f5467651fe6d928aab3a59b1d46c3c572e 100644 (file)
--- 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",