Fix bugs and add alignment value

- Weston J

Author: PhyXTGears-1720

src/main/deploy/config.toml

@@ -26,6 +26,10 @@ distanceMultiplierForwards = 0.65
 distanceMultiplierStafing = 0.07
 distanceMultiplierTurning = 0.4
 
+LEFT_OFFSET = 8.2
+RIGHT_OFFSET = -8.2
+CENTER_OFFSET = 0.0
+
 LIMELIGHT_INIT_FAILED_TIME = 0.4
 
 STOP_MOVING_THERSHOLD = 0.58

src/main/java/frc/robot/Robot.java

@@ -170,12 +170,12 @@ public void teleopPeriodic() {
         if (leftBumper || rightBumper) {
             System.out.println("Driving to april tag...");
 
-            mVisionTeleopCommand.mOffset = leftBumper ? 8.2 : -8.2;
+            mVisionTeleopCommand.setOffset(leftBumper ? Vision.Alignment.LEFT : Vision.Alignment.RIGHT);
 
             kDriveTeleopCommand.cancel();
             mVisionTeleopCommand.schedule();
 
-        } else if (kDriveController.getLeftBumperButtonReleased()) {
+        } else if (kDriveController.getLeftBumperButtonReleased() || kDriveController.getRightBumperButtonReleased()) {
             mVisionTeleopCommand.cancel();
             kDriveTeleopCommand.schedule();
             System.out.println("Cancel");

src/main/java/frc/robot/commands/visionDriveCommand/VisionAutoCommand.java

@@ -14,16 +14,19 @@
 
 public class VisionAutoCommand extends Command {
 
-    // config
+    // vars
+
+    private double mOffset;
 
     private final Vision kVisionSubsystem;
     private final Drivetrain kDrivetrain;
     private final Time kRobotPeriod;
 
     private boolean mIsDoneStafing = false;
     private boolean mIsDoneMoving = false;
+    private boolean mDoneAutoInit = false;
 
-    private final int mTargetDriveAprilTagId;
+    private int mTargetDriveAprilTagId;
 
     private AngularVelocity mAngularVelocity = Units.RadiansPerSecond.zero();
     private LinearVelocity mStafeVelocity = Units.MetersPerSecond.zero();
@@ -34,9 +37,10 @@ public class VisionAutoCommand extends Command {
     public VisionAutoCommand(
             Vision visionSubsystem,
             Drivetrain drivetrain,
-            int targetDriveAprilTagId,
+            int targetTagId1,
+            int targetTagId2,
             Time period) {
-        mTargetDriveAprilTagId = targetDriveAprilTagId;
+
         kVisionSubsystem = visionSubsystem;
         kDrivetrain = drivetrain;
         kRobotPeriod = period;
@@ -46,6 +50,10 @@ public VisionAutoCommand(
         addRequirements(kDrivetrain, kVisionSubsystem);
     }
 
+    public void setOffset(Vision.Alignment alignment) {
+        mOffset = mConfig.getOffset(alignment);
+    }
+
     @Override
     public void initialize() {
         // reset some vars
@@ -60,7 +68,7 @@ public void execute() {
         if (mTargetDriveAprilTagId == -1)
             return;
 
-        Vision.VisionTagData data = kVisionSubsystem.robotDriveToAprilTag(mTargetDriveAprilTagId, 0.0);
+        Vision.VisionTagData data = kVisionSubsystem.robotDriveToAprilTag(mTargetDriveAprilTagId, mOffset);
 
         double heading = kDrivetrain.getHeading().in(Units.Degree);
         double targetHeading = fmod(Vision.rotations[mTargetDriveAprilTagId - 1], 360.0, 180.0);
@@ -172,11 +180,11 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        if (mIsDoneMoving && !mIsDoneStafing) {
-            System.out.println("stafing is holding us up");
-        } else if (mIsDoneStafing && !mIsDoneMoving) {
-            System.out.println("moving is holding us up");
-        }
+        // if (mIsDoneMoving && !mIsDoneStafing) {
+        //     System.out.println("stafing is holding us up");
+        // } else if (mIsDoneStafing && !mIsDoneMoving) {
+        //     System.out.println("moving is holding us up");
+        // }
 
         if ((mIsDoneStafing && mIsDoneMoving)) {
 

src/main/java/frc/robot/commands/visionDriveCommand/VisionTeleopCommand.java

@@ -15,9 +15,9 @@
 
 public class VisionTeleopCommand extends Command {
 
-    // config
+    // vars
 
-    public double mOffset;
+    private double mOffset;
 
     private final Vision kVisionSubsystem;
     private final Drivetrain kDrivetrain;
@@ -53,6 +53,10 @@ public VisionTeleopCommand(
         addRequirements(kDrivetrain, kVisionSubsystem);
     }
 
+    public void setOffset(Vision.Alignment alignment) {
+        mOffset = mConfig.getOffset(alignment);
+    }
+
     @Override
     public void initialize() {
         // reset some vars
@@ -100,6 +104,7 @@ public void execute() {
 
         kVisionSubsystem.setFilter(-1);
 
+
         // System.out.println("[drive progress] " + Boolean.toString(mIsDoneMoving) + ",
         // " + Boolean.toString(mIsDoneStafing));
 
@@ -200,11 +205,11 @@ public void execute() {
     public boolean isFinished() {
         boolean limelightInitFailed = (kTimer.get() >= mConfig.LIMELIGHT_INIT_FAILED_TIME && !mDoneInitTeleop);
 
-        if (mIsDoneMoving && !mIsDoneStafing) {
-            System.out.println("stafing is holding us up");
-        } else if (mIsDoneStafing && !mIsDoneMoving) {
-            System.out.println("moving is holding us up");
-        }
+        // if (mIsDoneMoving && !mIsDoneStafing) {
+        //     System.out.println("stafing is holding us up");
+        // } else if (mIsDoneStafing && !mIsDoneMoving) {
+        //     System.out.println("moving is holding us up");
+        // }
 
         if (limelightInitFailed) {
             System.out.println("Limelight init failed");