Stop build day

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/y2025/cpp/FunkyRobot.cc

@@ -18,7 +18,6 @@
 #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"
@@ -28,6 +27,9 @@
 
 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() {
@@ -62,6 +64,16 @@ void FunkyRobot::OnInitialize() {
         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}); }));
@@ -75,19 +87,23 @@ void FunkyRobot::InitTeleop() {
   container_.coral_ss_.SetDefaultCommand(CoralCommand{container_});
   container_.algal_ss_.SetDefaultCommand(AlgalCommand{container_});
   container_.climber_.SetDefaultCommand(ClimberCommand{container_});
-  // container_.coralgae_.SetDefaultCommand(CoralgaeCommand{container_});
 
   ControlTriggerInitializer::InitTeleopTriggers(container_);
 }
 
 void FunkyRobot::OnPeriodic() {
-  if (!gyro_switch_.Get()) { container_.drivetrain_.SetBearing(0_deg); }
+  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_--;
@@ -107,7 +123,16 @@ void FunkyRobot::OnPeriodic() {
     coast_count_ = GetPreferenceValue_int("num_coasting_loops");
   }
 
-  if (coast_count_ > 0 && !IsTeleopEnabled())
+  if (homing_count_ > 0) homing_count_--;
+  if (homing_count_gyro > 0) homing_count_gyro--;
+
+  bool isDisabled = frc::DriverStation::IsDisabled();
+
+  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
@@ -127,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);
+  }
 
   container_.drivetrain_.SetTarget({target});
 }

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/reef_auto_align.h"
 #include "frc846/robot/swerve/aim_command.h"
@@ -30,11 +32,20 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
       container, false, 5_fps, 15_fps_sq, 15_fps_sq}
                    .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_;
   }));

src/y2025/cpp/subsystems/abstract/control_input.cc

@@ -12,6 +12,8 @@ void ControlInputSubsystem::Setup() {
 
   RegisterPreference("rotation_deadband", 0.07);
   RegisterPreference("rotation_exponent", 2);
+
+  RegisterPreference("op_deadband", 0.35);
 }
 
 ControlInputTarget ControlInputSubsystem::ZeroTarget() const {
@@ -76,30 +78,31 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() {
 
   ci_readings_.targeting_algae = dr_readings.left_trigger;
 
-  ci_readings_.auto_align = dr_readings.a_button;
-
-  if (dr_readings.b_button && !previous_driver_.b_button)
-    ci_readings_.position_algal = !ci_readings_.position_algal;
+  ci_readings_.auto_align = dr_readings.rsb;
 
-  if (dr_readings.y_button && !previous_driver_.y_button)
-    ci_readings_.position_coral = !ci_readings_.position_coral;
+  if (dr_readings.right_trigger && !previous_driver_.right_trigger)
+    ci_readings_.position_algal = !previous_readings_.position_algal;
+  else
+    ci_readings_.position_algal = previous_readings_.position_algal;
 
-  else if (op_readings.a_button)
-    ci_readings_.coral_state = CoralStates::kCoral_ScoreL2;
-  else if (op_readings.b_button)
-    ci_readings_.coral_state = CoralStates::kCoral_ScoreL3;
-  else if (op_readings.x_button)
+  if (dr_readings.y_button)
     ci_readings_.coral_state = CoralStates::kCoral_ScoreL4;
+  else if (dr_readings.x_button)
+    ci_readings_.coral_state = CoralStates::kCoral_ScoreL3;
+  else if (dr_readings.a_button)
+    ci_readings_.coral_state = CoralStates::kCoral_ScoreL2;
+  else if (dr_readings.b_button)
+    ci_readings_.coral_state = CoralStates::kCoral_StowNoPiece;
   else
     ci_readings_.coral_state = previous_readings_.coral_state;
 
-  if ((int)op_readings.pov == 0)
+  if (op_readings.pov == frc846::robot::XboxPOV::kUp)
     ci_readings_.algal_state = AlgalStates::kAlgae_Net;
-  else if ((int)op_readings.pov == 90)
+  else if (op_readings.pov == frc846::robot::XboxPOV::kRight)
     ci_readings_.algal_state = AlgalStates::kAlgae_L3Pick;
-  else if ((int)op_readings.pov == 180)
+  else if (op_readings.pov == frc846::robot::XboxPOV::kDown)
     ci_readings_.algal_state = AlgalStates::kAlgae_Processor;
-  else if ((int)op_readings.pov == 270)
+  else if (op_readings.pov == frc846::robot::XboxPOV::kLeft)
     ci_readings_.algal_state = AlgalStates::kAlgae_L2Pick;
   else if (op_readings.right_trigger)
     ci_readings_.algal_state = AlgalStates::kAlgae_GroundIntake;
@@ -108,8 +111,28 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() {
   else
     ci_readings_.algal_state = previous_readings_.algal_state;
 
-  if (op_readings.left_bumper) ci_readings_.score_coral = true;
-  if (dr_readings.right_trigger) ci_readings_.score_algae = true;
+  double op_deadband = GetPreferenceValue_double("op_deadband");
+
+  if (op_readings.left_stick_y > op_deadband)
+    ci_readings_.inc_telescope = true;
+  else if (op_readings.left_stick_y < -op_deadband)
+    ci_readings_.dec_telescope = true;
+  if (op_readings.left_stick_x > op_deadband)
+    ci_readings_.inc_c_wrist = true;
+  else if (op_readings.left_stick_x < -op_deadband)
+    ci_readings_.dec_c_wrist = true;
+
+  if (op_readings.right_stick_y > op_deadband)
+    ci_readings_.inc_elevator = true;
+  else if (op_readings.right_stick_y < -op_deadband)
+    ci_readings_.dec_elevator = true;
+  if (op_readings.right_stick_x > op_deadband)
+    ci_readings_.inc_a_wrist = true;
+  else if (op_readings.right_stick_x < -op_deadband)
+    ci_readings_.dec_a_wrist = true;
+
+  ci_readings_.score_coral = op_readings.left_bumper;
+  ci_readings_.score_algae = (dr_readings.pov == frc846::robot::XboxPOV::kDown);
 
   if (op_readings.left_trigger && !previous_operator_.left_trigger) {
     climb_state_ += 1;

src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc

@@ -21,7 +21,9 @@ AlgalEESubsystem::AlgalEESubsystem()
           GetCurrentConfig(motor_configs_)},
       esc_2_{frc846::control::base::SPARK_MAX_NEO550,
           (GetCurrentConfig(GetModifiedConfig(motor_configs_,
-              ports::algal_ss_::end_effector_::kEE2_CANID, true)))} {}
+              ports::algal_ss_::end_effector_::kEE2_CANID, true)))} {
+  RegisterPreference("idle_speed", -0.04);
+}
 
 frc846::control::config::MotorConstructionParameters
 AlgalEESubsystem::GetCurrentConfig(
@@ -41,12 +43,12 @@ AlgalEESubsystem::GetCurrentConfig(
 
 void AlgalEESubsystem::Setup() {
   esc_1_.Setup();
-  esc_1_.EnableStatusFrames({frc846::control::config::kFaultFrame});
+  esc_1_.EnableStatusFrames({});
 
   esc_2_.Setup();
-  esc_2_.EnableStatusFrames({});
+  esc_2_.EnableStatusFrames({frc846::control::config::kFaultFrame});
 
-  esc_1_.ConfigReverseLimitSwitch(
+  esc_2_.ConfigReverseLimitSwitch(
       false, frc846::control::base::LimitSwitchDefaultState::kNormallyOff);
 }
 
@@ -59,14 +61,17 @@ bool AlgalEESubsystem::VerifyHardware() {
 
 AlgalEEReadings AlgalEESubsystem::ReadFromHardware() {
   AlgalEEReadings readings;
-  readings.has_piece_ = esc_1_.GetReverseLimitSwitchState();
+  readings.has_piece_ = esc_2_.GetReverseLimitSwitchState();
   Graph("readings/has_piece", readings.has_piece_);
   return readings;
 }
 
 void AlgalEESubsystem::WriteToHardware(AlgalEETarget target) {
   Graph("target/duty_cycle", target.duty_cycle_);
 
+  if (GetReadings().has_piece_ && target.duty_cycle_ > 0.0) {
+    target.duty_cycle_ = GetPreferenceValue_double("idle_speed");
+  }
   esc_1_.WriteDC(target.duty_cycle_);
   esc_2_.WriteDC(target.duty_cycle_);
 }