Merge branch 'stop_build_day' into at_autos

src/frc846/cpp/frc846/control/HigherMotorController.cc

@@ -15,6 +15,10 @@ void HigherMotorController::Setup() {
       mmtype_, constr_params_);
 }
 
+void HigherMotorController::SetNeutralMode(bool brake) {
+  frc846::control::MotorMonkey::SetNeutralMode(slot_id_, brake);
+}
+
 bool HigherMotorController::VerifyConnected() {
   return frc846::control::MotorMonkey::VerifyConnected();
 }

src/frc846/cpp/frc846/control/MotorMonkey.cc

@@ -181,6 +181,12 @@ void MotorMonkey::Tick(bool disabled) {
   }
 }
 
+void MotorMonkey::SetNeutralMode(size_t slot_id, bool brake_mode) {
+  CHECK_SLOT_ID();
+  controller_registry[slot_id]->SetNeutralMode(brake_mode);
+  LOG_IF_ERROR("SetNeutralMode");
+}
+
 units::ampere_t MotorMonkey::WriteMessages(units::ampere_t max_draw) {
   units::ampere_t total_current = 0.0_A;
   std::queue<MotorMessage> temp_messages{control_messages};

src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc

@@ -98,7 +98,7 @@ void SparkMXFX_interm::SetVoltageCompensation(
 void SparkMXFX_interm::SetGains(frc846::control::base::MotorGains gains) {
   gains_ = gains;
   configs.closedLoop.Pidf(
-      gains_.kP, gains_.kI, std::abs(gains_.kD), gains_.kFF);
+      gains_.kP, gains_.kI, std::abs(gains_.kD), std::abs(gains_.kFF));
 
   APPLY_CONFIG_NO_RESET();
 }

src/frc846/include/frc846/control/HigherMotorController.h

@@ -30,6 +30,8 @@ class HigherMotorController {
 
   void SetGains(frc846::control::base::MotorGains gains);
 
+  void SetNeutralMode(bool brake);
+
   /*
   SetLoad()
 

src/frc846/include/frc846/control/MotorMonkey.h

@@ -42,6 +42,8 @@ class MotorMonkey {
   */
   static void Tick(bool disabled);
 
+  static void SetNeutralMode(size_t slot_id, bool brake_mode);
+
   /*
   WriteMessages()
 

src/y2025/cpp/FunkyRobot.cc

@@ -18,15 +18,19 @@
 #include "commands/teleop/algal_command.h"
 #include "commands/teleop/climber_command.h"
 #include "commands/teleop/coral_command.h"
-#include "commands/teleop/coralgae_command.h"
 #include "commands/teleop/drive_command.h"
 #include "control_triggers.h"
 #include "field.h"
 #include "frc846/wpilib/NTAction.h"
 #include "rsighandler.h"
 #include "subsystems/hardware/leds_logic.h"
 
-FunkyRobot::FunkyRobot() : GenericRobot{&container_} {}
+FunkyRobot::FunkyRobot() : GenericRobot{&container_} {
+  RegisterPreference("num_coasting_loops", 1000);
+  RegisterPreference("homing_flash_loops", 50);
+  RegisterPreference("elevator_home_height", 14_in);
+  RegisterPreference("telescope_home_height", 33_in);
+}
 
 void FunkyRobot::OnInitialize() {
   Field::Setup();
@@ -50,6 +54,26 @@ void FunkyRobot::OnInitialize() {
       "zero_bearing", new frc846::wpilib::NTAction(
                           [this] { container_.drivetrain_.ZeroBearing(); }));
 
+  frc::SmartDashboard::PutData(
+      "brake_for_time", new frc846::wpilib::NTAction([this] {
+        container_.algal_ss_.elevator.CoastSubsystem();
+        container_.coral_ss_.telescope.CoastSubsystem();
+        container_.climber_.CoastSubsystem();
+        container_.algal_ss_.algal_wrist.CoastSubsystem();
+        container_.coral_ss_.coral_wrist.CoastSubsystem();
+        coast_count_ = GetPreferenceValue_int("num_coasting_loops");
+      }));
+
+  frc::SmartDashboard::PutData(
+      "home_telescope_elevator", new frc846::wpilib::NTAction([this] {
+        container_.coral_ss_.telescope.HomeSubsystem(
+            GetPreferenceValue_unit_type<units::inch_t>(
+                "telescope_home_height"));
+        container_.algal_ss_.elevator.HomeSubsystem(
+            GetPreferenceValue_unit_type<units::inch_t>(
+                "telescope_home_height"));
+      }));
+
   frc::SmartDashboard::PutData("zero_odometry",
       new frc846::wpilib::NTAction(
           [this] { container_.drivetrain_.SetPosition({0_in, 0_in}); }));
@@ -60,23 +84,59 @@ void FunkyRobot::OnDisable() {}
 void FunkyRobot::InitTeleop() {
   container_.drivetrain_.SetDefaultCommand(DriveCommand{container_});
 
-  // container_.coral_ss_.SetDefaultCommand(CoralCommand{container_});
-  // container_.algal_ss_.SetDefaultCommand(AlgalCommand{container_});
-  // container_.climber_.SetDefaultCommand(ClimberCommand{container_});
-  // container_.coralgae_.SetDefaultCommand(CoralgaeCommand{container_});
+  container_.coral_ss_.SetDefaultCommand(CoralCommand{container_});
+  container_.algal_ss_.SetDefaultCommand(AlgalCommand{container_});
+  container_.climber_.SetDefaultCommand(ClimberCommand{container_});
 
   ControlTriggerInitializer::InitTeleopTriggers(container_);
 }
 
 void FunkyRobot::OnPeriodic() {
+  if (!gyro_switch_.Get()) {
+    container_.drivetrain_.SetBearing(0_deg);
+    homing_count_gyro = GetPreferenceValue_int("homing_flash_loops");
+  }
+
   if (!home_switch_.Get()) {
     container_.algal_ss_.elevator.HomeSubsystem(
-        robot_constants::elevator::elevator_hall_effect);
+        GetPreferenceValue_unit_type<units::inch_t>("elevator_home_height"));
     container_.coral_ss_.telescope.HomeSubsystem(
-        robot_constants::telescope::telescope_hall_effect);
+        GetPreferenceValue_unit_type<units::inch_t>("telescope_home_height"));
+
+    homing_count_ = GetPreferenceValue_int("homing_flash_loops");
+  }
+
+  if (coast_count_ > 0) coast_count_--;
+  if (coast_count_ == 1 || coast_count_ == 7) {
+    container_.algal_ss_.elevator.BrakeSubsystem();
+    container_.coral_ss_.telescope.BrakeSubsystem();
+    container_.climber_.BrakeSubsystem();
+    container_.algal_ss_.algal_wrist.BrakeSubsystem();
+    container_.coral_ss_.coral_wrist.BrakeSubsystem();
   }
+  if (!coast_switch_.Get() && !IsTeleopEnabled()) {
+    container_.algal_ss_.elevator.CoastSubsystem();
+    container_.coral_ss_.telescope.CoastSubsystem();
+    container_.climber_.CoastSubsystem();
+    container_.algal_ss_.algal_wrist.CoastSubsystem();
+    container_.coral_ss_.coral_wrist.CoastSubsystem();
+    coast_count_ = GetPreferenceValue_int("num_coasting_loops");
+  }
+
+  if (homing_count_ > 0) homing_count_--;
+  if (homing_count_gyro > 0) homing_count_gyro--;
+
+  bool isDisabled = frc::DriverStation::IsDisabled();
 
-  LEDsLogic::UpdateLEDs(&container_);
+  if (homing_count_ > 0 && isDisabled)
+    LEDsLogic::SetLEDsState(&container_, kLEDsHoming);
+  else if (homing_count_gyro > 0 && isDisabled)
+    LEDsLogic::SetLEDsState(&container_, kLEDsHomingGyro);
+  else if (coast_count_ > 0 && isDisabled)
+    LEDsLogic::CoastingLEDs(&container_,
+        (1.0 * coast_count_) / GetPreferenceValue_int("num_coasting_loops"));
+  else
+    LEDsLogic::UpdateLEDs(&container_);
 
   AntiTippingCalculator::SetTelescopeHeight(
       container_.coral_ss_.telescope.GetReadings().position);
@@ -92,7 +152,6 @@ void FunkyRobot::OnPeriodic() {
 void FunkyRobot::InitTest() {
   container_.drivetrain_.SetDefaultCommand(DriveCommand{container_});
   container_.climber_.SetDefaultCommand(DinosaurClimberCommand{container_});
-  container_.coralgae_.SetDefaultCommand(CoralgaeCommand{container_});
 
   frc2::Trigger start_dinosaur_a([] { return true; });
   start_dinosaur_a.WhileTrue(frc2::SequentialCommandGroup{

src/y2025/cpp/commands/teleop/algal_command.cc

@@ -19,6 +19,16 @@ void AlgalCommand::Periodic() {
 
   algal_target.score = ci_readings.score_algae;
 
+  if (ci_readings.inc_elevator)
+    container_.algal_ss_.adjustElevator(true);
+  else if (ci_readings.dec_elevator)
+    container_.algal_ss_.adjustElevator(false);
+
+  if (ci_readings.inc_a_wrist)
+    container_.algal_ss_.adjustWrist(true);
+  else if (ci_readings.dec_a_wrist)
+    container_.algal_ss_.adjustWrist(false);
+
   container_.algal_ss_.SetTarget(algal_target);
 }
 

src/y2025/cpp/commands/teleop/coral_command.cc

@@ -12,14 +12,24 @@ void CoralCommand::Periodic() {
   CoralSSTarget coral_target{};
   auto ci_readings = container_.control_input_.GetReadings();
 
-  if (ci_readings.position_algal)
+  if (ci_readings.coral_state != kCoral_StowNoPiece)
     coral_target.state = ci_readings.coral_state;
   else if (container_.coral_ss_.coral_end_effector.GetReadings().has_piece_)
     coral_target.state = kCoral_StowWithPiece;
   else
     coral_target.state = kCoral_StowNoPiece;
   coral_target.score = ci_readings.score_coral;
 
+  if (ci_readings.inc_elevator)
+    container_.coral_ss_.adjustTelescope(true);
+  else if (ci_readings.dec_elevator)
+    container_.coral_ss_.adjustTelescope(false);
+
+  if (ci_readings.inc_c_wrist)
+    container_.coral_ss_.adjustWrist(true);
+  else if (ci_readings.dec_c_wrist)
+    container_.coral_ss_.adjustWrist(false);
+
   container_.coral_ss_.SetTarget(coral_target);
 }
 

src/y2025/cpp/commands/teleop/drive_command.cc

@@ -3,6 +3,7 @@
 #include <utility>
 
 #include "calculators/AntiTippingCalculator.h"
+#include "frc846/math/fieldpoints.h"
 
 DriveCommand::DriveCommand(RobotContainer &container)
     : frc846::robot::GenericCommand<RobotContainer, DriveCommand>{
@@ -98,8 +99,35 @@ void DriveCommand::Periodic() {
 
   target.angular_velocity = rotation * max_omega;
 
-  if (frc::DriverStation::GetAlliance() == frc::DriverStation::Alliance::kBlue)
-    target.velocity = target.velocity.rotate(180_deg);
+  bool isBlue = (frc::DriverStation::GetAlliance() ==
+                 frc::DriverStation::Alliance::kBlue);
+
+  if (isBlue) target.velocity = target.velocity.rotate(180_deg);
+
+  if (ci_readings_.auto_align) {
+    units::degree_t target_angle = 1000_deg;
+    if (ci_readings_.algal_state == kAlgae_Net &&
+        container_.algal_ss_.algal_end_effector.GetReadings().has_piece_) {
+      target_angle = isBlue ? 180_deg : 0_deg;
+    } else if (ci_readings_.algal_state == kAlgae_Processor &&
+               container_.algal_ss_.algal_end_effector.GetReadings()
+                   .has_piece_) {
+      target_angle = isBlue ? -90_deg : 90_deg;
+    } else if (ci_readings_.coral_state == kCoral_StowNoPiece &&
+               !container_.coral_ss_.coral_end_effector.GetReadings()
+                    .has_piece_) {
+      if (container_.drivetrain_.GetReadings().pose.position[0] >
+          (frc846::math::FieldPoint::field_size_x / 2))
+        target_angle = 180_deg - 54_deg;
+      else
+        target_angle = 180_deg + 54_deg;
+
+      if (isBlue) target_angle = 180_deg - target_angle;
+    }
+    if (target_angle <= 720_deg)
+      target.angular_velocity =
+          container_.drivetrain_.ApplyBearingPID(target_angle);
+  }
 
   if (ci_readings_.auto_align) {
     units::degree_t target_bearing = 0_deg;

src/y2025/cpp/control_triggers.cc

@@ -5,6 +5,8 @@
 #include <frc2/command/button/Trigger.h>
 
 #include "commands/teleop/complete_gpd_command.h"
+#include "commands/teleop/gpd_ss_command.h"
+#include "commands/teleop/lock_gpd_command.h"
 #include "commands/teleop/lock_to_reef_command.h"
 #include "commands/teleop/processor_auto_align.h"
 #include "commands/teleop/reef_auto_align.h"
@@ -43,11 +45,20 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
       &(container.drivetrain_), 0_deg}
           .ToPtr());
 
+  frc2::Trigger{[&] {
+    return container.control_input_.GetReadings().targeting_algae &&
+
+           !container.algal_ss_.algal_end_effector.GetReadings().has_piece_;
+  }}.OnTrue(GPDSSCommand{container}.Until([&] {
+    return !container.control_input_.GetReadings().targeting_algae ||
+           container.algal_ss_.algal_end_effector.GetReadings().has_piece_;
+  }));
+
   frc2::Trigger{[&] {
     return container.control_input_.GetReadings().targeting_algae &&
            container.GPD_.GetReadings().gamepieces.size() != 0U &&
            !container.algal_ss_.algal_end_effector.GetReadings().has_piece_;
-  }}.OnTrue(CompleteGPDCommand{container}.Until([&] {
+  }}.OnTrue(LockGPDCommand{container}.Until([&] {
     return !container.control_input_.GetReadings().targeting_algae ||
            container.algal_ss_.algal_end_effector.GetReadings().has_piece_;
   }));