4PC works all sidees + 1PC + CPR April Tag Fix

src/frc846/cpp/frc846/robot/calculators/AprilTagCalculator.cc

@@ -2,9 +2,9 @@
 
 #include <units/math.h>
 
+#include "frc846/math/collection.h"
 #include "frc846/robot/GenericRobot.h"
 #include "frc846/wpilib/time.h"
-#include "frc846/math/collection.h"
 
 namespace frc846::robot::calculators {
 
@@ -53,7 +53,7 @@ ATCalculatorOutput AprilTagCalculator::calculate(ATCalculatorInput input) {
           constants_.tag_locations[tags[1]].x_pos,
           constants_.tag_locations[tags[1]].y_pos};
 
-      auto rotated_tag1= loc_tag_1.rotate(bearingAtCapture, false);
+      auto rotated_tag1 = loc_tag_1.rotate(bearingAtCapture, false);
       auto rotated_tag2 = loc_tag_2.rotate(bearingAtCapture, false);
 
       double tag1Slope = 1 / units::math::tan(tx[0]);
@@ -70,27 +70,32 @@ ATCalculatorOutput AprilTagCalculator::calculate(ATCalculatorInput input) {
           constants_.camera_x_offsets[i], constants_.camera_y_offsets[i]};
 
       frc846::math::Vector2D uncompensatedPos = camera_pos - cam_offset;
-      uncompensatedPos=uncompensatedPos.rotate(bearingAtCapture);
+      uncompensatedPos = uncompensatedPos.rotate(bearingAtCapture);
 
       frc846::math::Vector2D velComp = {
           (input.pose.velocity[0] + input.old_pose.velocity[0]) / 2 *
               (tl + input.fudge_latency[i]),
           (input.pose.velocity[1] + input.old_pose.velocity[1]) / 2 *
               (tl + input.fudge_latency[i])};
-    //   if (frc846::math::modulo(fieldTag1Tx, 90_deg)>15_deg &&frc846::math::modulo(fieldTag2Tx, 90_deg)>15_deg){
-    //   
-      frc846::math::Vector2D est_pos =uncompensatedPos+velComp;
-
-    //   output.pos += (est_pos) * 1.2;
-    //   variance +=
-    //       1 / std::max(
-    //               (input.triangularVarianceCoeff *
-    //                   std::sqrt((distances.at(0)+distances.at(1)).to<double>()/2) *
-    //                   std::pow(
-    //                       1 + input.pose.velocity.magnitude().to<double>()/12, 2) *
-    //                   std::pow(1 + input.angular_velocity.to<double>()/300, 2)),
-    //               0.0000000001);
-    //   totalTagWeight += 180/(distances.at(0).to<double>()+distances.at(1).to<double>());
+      //   if (frc846::math::modulo(fieldTag1Tx, 90_deg)>15_deg
+      //   &&frc846::math::modulo(fieldTag2Tx, 90_deg)>15_deg){
+      //
+      frc846::math::Vector2D est_pos = uncompensatedPos + velComp;
+
+      //   output.pos += (est_pos) * 1.2;
+      //   variance +=
+      //       1 / std::max(
+      //               (input.triangularVarianceCoeff *
+      //                   std::sqrt((distances.at(0)+distances.at(1)).to<double>()/2)
+      //                   * std::pow(
+      //                       1 +
+      //                       input.pose.velocity.magnitude().to<double>()/12,
+      //                       2) *
+      //                   std::pow(1 + input.angular_velocity.to<double>()/300,
+      //                   2)),
+      //               0.0000000001);
+      //   totalTagWeight +=
+      //   180/(distances.at(0).to<double>()+distances.at(1).to<double>());
 
     }
     // CASE 2: Single tag estimate
@@ -102,20 +107,20 @@ ATCalculatorOutput AprilTagCalculator::calculate(ATCalculatorInput input) {
             (input.pose.velocity[1] + input.old_pose.velocity[1]) / 2 *
                 (tl + input.fudge_latency[i])};
         // if (distances.at(0) < 120_in) {
-          output.pos += (getPos(bearingAtCapture, tx.at(0), distances.at(0),
-                             tags.at(0), i) +
-                            velComp) *
-                        (48) / distances.at(0).to<double>();
-          variance += // -0.247, 0.0699
-              1 /
-              std::max(
-                  (input.aprilVarianceCoeff *
-                      distances.at(0).to<double>() *
-                      std::pow(
-                          1 + input.pose.velocity.magnitude().to<double>()/12, 2) *
-                      std::pow(1 + input.angular_velocity.to<double>()/300, 2)),
-                  0.0000000001);
-          totalTagWeight += (48) / distances.at(0).to<double>();
+        output.pos += (getPos(bearingAtCapture, tx.at(0), distances.at(0),
+                           tags.at(0), i) +
+                          velComp) *
+                      (48) / distances.at(0).to<double>();
+        variance +=  // -0.247, 0.0699
+            1 /
+            std::max(
+                (input.aprilVarianceCoeff * distances.at(0).to<double>() *
+                    std::pow(
+                        1 + input.pose.velocity.magnitude().to<double>() / 12,
+                        2) *
+                    std::pow(1 + input.angular_velocity.to<double>() / 300, 2)),
+                0.0000000001);
+        totalTagWeight += (48) / distances.at(0).to<double>();
         // }
       }
     }

src/frc846/cpp/frc846/robot/swerve/drive_to_point_command.cc

@@ -118,9 +118,9 @@ void DriveToPointCommand::Execute() {
       (target_.point - dt_readings.estimated_pose.position).magnitude() *
       units::math::sin(
           (target_.point - dt_readings.estimated_pose.position).angle() -
-          direction_offset)*(drivetrain_->GetPreferenceValue_double("drive_correctional_gain")*max_speed_.to<double>());
-
-
+          direction_offset) *
+      (drivetrain_->GetPreferenceValue_double("drive_correctional_gain") *
+          max_speed_.to<double>());
 
   frc846::math::VectorND<units::feet_per_second_squared, 2> corr_accl;
 
@@ -194,8 +194,7 @@ bool DriveToPointCommand::IsFinished() {
              drivetrain_readings.pose.velocity.magnitude() <
                  drivetrain_
                      ->GetPreferenceValue_unit_type<units::feet_per_second_t>(
-                         "vel_stopped_thresh") &&
-             (target_.point - current_point).magnitude() < 1.5_in)
+                         "vel_stopped_thresh"))
 
          //                  ||
          //  num_stalled_loops_ >l

src/frc846/cpp/frc846/robot/swerve/drivetrain.cc

@@ -6,8 +6,8 @@
 #include "frc846/robot/swerve/control/swerve_ol_calculator.h"
 #include "frc846/robot/swerve/swerve_module.h"
 
-//TODO: add LED indicator
-//TOOD: add button for alignment against field wall
+// TODO: add LED indicator
+// TOOD: add button for alignment against field wall
 
 namespace frc846::robot::swerve {
 

src/frc846/cpp/frc846/robot/swerve/lock_to_point_command.cc

@@ -62,9 +62,12 @@ void LockToPointCommand::Execute() {
                         .estimated_pose.velocity.magnitude()
                         .to<double>(),
                     0.0);
-    speed_target = units::math::min(units::math::max(speed_target,
-        -drivetrain_->GetPreferenceValue_unit_type<units::feet_per_second_t>(
-            "lock_max_speed")), drivetrain_->GetPreferenceValue_unit_type<units::feet_per_second_t>(
+    speed_target = units::math::min(
+        units::math::max(speed_target,
+            -drivetrain_
+                ->GetPreferenceValue_unit_type<units::feet_per_second_t>(
+                    "lock_max_speed")),
+        drivetrain_->GetPreferenceValue_unit_type<units::feet_per_second_t>(
             "lock_max_speed"));
 
     drivetrain_->SetTarget(frc846::robot::swerve::DrivetrainOLControlTarget{
@@ -78,7 +81,3 @@ void LockToPointCommand::End(bool interrupted) { drivetrain_->SetTargetZero(); }
 bool LockToPointCommand::IsFinished() { return false; }
 
 }  // namespace frc846::robot::swerve
-
-//deadband .5, 
-//p -2
-//d .01

src/y2025/cpp/FunkyRobot.cc

@@ -65,7 +65,7 @@ void FunkyRobot::InitTeleop() {
   // container_.climber_.SetDefaultCommand(ClimberCommand{container_});
   // container_.coralgae_.SetDefaultCommand(CoralgaeCommand{container_});
 
-  // ControlTriggerInitializer::InitTeleopTriggers(container_);
+  ControlTriggerInitializer::InitTeleopTriggers(container_);
 }
 
 void FunkyRobot::OnPeriodic() {

src/y2025/cpp/autos/auton_seqs.cc

@@ -25,7 +25,7 @@ using FPT = frc846::math::FieldPoint;
 
 #define MAX_ACCEL_1PC 7_fps_sq
 #define MAX_DECEL_1PC 7_fps_sq
-#define MAX_VEL_1PC 6_fps
+#define MAX_VEL_1PC 5_fps
 
 #define MAX_ACCEL_LEAVE 10_fps_sq
 #define MAX_DECEL_LEAVE 10_fps_sq
@@ -123,11 +123,12 @@ ThreePieceAuto::ThreePieceAuto(
               START(158.5_in - 73.25_in, START_Y,
                   180_deg),  // Fix to use April Tags instead.
               // WAIT{0.25_s},
+              // PARALLEL_DEADLINE
               DRIVE_TO_REEF(3PC, 11),
               // CORAL_POS(kCoral_ScoreL4, true),
               WAIT{.1_s},
               // CORAL_POS(kCoral_StowNoPiece, false),
-              DRIVE(3PC, 75_in, 120_in, 40_deg, 10_fps),
+              // DRIVE(3PC, 75_in, 120_in, 40_deg, 10_fps),
               DRIVE_TO_SOURCE(3PC),
               WAIT{.65_s},
               DRIVE_TO_REEF(3PC, 8),
@@ -156,14 +157,14 @@ OnePieceAndNetAuto::OnePieceAndNetAuto(
               START(158.5_in, START_Y, 180_deg),
               WAIT{0.25_s},
               DRIVE_TO_REEF(1PC, 1),
-              CORAL_POS(kCoral_ScoreL4, true),
+              // CORAL_POS(kCoral_ScoreL4, true),
               WAIT{1_s},
-              CORAL_POS(kCoral_StowNoPiece, false),
-              ALGAL_POS(kAlgae_L3Pick, false),
+              // CORAL_POS(kCoral_StowNoPiece, false),
+              // ALGAL_POS(kAlgae_L3Pick, false),
               WAIT{1_s},
               DRIVE(1PC, 100_in, START_Y - 30_in, 0_deg, 0_fps),
               DRIVE(1PC, 100_in, START_Y, 0_deg, 0_fps),
-              ALGAL_POS(kAlgae_Net, true),
+              // ALGAL_POS(kAlgae_Net, true),
               WAIT{1_s},
           }} {}
 

src/y2025/cpp/control_triggers.cc

@@ -7,10 +7,10 @@
 #include "commands/teleop/complete_gpd_command.h"
 #include "commands/teleop/lock_to_reef_command.h"
 #include "commands/teleop/processor_auto_align.h"
-#include "frc846/robot/swerve/lock_to_point_command.h"
 #include "commands/teleop/reef_auto_align.h"
 #include "frc846/robot/swerve/aim_command.h"
 #include "frc846/robot/swerve/drive_to_point_command.h"
+#include "frc846/robot/swerve/lock_to_point_command.h"
 #include "reef.h"
 
 void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
@@ -39,7 +39,8 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
           .ToPtr());
   frc2::Trigger{[&] {
     return container.control_input_.GetReadings().lock_processor;
-  }}.WhileTrue(frc846::robot::swerve::AimCommand{&(container.drivetrain_), 0_deg}
+  }}.WhileTrue(frc846::robot::swerve::AimCommand{
+      &(container.drivetrain_), 0_deg}
           .ToPtr());
 
   frc2::Trigger{[&] {

src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc

@@ -145,14 +145,15 @@ DrivetrainConstructor::getDrivetrainConfigs() {
   configs.camera_x_offsets = {-6.5_in, -6.5_in};
   configs.camera_y_offsets = {-8.25_in, -11.25_in};
   configs.cams = 2;
-  configs.april_locations = {{1, {28.429_in, 31.594_in}}, {2, {293.83_in, 35.41_in}}, 
-      {3, {317.15_in, 238.975_in}}, {6, {128.5_in, 163.18_in}},
-      {7, {155.25_in, 144_in}}, {8, {184.53_in, 158.1_in}},
-      {9, {189_in, 190.3_in}}, {10, {161.75_in, 209.5_in}},
-      {11, {131.7_in, 195.9_in}}, {12, {23.17_in, 655.45_in}}, {13, {289.3_in, 660_in}}, {16, {-0.15_in, 451.895_in}},
-      {17, {127.87_in, 528.19_in}}, {18, {161.75_in, 546.875_in}},
-      {19, {189.12_in, 532.68_in}}, {20, {184.53_in, 495.48_in}},
-      {21, {155.25_in, 481.375_in}},
+  configs.april_locations = {{1, {28.429_in, 31.594_in}},
+      {2, {293.83_in, 35.41_in}}, {3, {317.15_in, 238.975_in}},
+      {6, {128.5_in, 163.18_in}}, {7, {155.25_in, 144_in}},
+      {8, {184.53_in, 158.1_in}}, {9, {189_in, 190.3_in}},
+      {10, {161.75_in, 209.5_in}}, {11, {131.7_in, 195.9_in}},
+      {12, {23.17_in, 655.45_in}}, {13, {289.3_in, 660_in}},
+      {16, {-0.15_in, 451.895_in}}, {17, {127.87_in, 528.19_in}},
+      {18, {161.75_in, 546.875_in}}, {19, {189.12_in, 532.68_in}},
+      {20, {184.53_in, 495.48_in}}, {21, {155.25_in, 481.375_in}},
 
       {22, {132.47_in, 500.07_in}}};
 

src/y2025/cpp/subsystems/hardware/leds.cc

@@ -52,12 +52,7 @@ void LEDsSubsystem::WriteToHardware(LEDsTarget target) {
   if (target.state == kLEDsUnready) {
     SetStrip(RED);
   } else if (target.state == kLEDsDisabled) {
-    if (target.state == kisLinedUp) {
-      SetStrip(GREEN);
-      Flash(MED_FLASH);
-    } else {
-      SetStrip(ORANGE);
-    }
+    SetStrip(ORANGE);
   } else if (target.state == kLEDsAutonomous) {
     SetStrip(BLUE);
   } else if (target.state == kLEDsSequencing) {

src/y2025/cpp/subsystems/hardware/leds_logic.cc

@@ -1,20 +1,19 @@
 #include "subsystems/hardware/leds_logic.h"
 
+#include <iostream>
+
 #include "frc846/math/fieldpoints.h"
 
 LEDsState LEDsLogic::checkLoc(RobotContainer* container, LEDsTarget target) {
   std::string selected_auto = frc846::robot::GenericRobot::GetSelectedAuto();
 
-  if (selected_auto.empty()) {
-    return target.state = kLEDsDisabled;
-  }
-  units::inch_t auto_y = (311.5_in - 15_in);
+  // if (selected_auto.empty()) { return target.state = kLEDsDisabled; }
+  units::inch_t auto_y = (290_in);
 
   units::inch_t auto_3pc = (158.5_in - 73.25_in);
   units::inch_t auto_1pc = (158.5_in);
-  units::inch_t auto_leave = (158.5_in);
-
-  if (selected_auto.substr(0, 3) == "3PC") {
+  units::inch_t auto_leave = (158.5_in);                      
+  if (selected_auto.substr(0, 3) == "3PC") {                                                                                                       
     std::string blue_red = selected_auto.substr(4, 5);
     std::string left_right = selected_auto.substr(6);
 
@@ -25,37 +24,37 @@ LEDsState LEDsLogic::checkLoc(RobotContainer* container, LEDsTarget target) {
 
     if ((auto_start.point -
             container->drivetrain_.GetReadings().estimated_pose.position)
-            .magnitude() < 3_in) {
+            .magnitude() < 1_ft) {
       return kisLinedUp;
     }
   }
   if (selected_auto.substr(0, 4) == "1PCN") {
     std::string blue_red = selected_auto.substr(5, 6);
     std::string left_right = selected_auto.substr(7);
 
-    frc846::math::FieldPoint auto_start = {{auto_3pc, auto_y}, 0_deg, 0_fps};
+    frc846::math::FieldPoint auto_start = {{auto_1pc, auto_y}, 0_deg, 0_fps};
 
-    if (blue_red == "B") { auto_start = auto_start.mirror(true); }
-    if (left_right == "R") { auto_start = auto_start.mirrorOnlyX(true); }
+    // if (blue_red == "B") { auto_start = auto_start.mirror(true); }
+    // if (left_right == "R") { auto_start = auto_start.mirrorOnlyX(true); }
 
     if ((auto_start.point -
             container->drivetrain_.GetReadings().estimated_pose.position)
-            .magnitude() < 3_in) {
+            .magnitude() < 1_ft) {
       return kisLinedUp;
     }
   }
   if (selected_auto.substr(0, 5) == "LEAVE") {
     std::string blue_red = selected_auto.substr(6, 7);
     std::string left_right = selected_auto.substr(8);
 
-    frc846::math::FieldPoint auto_start = {{auto_3pc, auto_y}, 0_deg, 0_fps};
+    frc846::math::FieldPoint auto_start = {{auto_leave, auto_y}, 0_deg, 0_fps};
 
     if (blue_red == "B") { auto_start = auto_start.mirror(true); }
     if (left_right == "R") { auto_start = auto_start.mirrorOnlyX(true); }
 
     if ((auto_start.point -
             container->drivetrain_.GetReadings().estimated_pose.position)
-            .magnitude() < 3_in) {
+            .magnitude() < 1_ft) {
       return kisLinedUp;
     }
   } else {