fdas

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index 07d70a478ad830ca8aa7bbcc278f464390751cd6..ba33c045567602e9c24a85e49cc80e41d784e4ed 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -16,7 +16,7 @@ import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.turret.ShotInterpolation;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.turret.TurretConstants;
-import frc.robot.util.FieldZone;
+// import frc.robot.util.FieldZone;
 
 public class TurretAutoShoot extends Command {
     private Turret turret;
@@ -127,8 +127,8 @@ public class TurretAutoShoot extends Command {
         }
     }
 
-    public FieldZone getZone(Translation2d drivepose) {
-        return FieldConstants.getZone(drivepose);
-    }
+    // public FieldZone getZone(Translation2d drivepose) {
+    //     return FieldConstants.getZone(drivepose);
+    // }
 }
 

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 3d8eca0610bd3b035990fd0409627158083d6129..cf47b347da2b6dee964c3bd7925a9c8fca2d070b 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -95,6 +95,16 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             })
         );
 
+        driver.get(PS5Button.CIRCLE).onTrue(
+            new InstantCommand(()->{
+                turret.setFieldRelativeTarget(new Rotation2d(Math.PI), 0);
+            })
+        ).onFalse(
+            new InstantCommand(()->{
+                turret.setFieldRelativeTarget(new Rotation2d(0), 0);
+            })
+        );
+
         // driver.get(PS5Button.CROSS).onTrue(
         //     new InstantCommand(()->{
         //         if(turretJoyStickAim == null || !turretJoyStickAim.isScheduled()){

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
index f55b8f9fb8458d28cf22bca842e5a918092c03cc..3119152a1b5639ebd83da5dd11f279df47e7e636 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
@@ -206,8 +206,6 @@ public class Module implements ModuleIO{
     }
     
     public void periodic() {
-        SmartDashboard.putNumber("encoder offset for " + getModuleType(), Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble()));
-
         updateInputs();
         Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
 

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 471f8b0c6648105f9f6fe27a90ff49c17296565d..1be64d16578df923804784e740483a678c5b98b7 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -1,9 +1,11 @@
 package frc.robot.subsystems.turret;
 
 import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.sim.TalonFXSimState;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
@@ -18,6 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc.robot.constants.Constants;
@@ -33,7 +36,7 @@ public class Turret extends SubsystemBase {
         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 = 1e7;
+    private static final double MAX_ACCEL_RAD_PER_SEC2 = 80.0;
 
     private static final double VERSA_RATIO = 5.0;
     private static final double TURRET_RATIO = 140.0 / 10.0;
@@ -91,6 +94,15 @@ public class Turret extends SubsystemBase {
             new com.ctre.phoenix6.configs.TalonFXConfiguration() {{
             MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
             }});
+        
+        motor.getConfigurator().apply(
+            new TalonFXConfiguration() {{
+                Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
+            }}
+        );
+
+        setpoint = new State(getPositionRad(), 0.0);
+        lastGoalRad = setpoint.position;
 
         if (RobotBase.isSimulation()) {
         simState = motor.getSimState();
@@ -175,10 +187,18 @@ public class Turret extends SubsystemBase {
         // --- Visualization ---
         ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
 
-        SmartDashboard.putNumber("Turret/GoalDeg",
+        SmartDashboard.putNumber("Turret GoalDeg",
             Units.radiansToDegrees(best));
-        SmartDashboard.putNumber("Turret/SetpointDeg",
+        SmartDashboard.putNumber("Turret SetpointDeg",
             Units.radiansToDegrees(setpoint.position));
+        SmartDashboard.putNumber("Turret motorPosRot",
+            Units.radiansToDegrees(motorPosRot));
+        SmartDashboard.putNumber("Turret motorVelRotPerSec",
+            Units.radiansToDegrees(motorVelRotPerSec));
+        SmartDashboard.putNumber("Turret Position Deg",
+            Units.radiansToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO);
+
+        // SmartDashboard.putData("Spin to 90 deg", new InstantCommand(()->{setFieldRelativeTarget(new Rotation2d(Math.PI), 0);}));
     }
 
     /* ---------------- Simulation ---------------- */