Merge pull request #51 from FRCTeam3255/advantagescope

Beach Blitz Changes

README.md

@@ -1,8 +1,9 @@
 # FRC Team 3255's 2023 Off-Season Robot
+Not a newcomer? Skip to the code [`here!`](src/main/java/)
 
 View videos of this robot on our [Instagram]() and [YouTube]()!
 
-This robot will be competing at the following competitions:
+This robot competed at the following competitions:
 
 - [*Battle At The Border*](https://battleattheborder.wordpress.com/)
 - [*Beach Blitz*](https://www.thebluealliance.com/event/2023cabl)

src/main/deploy/pathplanner/cableCoCoCoYeetDock.path

@@ -25,15 +25,15 @@
     {
       "anchorPoint": {
         "x": 5.7405146345456,
-        "y": 1.05
+        "y": 0.95
       },
       "prevControl": {
         "x": 4.6045569650116525,
-        "y": 1.0499999999999998
+        "y": 0.9499999999999993
       },
       "nextControl": {
         "x": 6.622297658930173,
-        "y": 1.0499999999999998
+        "y": 0.9499999999999993
       },
       "holonomicAngle": 180.0,
       "isReversal": false,
@@ -50,15 +50,15 @@
     {
       "anchorPoint": {
         "x": 6.55,
-        "y": 1.05
+        "y": 0.95
       },
       "prevControl": {
         "x": 6.55156507019363,
-        "y": 1.0544294162576247
+        "y": 0.9544294162576246
       },
       "nextControl": {
         "x": 6.54843492980637,
-        "y": 1.045570583742375
+        "y": 0.9455705837423749
       },
       "holonomicAngle": 180.0,
       "isReversal": false,

src/main/deploy/pathplanner/cableCoCuDock.path

@@ -197,7 +197,7 @@
   ],
   "markers": [
     {
-      "position": 0.9036363636363557,
+      "position": 0.8,
       "names": [
         "cubeDeployIntake"
       ]

src/main/deploy/pathplanner/openCoCu.path

@@ -97,14 +97,39 @@
         "waitTime": 0
       }
     },
+    {
+      "anchorPoint": {
+        "x": 2.92,
+        "y": 4.74
+      },
+      "prevControl": {
+        "x": 3.2814389139580955,
+        "y": 4.726353357710476
+      },
+      "nextControl": {
+        "x": 2.7662174982469625,
+        "y": 4.745806277937342
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
     {
       "anchorPoint": {
         "x": 2.07,
-        "y": 4.427228854457709
+        "y": 4.44
       },
       "prevControl": {
-        "x": 2.07263061942092,
-        "y": 4.284952917197655
+        "x": 2.3322891090226623,
+        "y": 4.487733078030258
       },
       "nextControl": null,
       "holonomicAngle": 0,

src/main/deploy/pathplanner/openCoCuDock.path

@@ -197,7 +197,7 @@
   ],
   "markers": [
     {
-      "position": 0.8527272727272658,
+      "position": 0.8,
       "names": [
         "cubeDeployIntake"
       ]

src/main/java/frc/robot/Constants.java

@@ -118,10 +118,10 @@ public static final class constControllers {
       MODULE_2.position,
       MODULE_3.position);
 
-  public static final double PRAC_FRONT_LEFT_ABSOLUTE_ENCODER_OFFSET = 111.708984;
-  public static final double PRAC_FRONT_RIGHT_ABSOLUTE_ENCODER_OFFSET = 340.312500;
+  public static final double PRAC_FRONT_LEFT_ABSOLUTE_ENCODER_OFFSET = 114.521484;
+  public static final double PRAC_FRONT_RIGHT_ABSOLUTE_ENCODER_OFFSET = 341.806641;
   public static final double PRAC_BACK_LEFT_ABSOLUTE_ENCODER_OFFSET = 289.511719;
-  public static final double PRAC_BACK_RIGHT_ABSOLUTE_ENCODER_OFFSET = 245.039062;
+  public static final double PRAC_BACK_RIGHT_ABSOLUTE_ENCODER_OFFSET = 245.566406;
 
   // module positions follow the WPILib robot coordinate system
   // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system
@@ -199,6 +199,7 @@ public static final class constWrist {
     public static final boolean ABSOLUTE_ENCODER_INVERT = true;
 
     public static final double GEAR_RATIO = 40.09;
+
   }
 
   public static final class constElevator {
@@ -213,6 +214,7 @@ public static final class constElevator {
 
     public static final double CIRCUMFERENCE = 0.13972;
     public static final double GEAR_RATIO = 2.5;
+
   }
 
   public static final class constVision {

src/main/java/frc/robot/RobotContainer.java

@@ -10,13 +10,20 @@
 import com.pathplanner.lib.auto.PIDConstants;
 import com.pathplanner.lib.auto.SwerveAutoBuilder;
 
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.LEDs;
 import frc.robot.subsystems.Intake;
@@ -39,6 +46,7 @@
 import frc.robot.commands.PrepGamePiece;
 import frc.robot.commands.SetLEDs;
 import frc.robot.commands.Stow;
+import frc.robot.commands.UpdateMechanismPoses;
 import frc.robot.commands.YeetGamePiece;
 import frc.robot.commands.Autos.Cable.*;
 import frc.robot.commands.Autos.Center.*;
@@ -70,6 +78,17 @@ public class RobotContainer {
   public static SwerveAutoBuilder swerveAutoBuilder;
 
   public RobotContainer() {
+    // Set out log file to be in its own folder
+    if (Robot.isSimulation()) {
+      DataLogManager.start("src/main/AdvantageScope Logs");
+    } else {
+      DataLogManager.start();
+    }
+    // Log data that is being put to shuffleboard
+    DataLogManager.logNetworkTables(true);
+    // Log the DS data and joysticks
+    DriverStation.startDataLog(DataLogManager.getLog(), true);
+
     // -- Creating Autos --
     HashMap<String, Command> autoEventMap = new HashMap<>();
     autoEventMap.put("cubeDeployIntake", new IntakeGamePiece(subWrist, subIntake, subElevator, subLEDs, GamePiece.CUBE,
@@ -93,7 +112,7 @@ public RobotContainer() {
     autoEventMap.put("stow", new Stow(subWrist, subIntake, subElevator));
     ;
     swerveAutoBuilder = new SwerveAutoBuilder(
-        subDrivetrain::getPose,
+        subDrivetrain::getPose2d,
         subDrivetrain::resetPose,
         subDrivetrain.swerveKinematics,
         new PIDConstants(
@@ -123,15 +142,15 @@ public RobotContainer() {
             conDriver.btn_B,
             conDriver.btn_A,
             conDriver.btn_X));
-    // subVision.setDefaultCommand(new AddVisionMeasurement(subDrivetrain,
-    // subVision));
+    subVision.setDefaultCommand(new UpdateMechanismPoses(subDrivetrain, subElevator, subWrist, subVision));
     subLEDs.setDefaultCommand(new SetLEDs(subLEDs, subDrivetrain, subIntake));
 
     configureBindings();
     configureAutoSelector();
 
     Timer.delay(2.5);
     resetToAbsolutePositions();
+
   }
 
   /**
@@ -146,6 +165,7 @@ public void resetToAbsolutePositions() {
   private void configureBindings() {
 
     // Driver
+    // src\main\documentation\driverControls23.png
 
     // "reset gyro" for field relative but actually resets the orientation at a
     // higher level
@@ -163,6 +183,7 @@ private void configureBindings() {
         .whileTrue(Commands.run(() -> subLEDs.setLEDPattern(constLEDs.DEFENSE_MODE_COLOR)));
 
     // Operator
+    // src\main\documentation\operatorControls23.png
 
     // Intake Cone (RB)
     conOperator.btn_RightBumper.onTrue(new IntakeGamePiece(subWrist, subIntake, subElevator, subLEDs, GamePiece.CONE,

src/main/java/frc/robot/RobotPreferences.java

@@ -183,7 +183,7 @@ public static final class prefElevator {
         0.08);
     public static final SN_DoublePreference elevatorIntakeCubePos = new SN_DoublePreference("elevatorIntakeCubePos",
         0.08);
-    public static final SN_DoublePreference elevatorShelf = new SN_DoublePreference("elevatorShelf", 1.22);
+    public static final SN_DoublePreference elevatorShelf = new SN_DoublePreference("elevatorShelf", 0.593373);
     public static final SN_DoublePreference elevatorSingle = new SN_DoublePreference("elevatorSingle", 0.096467);
     public static final SN_DoublePreference elevatorStow = new SN_DoublePreference("elevatorStow", 0.15);
 
@@ -205,6 +205,9 @@ public static final class prefElevator {
     public static final SN_DoublePreference elevatorEncoderCountsPerMeter = new SN_DoublePreference(
         "elevatorEncoderCountsPerMeter", 36644.718);
 
+    public static final SN_DoublePreference elevatorCarriageSimLength = new SN_DoublePreference(
+        "elevatorCarriageSimLength", 0.5);
+
   }
 
   public static final class prefWrist {
@@ -224,7 +227,7 @@ public static final class prefWrist {
         2);
 
     public static final SN_DoublePreference wristIntakeAngle = new SN_DoublePreference("wristIntakeAngle", 146);
-    public static final SN_DoublePreference wristShelfAngle = new SN_DoublePreference("wristShelfAngle", 142);
+    public static final SN_DoublePreference wristShelfAngle = new SN_DoublePreference("wristShelfAngle", 97.3);
     public static final SN_DoublePreference wristSingleAngle = new SN_DoublePreference("wristSingleAngle", 81.515321);
 
     public static final SN_DoublePreference wristStowAngle = new SN_DoublePreference("wristStowAngle", 44);

src/main/java/frc/robot/commands/Autos/Cable/CableCoCoYeetDock.java

@@ -40,6 +40,7 @@ public CableCoCoYeetDock(Drivetrain subDrivetrain, Intake subIntake, Wrist subWr
         Commands.runOnce(() -> subDrivetrain.resetRotation()),
         Commands.runOnce(() -> subDrivetrain.setNavXAngleAdjustment(
             subDrivetrain.cableCoCoYeetDock.getInitialHolonomicPose().getRotation().getDegrees())),
+        Commands.runOnce(() -> subDrivetrain.resetPose(subDrivetrain.cableCoCoYeetDock.getInitialHolonomicPose())),
 
         // Intake cone
         Commands.runOnce(() -> subIntake.setDesiredGamePiece(GamePiece.CONE)),

src/main/java/frc/robot/commands/Autos/Cable/CableCoCuHigh.java

@@ -40,6 +40,7 @@ public CableCoCuHigh(Drivetrain subDrivetrain, Intake subIntake, Wrist subWrist,
         Commands.runOnce(() -> subDrivetrain.resetRotation()),
         Commands.runOnce(() -> subDrivetrain.setNavXAngleAdjustment(
             subDrivetrain.cableCoCu.getInitialHolonomicPose().getRotation().getDegrees())),
+        Commands.runOnce(() -> subDrivetrain.resetPose(subDrivetrain.cableCoCu.getInitialHolonomicPose())),
 
         // Intake cone
         Commands.runOnce(() -> subIntake.setDesiredGamePiece(GamePiece.CONE)),

src/main/java/frc/robot/commands/Autos/Center/CenterCo.java

@@ -38,6 +38,7 @@ public CenterCo(Drivetrain subDrivetrain, Intake subIntake, Wrist subWrist, Elev
         Commands.runOnce(() -> subDrivetrain.resetRotation()),
         Commands.runOnce(() -> subDrivetrain.setNavXAngleAdjustment(
             subDrivetrain.centerCoDock.getInitialHolonomicPose().getRotation().getDegrees())),
+        Commands.runOnce(() -> subDrivetrain.resetPose(subDrivetrain.centerCoDock.getInitialHolonomicPose())),
 
         // Intake cone
         Commands.runOnce(() -> subIntake.setDesiredGamePiece(GamePiece.CONE)),

src/main/java/frc/robot/commands/Autos/Center/CenterCoDock.java

@@ -40,6 +40,7 @@ public CenterCoDock(Drivetrain subDrivetrain, Intake subIntake, Wrist subWrist,
         Commands.runOnce(() -> subDrivetrain.resetRotation()),
         Commands.runOnce(() -> subDrivetrain.setNavXAngleAdjustment(
             subDrivetrain.centerCoDock.getInitialHolonomicPose().getRotation().getDegrees())),
+        Commands.runOnce(() -> subDrivetrain.resetPose(subDrivetrain.centerCoDock.getInitialHolonomicPose())),
 
         // Intake cone
         Commands.runOnce(() -> subIntake.setDesiredGamePiece(GamePiece.CONE)),

src/main/java/frc/robot/commands/Autos/Open/OpenCoCoCoYeetDock.java

@@ -40,6 +40,7 @@ public OpenCoCoCoYeetDock(Drivetrain subDrivetrain, Intake subIntake, Wrist subW
         Commands.runOnce(() -> subDrivetrain.resetRotation()),
         Commands.runOnce(() -> subDrivetrain.setNavXAngleAdjustment(
             subDrivetrain.openCoCoCoYeetDock.getInitialHolonomicPose().getRotation().getDegrees())),
+        Commands.runOnce(() -> subDrivetrain.resetPose(subDrivetrain.openCoCoCoYeetDock.getInitialHolonomicPose())),
 
         // Intake cone
         Commands.runOnce(() -> subIntake.setDesiredGamePiece(GamePiece.CONE)),

src/main/java/frc/robot/commands/Autos/Open/OpenCoCuHigh.java

@@ -40,6 +40,7 @@ public OpenCoCuHigh(Drivetrain subDrivetrain, Intake subIntake, Wrist subWrist,
         Commands.runOnce(() -> subDrivetrain.resetRotation()),
         Commands.runOnce(() -> subDrivetrain.setNavXAngleAdjustment(
             subDrivetrain.openCoCu.getInitialHolonomicPose().getRotation().getDegrees())),
+        Commands.runOnce(() -> subDrivetrain.resetPose(subDrivetrain.openCoCu.getInitialHolonomicPose())),
 
         // Intake cone
         Commands.runOnce(() -> subIntake.setDesiredGamePiece(GamePiece.CONE)),