simple auto shoot fixed useless shit

diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
index 7ea06b564da31133b365df59a792fcd06f183be3..902ec7c5e3a2c768a0b2f4e69ed090d03c5f8a3c 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
@@ -1,19 +1,16 @@
 package frc.robot.commands.gpm;
 
-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;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
-import frc.robot.constants.FieldConstants.FieldZone;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.turret.Turret;
 
 public class SimpleAutoShoot extends Command {
@@ -23,25 +20,13 @@ public class SimpleAutoShoot extends Command {
 
     private double fieldAngleRad;
     private double turretSetpoint;
-    private double adjustedSetpoint;
-    private double yawToTagCamera;
-    private double yawToTag;
 
-    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;
-        this.shooter = shooter;
         drivepose  = drivetrain.getPose().getTranslation();
         
         addRequirements(turret);
@@ -79,69 +64,32 @@ public class SimpleAutoShoot 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);
-    }
-
-    public void updateYawToTag(){
-        // Calculate the yaw offset to the tag
-        double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY();
-        double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX();
-        double angleToTag = Units.radiansToDegrees(Math.atan(D_y / D_x));
-        yawToTag = angleToTag - Units.radiansToDegrees(fieldAngleRad);
     }
 
     @Override
     public void initialize() {
         // Initialize setpoint calculation and set the initial goal for the turret
         updateTurretSetpoint(drivepose);
-        // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2));
         turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0);
     }
 
     @Override
     public void execute() {
-        //shooter.setShooterPower(ShotInterpolation.shooterPowerMap.get(FieldConstants.getHubTranslation().toTranslation2d().getDistance(drivetrain.getPose().getTranslation())));
         // Continuously update setpoints and adjust based on vision if available
         drivepose = drivetrain.getPose().getTranslation();
         updateTurretSetpoint(drivepose);
-        updateYawToTag();
-
-        // double turretVelocity =
-        // turretAngleFilter.calculate(
-        //     new Rotation2d(Units.degreesToRadians(turretSetpoint)).minus(new Rotation2d(Units.degreesToRadians(lastTurretAngle))).getRadians() / 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) - velocityAdjustment);
-        // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3);
-        //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity);
-
-        lastTurretAngle = turretSetpoint;
-        lastFrameVelocity = drivetrain.getAngularRate(2);
+        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
+        shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY);
+        
     }
 
     @Override
     public void end(boolean interrupted) {
         // Set the turret to a safe position when the command ends
         turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
+        shooter.setShooter(0.0);
     }
 
-    public boolean leftSide(Translation2d drivepose) {
-        if (drivepose.getY() > (FieldConstants.FIELD_WIDTH / 2)) {
-            return true;
-        } else {
-            return false;
-        }
-    }
-
-    public FieldZone getZone(Translation2d drivepose) {
-        return FieldConstants.getZone(drivepose);
-    }
 }