Merge pull request #111 from strykeforce/4or5ThenWing3ThenSearch

4or5 then wing3 then search

Author: mwitcpalek

src/main/deploy/paths/MiddleInitial1_WingNote1.toml

@@ -3,19 +3,19 @@ max_acceleration = 3.0 #m/s
 start_velocity = 0.0 #m/s
 end_velocity = 0.0 #m/s 
 is_reversed = false
-target_yaw = 26.6 #degrees - old 22.0
+target_yaw = 13.0 #degrees - old 26.6
 
 [start_pose]
     dataPoint = "MI1"
     dX = 0.0
-    dY = 0.0
-    angle = 0.0
+    dY = 0.25
+    angle = 30.0
 
 [end_pose]
     dataPoint "W1"
     dX = -0.1
-    dY = -0.35
-    angle = 26.6
+    dY = 0.05
+    angle = 13.0
 
 [[internal_points]]
     x = 1.9

src/main/deploy/paths/MiddleNote4_NonAmpShoot4.toml

@@ -0,0 +1,24 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -34.439 #degrees
+
+[start_pose]
+    dataPoint = "M4"
+    dX = -0.3
+    dY = 0.55
+    angle = 180.0
+
+[end_pose]
+    dataPoint = "NAS4"
+    angle = 143.27
+
+[[internal_points]]
+    x = 5.56
+    y = 1.8
+
+[[internal_points]]
+    x = 2.483
+    y = 3.154

src/main/deploy/paths/MiddleNote5_NonAmpShoot4.toml

@@ -0,0 +1,22 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -34.439 #degrees
+
+[start_pose]
+    dataPoint = "M5"
+    dX = -0.5
+    dY = 0.35
+
+[end_pose]
+    dataPoint = "NAS4"
+
+[[internal_points]]
+    x = 5.021
+    y = 1.395
+
+[[internal_points]]
+    x = 2.483
+    y = 3.154

src/main/deploy/paths/MiddleShoot3_WingNote3.toml

@@ -11,7 +11,7 @@ target_yaw = -30.0 #degrees
 [end_pose]
     dataPoint = "W3"
     dX = 0.1
-    dY = 0.15
+    dY = 0.2
     angle = -30.0
 
 [[internal_points]]

src/main/deploy/paths/NonAmpInitial1_MiddleNote4.toml

@@ -10,8 +10,8 @@ target_yaw = 0.0 #degrees
 
 [end_pose]
     dataPoint = "M4"
-    dX = -0.5
-    dY = 0.75
+    dX = -0.3
+    dY = 0.55
     angle = 0.0
 
 

src/main/deploy/paths/NonAmpShoot4_MiddleNote4.toml

@@ -0,0 +1,22 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "NAS4"
+    angle = -36.72
+
+[end_pose]
+    dataPoint = "M4"
+    angle = 0.0
+
+[[internal_points]]
+    x = 2.71
+    y = 2.8
+
+[[internal_points]]
+    x = 5.56
+    y = 2.0

src/main/deploy/paths/NonAmpShoot4_MiddleNote5.toml

@@ -0,0 +1,16 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "NAS4"
+
+[end_pose]
+    dataPoint = "M5"
+
+[[internal_points]]
+    x = 5.021
+    y = 1.395

src/main/deploy/paths/NonampShoot4_WingNote3.toml

@@ -0,0 +1,19 @@
+max_velocity = 3.0 #m/s
+max_acceleration = 2.5 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "NAS4"
+    angle = 0.0
+[end_pose]
+    dataPoint = "W3"
+    dX = -0.1
+    dY = 0.0
+    angle = 0.0
+
+[[internal_points]]
+    x = 2.2
+    y = 4.11

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import ch.qos.logback.classic.util.ContextInitializer;
+import com.ctre.phoenix6.SignalLogger;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -147,6 +148,7 @@ public void autonomousExit() {
 
   @Override
   public void teleopInit() {
+    SignalLogger.start();
     m_robotContainer.enableDeadeye();
     logger.info("Teleop Init | Match #{}", DriverStation.getMatchNumber());
     m_robotContainer.setIsAuto(false);

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

@@ -113,6 +113,7 @@
 import frc.robot.subsystems.magazine.MagazineSubsystem;
 import frc.robot.subsystems.pathHandler.PathHandler;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.FeedMode;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
 import frc.robot.subsystems.shooter.ShooterIOFX;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
@@ -327,7 +328,7 @@ public RobotContainer() {
     configureTuningDashboard();
     robotStateSubsystem.setAllianceColor(Alliance.Blue);
 
-    RobotController.setBrownoutVoltage(6.3);
+    RobotController.setBrownoutVoltage(5.75); // 6.3, want test 5.75
 
     // configureTelemetry();
     // configurePitDashboard();
@@ -641,6 +642,13 @@ private void configureMatchDashboard() {
         .withPosition(5, 2)
         .withSize(1, 1);
 
+    allianceColor =
+        Shuffleboard.getTab("Match")
+            .addBoolean(
+                "Middle Pass Mode?", () -> robotStateSubsystem.getFeedMode() == FeedMode.MIDDLE)
+            .withSize(1, 1)
+            .withPosition(5, 3);
+
     allianceColor =
         Shuffleboard.getTab("Match")
             .addBoolean("AllianceColor", () -> alliance != Alliance.Blue)
@@ -848,7 +856,9 @@ public void setAllianceColor(Alliance alliance) {
     allianceColor.withProperties(Map.of("colorWhenTrue", "red", "colorWhenFalse", "blue"));
     robotStateSubsystem.setAllianceColor(alliance);
 
+    driveSubsystem.setRecordTrajectoryAllowed(true);
     autoSwitch.getAutoCommand().generateTrajectory();
+    driveSubsystem.setRecordTrajectoryAllowed(false);
 
     // Flips gyro angle if alliance is red team
     if (robotStateSubsystem.getAllianceColor() == Alliance.Red) {
@@ -907,7 +917,9 @@ private void configureOperatorBindings() {
 
     // UnJam Note
     new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
-        .onTrue(new UnJamUpperNoteCommand(magazineSubsystem));
+        .onTrue(
+            new UnJamUpperNoteCommand(magazineSubsystem, intakeSubsystem, robotStateSubsystem)
+                .withTimeout(0.5));
 
     // Open Loop Elbow
     // new Trigger((() -> xboxController.getRightY() > RobotConstants.kJoystickDeadband))