fully refactor swerve aim, change position to the amp corner

grt192 · Apr 2, 2024 · c66855a · c66855a
1 parent 796a4c3
commit c66855a
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/commands/auton/SetCalculatedAngleCommand.java b/src/main/java/frc/robot/commands/auton/SetCalculatedAngleCommand.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.SwerveConstants;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 
 /** SetCalculatedAngleCommand. */
@@ -12,11 +13,18 @@ public class SetCalculatedAngleCommand extends Command {
     public SetCalculatedAngleCommand(SwerveSubsystem swerve) {
         this.swerve = swerve;
         addRequirements(swerve);
+
     } 
 
+    @Override
+    public void initialize() {
+        swerve.targetSpeaker();
+        swerve.setAim(true);
+    }
+
     @Override
     public void execute() {
-        swerve.setSwerveAimDrivePowers(0, 0);
+        swerve.setDrivePowers(0, 0, 0);
     }
 
     @Override
@@ -27,5 +35,6 @@ public boolean isFinished() {
     @Override
     public void end(boolean interrupted) {
         swerve.setRobotRelativeDrivePowers(0, 0, 0);
+        swerve.setAim(false);
     }
 }
diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelShuttleCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelShuttleCommand.java
@@ -18,8 +18,8 @@ public class ShooterFlywheelShuttleCommand extends Command {
     private static final double ERROR_MULTIPLIER = 0.08;
     private static final double TARGET_ANGLE = Units.degreesToRadians(70);
 
-    private final Translation2d BLUE_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(114), Units.inchesToMeters(323 - 96));
-    private final Translation2d RED_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(652.73 - 114), Units.inchesToMeters(323 - 96));
+    private final Translation2d BLUE_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(0), Units.inchesToMeters(323));
+    private final Translation2d RED_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(323));
 
     private final SwerveSubsystem swerveSubsystem;
     private final ShooterFlywheelSubsystem flywheelSubsystem;

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -56,6 +56,7 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.SwerveConstants;
 import frc.robot.util.GRTUtil;
 import frc.robot.vision.ApriltagWrapper;
 import java.util.Optional;
@@ -275,6 +276,8 @@ public void periodic() {
             printModuleAngles();
         }
 
+        System.out.println(aiming);
+
     }
 
     /** Executes swerve X locking, putting swerve's wheels into an X configuration to prevent motion.
@@ -412,6 +415,10 @@ public void setTargetPoint(Translation2d targetPoint) {
         this.targetPoint = targetPoint;
     }
 
+    public void targetSpeaker(){
+        targetPoint = redSupplier.getAsBoolean() ? SwerveConstants.RED_SPEAKER_POS : SwerveConstants.BLUE_SPEAKER_POS;
+    }
+
     public ChassisSpeeds getAimChassisSpeeds(ChassisSpeeds currentSpeeds){
         if (aiming) {
             Rotation2d currentRotation = getRobotPosition().getRotation();