FRC2240 · philipbiolog · Apr 16, 2025 · Apr 16, 2025 · Apr 16, 2025 · Apr 17, 2025
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -31,6 +31,7 @@ void RobotContainer::add_named_commands()
   pathplanner::NamedCommands::registerCommand("algae_l2", frc2::cmd::Print("start l2").AndThen(set_state(CONSTANTS::MANIPULATOR_STATES::ALGAE_L2)).AndThen(frc2::cmd::Print("end l2")));
   pathplanner::NamedCommands::registerCommand("algae_score", frc2::cmd::Print("start algae score").AndThen(score_algae()).AndThen(frc2::cmd::Print("end algae score")));
   pathplanner::NamedCommands::registerCommand("barge", frc2::cmd::Print("start barge").AndThen(set_state(CONSTANTS::MANIPULATOR_STATES::BARGE)).AndThen(frc2::cmd::Print("end barge")));
+  pathplanner::NamedCommands::registerCommand("algae_l3", frc2::cmd::Print("start l3").AndThen(set_state(CONSTANTS::MANIPULATOR_STATES::ALGAE_L3)).AndThen(frc2::cmd::Print("end l3")));
 }
 
 void RobotContainer::SetPID()
@@ -136,8 +137,12 @@ void RobotContainer::ConfigureBindings()
       .ToggleOnTrue(m_poweredfun.spin(0_A));
 
   // L1 state command on driver 2
+  // frc2::Trigger([this]() -> bool
+  //               { return this->m_stick1.LeftBumper().Get(); })
+  //     .OnTrue(set_state(CONSTANTS::MANIPULATOR_STATES::L1));
+
   frc2::Trigger([this]() -> bool
-                { return this->m_stick1.LeftBumper().Get(); })
+                { return this->m_stick0.B().Get(); })
       .OnTrue(set_state(CONSTANTS::MANIPULATOR_STATES::L1));
 
 }

diff --git a/src/main/cpp/subsystems/Elevator.cpp b/src/main/cpp/subsystems/Elevator.cpp
@@ -32,8 +32,6 @@ frc2::CommandPtr Elevator::set_position_command(units::angle::turn_t pos)
 {
     return frc2::RunCommand([this, pos]
                             {         
-        frc::SmartDashboard::PutNumber("elv/desired", pos.value());
-        frc::SmartDashboard::PutNumber("elv/delta", m_motor.GetPosition().GetValueAsDouble() - pos.value());
         set_position(pos); },
                             {this})
         .Until([this, pos]
@@ -58,5 +56,7 @@ units::angle::turn_t Elevator::get_position()
 
 void Elevator::set_position(units::angle::turn_t pos)
 {
+    frc::SmartDashboard::PutNumber("elv/desired", pos.value());
+    frc::SmartDashboard::PutNumber("elv/delta", m_motor.GetPosition().GetValueAsDouble() - pos.value());
     m_motor.SetControl(control_req.WithPosition(pos));
 };
diff --git a/src/main/cpp/swerve/Trajectory.cpp b/src/main/cpp/swerve/Trajectory.cpp
@@ -95,19 +95,19 @@ frc2::CommandPtr Trajectory::manual_drive(bool field_relative)
         units::meters_per_second_t front_back;
         units::angular_velocity::radians_per_second_t rot;
 
-        if (m_stick->B().Get())
-        {
-          left_right = (frc::ApplyDeadband(m_stick->GetLeftX(), 0.1) *
-                        CONSTANTS::DRIVE::SLOW_MODE) *
-                       (CONSTANTS::DRIVE::TELEOP_MAX_SPEED);
-          front_back = (frc::ApplyDeadband(m_stick->GetLeftY(), 0.1) *
-                        CONSTANTS::DRIVE::SLOW_MODE) *
-                       (CONSTANTS::DRIVE::TELEOP_MAX_SPEED);
-          rot = (frc::ApplyDeadband(m_stick->GetRightX(), .1) *
-                 CONSTANTS::DRIVE::SLOW_MODE) *
-                (m_drivetrain->TELEOP_MAX_ANGULAR_SPEED);
-        }
-        else
+        // if (m_stick->B().Get())
+        // {
+        //   left_right = (frc::ApplyDeadband(m_stick->GetLeftX(), 0.1) *
+        //                 CONSTANTS::DRIVE::SLOW_MODE) *
+        //                (CONSTANTS::DRIVE::TELEOP_MAX_SPEED);
+        //   front_back = (frc::ApplyDeadband(m_stick->GetLeftY(), 0.1) *
+        //                 CONSTANTS::DRIVE::SLOW_MODE) *
+        //                (CONSTANTS::DRIVE::TELEOP_MAX_SPEED);
+        //   rot = (frc::ApplyDeadband(m_stick->GetRightX(), .1) *
+        //          CONSTANTS::DRIVE::SLOW_MODE) *
+        //         (m_drivetrain->TELEOP_MAX_ANGULAR_SPEED);
+        // }
+        // else
         {
           left_right = frc::ApplyDeadband(m_stick->GetLeftX(), 0.1) *
                        (CONSTANTS::DRIVE::TELEOP_MAX_SPEED);

diff --git a/src/main/deploy/pathplanner/autos/Barge.auto b/src/main/deploy/pathplanner/autos/Barge.auto
@@ -23,9 +23,22 @@
           }
         },
         {
-          "type": "named",
+          "type": "race",
           "data": {
-            "name": "score_l4"
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 0.25
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "score_l4"
+                }
+              }
+            ]
           }
         },
         {
@@ -61,7 +74,19 @@
         {
           "type": "path",
           "data": {
-            "pathName": "b_drive_back"
+            "pathName": "barge_to_algae"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Barge_2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "end"
           }
         }
       ]

diff --git a/src/main/deploy/pathplanner/autos/left 4gp.auto b/src/main/deploy/pathplanner/autos/left 4gp.auto
@@ -54,6 +54,12 @@
             ]
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.65
+          }
+        },
         {
           "type": "parallel",
           "data": {
@@ -92,6 +98,12 @@
             ]
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.65
+          }
+        },
         {
           "type": "parallel",
           "data": {

diff --git a/src/main/deploy/pathplanner/paths/(L)coral_(F)reef.path b/src/main/deploy/pathplanner/paths/(L)coral_(F)reef.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.551,
-        "y": 2.731
+        "x": 3.54,
+        "y": 2.78
       },
       "prevControl": {
-        "x": 3.099510870076272,
-        "y": 2.0084663558963567
+        "x": 3.0909093604241216,
+        "y": 2.055973136723295
       },
       "nextControl": null,
       "isLocked": false,
@@ -79,7 +79,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 58.00000000000001
+    "rotation": 58.18999999999999
   },
   "reversed": false,
   "folder": "left_4gp",

diff --git a/src/main/deploy/pathplanner/paths/(L)coral_(LF)reef1.path b/src/main/deploy/pathplanner/paths/(L)coral_(LF)reef1.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.8183921930380986,
-        "y": 2.5907213244798823
+        "x": 3.84,
+        "y": 2.59
       },
       "prevControl": {
-        "x": 3.688885440694816,
-        "y": 2.376880259439756
+        "x": 3.7099338575094762,
+        "y": 2.3764987152787254
       },
       "nextControl": null,
       "isLocked": false,
@@ -79,7 +79,7 @@
   },
   "goalEndState": {
     "velocity": 0.0,
-    "rotation": 58.8
+    "rotation": 58.650000000000006
   },
   "reversed": false,
   "folder": "left_4gp",

diff --git a/src/main/deploy/pathplanner/paths/(L)start_(LB)reef.path b/src/main/deploy/pathplanner/paths/(L)start_(LB)reef.path
@@ -20,8 +20,8 @@
         "y": 2.74
       },
       "prevControl": {
-        "x": 5.774378376447036,
-        "y": 1.9736232249806211
+        "x": 5.736483776411132,
+        "y": 1.9545695663528102
       },
       "nextControl": null,
       "isLocked": false,
@@ -61,7 +61,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 118.40000000000002
+    "rotation": 115.61000000000001
   },
   "reversed": false,
   "folder": "left_4gp",

diff --git a/src/main/deploy/pathplanner/paths/Barge_2.path b/src/main/deploy/pathplanner/paths/Barge_2.path
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.2,
+        "y": 5.3
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.2,
+        "y": 7.0320508075688775
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.649,
+        "y": 6.65
+      },
+      "prevControl": {
+        "x": 6.171098681458263,
+        "y": 6.65
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.2014134275618375,
+      "rotationDegrees": -119.99999999999999
+    },
+    {
+      "waypointRelativePos": 0.6,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.8,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "barge",
+      "waypointRelativePos": 0.25,
+      "endWaypointRelativePos": 0.92,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "barge"
+        }
+      }
+    },
+    {
+      "name": "score",
+      "waypointRelativePos": 0.92,
+      "endWaypointRelativePos": 1.0,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "barge"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "algae_score"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "barge",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/b_algae_intake.path b/src/main/deploy/pathplanner/paths/b_algae_intake.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.624,
+        "x": 6.4,
         "y": 4.14
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.374,
-        "y": 4.14
+        "x": 6.275,
+        "y": 3.9234936490538903
       },
       "isLocked": false,
       "linkedName": null
@@ -20,7 +20,7 @@
         "y": 3.9857349298392473
       },
       "prevControl": {
-        "x": 6.24888653438883,
+        "x": 6.49888653438883,
         "y": 3.9857349298392473
       },
       "nextControl": null,