aaa

diff --git a/src/main/java/frc/robot/commands/gpm/DoSysidThings.java b/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
index 762426581c322a98892e816e7fc47eb303c3ce7b..e860a2aaa4ba9bdf119790043afab13693f31e75 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
+++ b/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
@@ -22,8 +22,8 @@ public class DoSysidThings extends SequentialCommandGroup {
             routine.quasistatic(SysIdRoutine.Direction.kForward),
             routine.quasistatic(SysIdRoutine.Direction.kReverse),
             routine.dynamic(SysIdRoutine.Direction.kForward),
-            routine.dynamic(SysIdRoutine.Direction.kReverse),
-        )
+            routine.dynamic(SysIdRoutine.Direction.kReverse)
+        );
     }
 
     private void doLog(SysIdRoutineLog log) {

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index ff78d0973c6f8a1466251ed7a7e69c003257b77d..2949fd16747a483e9135aab9e5657586fac17478 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -30,11 +30,12 @@ public class TurretAutoShoot extends Command {
 
     private boolean turretVisionEnabled = false;
     private boolean SOTM = true;
-    private Translation2d drivepose = drivetrain.getPose().getTranslation();
+    private Translation2d drivepose;
     public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision) {
         this.turret = turret;
         this.drivetrain = drivetrain;
         this.turretVision = turretVision;
+        drivepose  = drivetrain.getPose().getTranslation();
         
         addRequirements(turret);
     }
@@ -89,7 +90,7 @@ public class TurretAutoShoot extends Command {
         // Calculate turret setpoint (angle relative to robot heading)
         turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
 
-        System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
+        // System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
     }
 
     public void adjustWithTurretCam() {
@@ -131,7 +132,7 @@ public class TurretAutoShoot extends Command {
             adjustWithTurretCam();
             turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(adjustedSetpoint)), -drivetrain.getAngularRate(2));
         } else{
-            turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
+            turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) * 1.0);
         }
     }
 

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 94f331d72566f19c337484f2442816509b551e19..a668993b680b774ea497a508c722743c6928b023 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -37,8 +37,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 double MAX_VEL_RAD_PER_SEC = 4*Units.degreesToRadians(360);
-    private static double MAX_ACCEL_RAD_PER_SEC2 = 4*Units.degreesToRadians(720);
+    private static double MAX_VEL_RAD_PER_SEC = 100;
+    private static double MAX_ACCEL_RAD_PER_SEC2 = 500000000;
 
     private static final double VERSA_RATIO = 5.0;
     private static final double TURRET_RATIO = 140.0 / 10.0;
@@ -76,14 +76,17 @@ public class Turret extends SubsystemBase implements TurretIO{
 
     /* ---------------- Tuning ---------------- */
 
-    private double kP = 15.0;
+    private double kP = 12.0;
     private double kD = 0.0;
 
     /* ---------------- Constructor ---------------- */
 
     public Turret() {
+               SmartDashboard.putNumber("FF Value", 0.1);
+
         motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN);
         motor.setNeutralMode(NeutralModeValue.Brake);
+               motor.setPosition(0);
 
         TalonFXConfiguration cfg = new TalonFXConfiguration();
         cfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
@@ -165,6 +168,7 @@ public class Turret extends SubsystemBase implements TurretIO{
 
     /* ---------------- Periodic ---------------- */
 
+       double FFValue;
     @Override
     public void periodic() {
         updateInputs();
@@ -202,10 +206,13 @@ public class Turret extends SubsystemBase implements TurretIO{
         double motorRot =
             Units.radiansToRotations(setpoint.position) * GEAR_RATIO;
 
-        motor.setControl(mmRequest.withPosition(motorRot));
+               FFValue = SmartDashboard.getNumber("FF Value", FFValue);
+        motor.setControl(mmRequest.withPosition(motorRot).withFeedForward(FFValue));
 
         ligament.setAngle(getPositionDeg());
 
+               FFValue = SmartDashboard.getNumber("FF Value", FFValue);
+
         SmartDashboard.putNumber("Turret Pos Deg", getPositionDeg());
         SmartDashboard.putNumber("Turret Goal Deg", Units.radiansToDegrees(best));
     }