a

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 628716a65eeed3d930ca0fd0492e6472891ee059..8f8b01516708e7e96ddb581e246a6257807cdc42 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -145,7 +145,7 @@ public class RobotContainer {
           e.printStackTrace();
         }
         if(turret != null){
-          turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
+          //turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
         }
         drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
         break;

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index dfb0ba9cb4e14f13fc3824fca25c429b84b3762d..d896b0dfd5520109e1528f1e69294ed6b0a1f006 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -62,10 +62,12 @@ public class Superstructure extends Command {
 
     private final double phaseDelay = 0.03; // Extrapolation delay due to latency
 
-    private Translation2d target = null;
+    private Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
 
     private PhaseManager phaseManager = new PhaseManager();
 
+    private double hoodOffset = 0.0;
+
     public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
         this.turret = turret;
         this.drivetrain = drivetrain;
@@ -143,6 +145,8 @@ public class Superstructure extends Command {
         
         lastTurretAngle = turretAngle;
 
+        Logger.recordOutput("Turret/Target Pose", target);
+
         Logger.recordOutput("Lookahead Pose", lookaheadPose);
 
         // Subtract the rotational angle of the robot from the setpoint
@@ -200,9 +204,11 @@ public class Superstructure extends Command {
 
     @Override
     public void execute() {
+        hoodOffset = SmartDashboard.getNumber("Hood Offset", hoodOffset);
+        SmartDashboard.putNumber("Hood Offset", hoodOffset);
         // Phase manager stuff
         phaseManager.update(drivepose, shooter, turret);
-        target = phaseManager.getTarget();
+        //target = phaseManager.getTarget();
 
         updateDrivePose();
         updateSetpoints(drivepose);
@@ -213,12 +219,12 @@ public class Superstructure extends Command {
             turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
             
             if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){
-                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
+                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE - hoodOffset), 0.0);
             } else{
                 hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity);
             }
 
-            shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
+            shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
             // if (phaseManager.shouldFeed()) {
             //     spindexer.maxSpindexer();

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index dd5e1710d85037315374f79fb297401929b1fd74..64825eb5aba937a9cee9e8411488a7a2524376ed 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -16,6 +16,7 @@ import frc.robot.commands.gpm.AutoShootCommand;
 import frc.robot.commands.gpm.ClimbDriveCommand;
 import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.Superstructure;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.drivetrain.Drivetrain;
@@ -146,7 +147,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
 
         // Auto shoot
         if (turret != null && hood != null && shooter != null) {
-            autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
+            autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
             controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
         }
 

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index d48f879a644df40b40299b2b1219ae70e687caaa..1dbf65fb13f42cfd66d46042628e5b50965a6e77 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -72,10 +72,10 @@ public class Turret extends SubsystemBase implements TurretIO{
                TalonFXConfiguration config = new TalonFXConfiguration();
                config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
     
-               config.Slot0.kP = 12.0; 
+               config.Slot0.kP = 10.0; 
                config.Slot0.kS = 0.1; // Static friction compensation
                config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio
-               config.Slot0.kD = 0.15; // The "Braking" term to stop overshoot
+               config.Slot0.kD = 0.20; // The "Braking" term to stop overshoot
 
                var mm = config.MotionMagic;
                mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO;
@@ -122,7 +122,7 @@ public class Turret extends SubsystemBase implements TurretIO{
                //Sets the initial motor position
                motor.setPosition(motorRotations);
 
-               motor.setPosition(0.0);
+               motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
 
                SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);}));
                SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);}));
@@ -256,7 +256,7 @@ public class Turret extends SubsystemBase implements TurretIO{
 
        @Override
        public void updateInputs() {
-               // inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+               inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
                inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
                inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
         inputs.encoderLeftRot = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble());