a

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 575a4251f25e201ec63769f86775afa5923cd9d3..68afc6f2467b0dcdd5620f1d1f17be7ad59c039b 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -86,7 +86,7 @@ public class RobotContainer {
       case Vertigo:
         vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
         turret = new Turret();
-        shooter = new Shooter();
+        //shooter = new Shooter();
 
         drive = new Drivetrain(vision, new GyroIOPigeon2());
         driver = new PS5ControllerDriverConfig(drive, shooter, turret);

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index 8864ab5f3ce2c381e93a448949f7da34e22ccec7..34cf371103973e532c74d4cc1d0fe3dab3d1324a 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -1,5 +1,7 @@
 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.Pose2d;
@@ -44,9 +46,10 @@ public class TurretAutoShoot extends Command {
     public void updateTurretSetpoint(Pose2d drivepose) {
         
         Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
-        Pose2d turretPosition = drivepose.transformBy(new Transform2d((TurretConstants.DISTANCE_FROM_ROBOT_CENTER), new Rotation2d()));
-        double turretToTargetDistance = target.getDistance(turretPosition.getTranslation());
-        
+        Pose2d turretPosition = drivepose;//.transformBy(new Transform2d((TurretConstants.DISTANCE_FROM_ROBOT_CENTER), new Rotation2d()));
+        //double turretToTargetDistance = target.getDistance(turretPosition.getTranslation());
+        Logger.recordOutput("brrr", turretPosition);
+
         // If the robot is moving, adjust the target position based on velocity
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
         ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
@@ -69,7 +72,7 @@ public class TurretAutoShoot extends Command {
 
         // Loop (20) until lookahreadTurretToTargetDistance converges
         //for (int i = 0; i < 20; i++) {
-            timeOfFlight = 1.0;
+            timeOfFlight = 0.0;
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
             lookaheadPose =
@@ -78,7 +81,10 @@ public class TurretAutoShoot extends Command {
                     turretPosition.getRotation());
             //lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation());
         //}
-        turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
+        turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();//.minus(new Rotation2d(Math.PI/4));
+        Logger.recordOutput("lookahead pose", lookaheadPose);
+        Logger.recordOutput("target pose", target);
+        System.out.println(turretAngle);
         if (lastTurretAngle == null) {
             lastTurretAngle = turretAngle;
         }
@@ -87,25 +93,25 @@ public class TurretAutoShoot extends Command {
             turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
         lastTurretAngle = turretAngle;
 
-        // Add 180 since drivetrain is backwards
-        double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() + Math.PI);
+        double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
         turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint), -180.0, 180.0);
     }
 
     public void updateDrivePose(){
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
-        drivepose = drivetrain.getPose().exp(
-            new Twist2d(
-                robotRelVel.vxMetersPerSecond * phaseDelay,
-                robotRelVel.vyMetersPerSecond * phaseDelay,
-                robotRelVel.omegaRadiansPerSecond * phaseDelay));
+        drivepose = drivetrain.getPose().rotateBy(new Rotation2d(Math.PI));
+        // .exp(
+        //     new Twist2d(
+        //         robotRelVel.vxMetersPerSecond * phaseDelay,
+        //         robotRelVel.vyMetersPerSecond * phaseDelay,
+        //         robotRelVel.omegaRadiansPerSecond * phaseDelay));
     }
 
     @Override
     public void execute() {
         updateDrivePose();
         updateTurretSetpoint(drivepose);
-        //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
+        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
         System.out.println("Turret Setpoint: " + turretSetpoint);
         //System.out.println("Turret goal velocity" + (turretVelocity - drivetrain.getAngularRate(2)));
     }

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index 5ec836d2a33e28c157b4d02c04a2465850dfc007..781871064718b64af9fee22f5b9e270a089edc2d 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -27,7 +27,7 @@ public class FieldConstants {
 
   /** Location of hub target */
   public static final Translation3d HUB_BLUE =
-      new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72));
+      new Translation3d(Units.inchesToMeters(156.8), 4.035, Units.inchesToMeters(72));
     
   // TODO: Update all of this
   public static final Translation3d NEUTRAL_LEFT =

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 37cd9d3fb42e5d2bfe6756c2dd7af492810ae91f..373aefc7d37ca8bce0524cb2a5bd9ebf1e78b8b8 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -91,6 +91,17 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                     })
         );
 
+        driver.get(PS5Button.TRIANGLE).onTrue(
+            new InstantCommand(()->{
+                        if (turretAutoShoot != null && turretAutoShoot.isScheduled()){
+                            turretAutoShoot.cancel();
+                        } else{
+                            turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain());
+                            CommandScheduler.getInstance().schedule(turretAutoShoot);
+                        }
+                    })
+        );
+
         driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> shooter.setFeeder(1))).onFalse(
             new InstantCommand(()->{
                 shooter.setFeeder(0);

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 11f3b96d8873e865b080b9ceaca28e5ce8ac126a..38b55a5281ca9bd4e107334a351fa261a71284ab 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -102,7 +102,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     }
 
     public void periodic() {
-        runVelocity(Units.rotationsToRadians(shooterTargetSpeed));
+        //runVelocity(Units.rotationsToRadians(shooterTargetSpeed));
         SmartDashboard.putNumber("Shot Power", shooterPower);
         shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);