Day 0

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

@@ -23,55 +23,55 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
   RegisterPreference("steer_gains/_kD", 0.0);
   RegisterPreference("steer_gains/_kF", 0.0);
 
-  RegisterPreference("bearing_gains/_kP", 0.5);
+  RegisterPreference("bearing_gains/_kP", 9);
   RegisterPreference("bearing_gains/_kI", 0.0);
-  RegisterPreference("bearing_gains/_kD", 0.0);
+  RegisterPreference("bearing_gains/_kD", -0.6);
   RegisterPreference("bearing_gains/deadband", 3.0_deg_per_s);
 
-  RegisterPreference("lock_gains/_kP", 0.5);
+  RegisterPreference("lock_gains/_kP", -2);
   RegisterPreference("lock_gains/_kI", 0.0);
-  RegisterPreference("lock_gains/_kD", 0.0);
-  RegisterPreference("lock_gains/deadband", 2_in);
+  RegisterPreference("lock_gains/_kD", 0.01);
+  RegisterPreference("lock_gains/deadband", .5_in);
   RegisterPreference("lock_adj_rate", 0.05_in);
   RegisterPreference("lock_max_speed", 7_fps);
   RegisterPreference("auto_max_speed", 15_fps);
 
   RegisterPreference("drive_to_subtract", 2_in);
 
-  RegisterPreference("bearing_latency", 100_ms);
+  RegisterPreference("bearing_latency", 0_ms);
   RegisterPreference("drive_latency", 0_ms);
 
   RegisterPreference("max_speed", 15_fps);
   RegisterPreference("max_omega", units::degrees_per_second_t{180});
   RegisterPreference("max_omega_cut", units::degrees_per_second_t{40});
 
-  RegisterPreference("odom_fudge_factor", 0.875);
+  RegisterPreference("odom_fudge_factor", 1.0);
   RegisterPreference("odom_variance", 0.2);
 
   RegisterPreference("steer_lag", 0.05_s);
   RegisterPreference("bearing_latency", 0.01_s);
 
-  RegisterPreference("pose_estimator/pose_variance", 0.01);
+  RegisterPreference("pose_estimator/pose_variance", 0.1);
   RegisterPreference("pose_estimator/velocity_variance", 1.0);
   RegisterPreference("pose_estimator/accel_variance", 1.0);
   RegisterPreference("pose_estimator/override", false);
 
-  RegisterPreference("april_tags/april_variance_coeff", 0.33);
-  RegisterPreference("april_tags/triangular_variance_coeff", 0.22);
-  RegisterPreference("april_tags/fudge_latency1", 70_ms);
-  RegisterPreference("april_tags/fudge_latency2", 110_ms);
+  RegisterPreference("april_tags/april_variance_coeff", 0.08);
+  RegisterPreference("april_tags/triangular_variance_coeff", 0.001);
+  RegisterPreference("april_tags/fudge_latency1", 140_ms);
+  RegisterPreference("april_tags/fudge_latency2", 350_ms);
 
   RegisterPreference("rc_control_speed", 2.5_fps);
 
   RegisterPreference("accel_spike_thresh", 45_fps_sq);
   RegisterPreference("max_past_accel_spike", 25);
   RegisterPreference("accel_vel_stopped_thresh", 0.7_fps);
-  RegisterPreference("vel_stopped_thresh", 1.0_fps);
+  RegisterPreference("vel_stopped_thresh", 0.7_fps);
   RegisterPreference("stopped_num_loops", 25);
 
   RegisterPreference("lock_drive_early", 12_in);
   RegisterPreference("lock_drive_fvel", 1_fps);
-  RegisterPreference("drive_correctional_gain", 1.0);
+  RegisterPreference("drive_correctional_gain", 0.1);
 
   odometry_.setConstants({});
   ol_calculator_.setConstants({

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

@@ -67,12 +67,12 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() {
   ci_readings_.translate_x = dr_readings.left_stick_x;
   ci_readings_.translate_y = dr_readings.left_stick_y;
 
-  ci_readings_.rc_p_y = (int)dr_readings.pov == 0;
-  ci_readings_.rc_p_x = (int)dr_readings.pov == 90;
-  ci_readings_.rc_n_y = (int)dr_readings.pov == 180;
-  ci_readings_.rc_n_x = (int)dr_readings.pov == 270;
-  ci_readings_.rc_control = ci_readings_.rc_p_y || ci_readings_.rc_n_y ||
-                            ci_readings_.rc_p_x || ci_readings_.rc_n_x;
+  // ci_readings_.rc_p_y = (int)dr_readings.pov == 0;  // TODO: potentially
+  // wrong? ci_readings_.rc_p_x = (int)dr_readings.pov == 90;
+  // ci_readings_.rc_n_y = (int)dr_readings.pov == 180;
+  // ci_readings_.rc_n_x = (int)dr_readings.pov == 270;
+  // ci_readings_.rc_control = ci_readings_.rc_p_y || ci_readings_.rc_n_y ||
+  //                           ci_readings_.rc_p_x || ci_readings_.rc_n_x;
 
   ci_readings_.rotation = dr_readings.right_stick_x;
 

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

@@ -16,8 +16,8 @@ GPDSubsystem::GPDSubsystem(
   frc::SmartDashboard::PutData("GPDField", &g_field);
 #endif
 
-  RegisterPreference("intake_to_cam_y", -15_in);
-  RegisterPreference("intake_to_cam_x", -3_in);
+  RegisterPreference("intake_to_cam_y", 0_in);
+  RegisterPreference("intake_to_cam_x", -13_in);
   RegisterPreference("cam_h_angle", 0_deg);
 
   RegisterPreference("max_gp_diff", 10_in);

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

@@ -11,8 +11,8 @@ AlgalEESubsystem::AlgalEESubsystem()
           .can_id = ports::algal_ss_::end_effector_::kEE1_CANID,
           .inverted = false,
           .brake_mode = true,
-          .motor_current_limit = 10_A,
-          .smart_current_limit = 5_A,
+          .motor_current_limit = 40_A,
+          .smart_current_limit = 30_A,
           .voltage_compensation = 12_V,
           .circuit_resistance = robot_constants::algae_ss_::wire_resistance,
           .rotational_inertia = frc846::wpilib::unit_kg_m_sq{3.0},

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

@@ -18,25 +18,25 @@ AlgalSuperstructure::AlgalSuperstructure()
       elevator(),
       algal_wrist(),
       algal_end_effector() {
-  REGISTER_SETPOINT("stow", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("processor", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("ground_intake", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("on_top_intake", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("net", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("l2_pick", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("l3_pick", 0_in, 0_deg, 0.0);
+  REGISTER_SETPOINT("stow", 29_in, 0_deg, 0.3);
+  REGISTER_SETPOINT("processor", 29_in, 25_deg, 0.2);
+  REGISTER_SETPOINT("ground_intake", 29_in, 40_deg, 0.9);
+  REGISTER_SETPOINT("on_top_intake", 29_in, 40_deg, 0.9);
+  REGISTER_SETPOINT("net", 68.4_in, 0_deg, 0.2);
+  REGISTER_SETPOINT("l2_pick", 36_in, 30_deg, 0.7);
+  REGISTER_SETPOINT("l3_pick", 45_in, 30_deg, 0.7);
 
   REGISTER_SETPOINT("dinosaur_A", 0_in, 0_deg, 0.0);
   REGISTER_SETPOINT("dinosaur_B", 0_in, 0_deg, 0.0);
 
   RegisterPreference("score_dc", -0.5);
 
-  RegisterPreference("init_elevator", false);
-  RegisterPreference("init_wrist", false);
-  RegisterPreference("init_ee", false);
+  RegisterPreference("init_elevator", true);
+  RegisterPreference("init_wrist", true);
+  RegisterPreference("init_ee", true);
 
-  RegisterPreference("elevator_tolerance", 3_in);
-  RegisterPreference("wrist_tolerance", 7_deg);
+  RegisterPreference("elevator_tolerance", 5_in);
+  RegisterPreference("wrist_tolerance", 12_deg);
 
   RegisterPreference("elevator_adjustment", 0.04_in);
   RegisterPreference("wrist_adjustment", 0.2_deg);

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

@@ -2,6 +2,7 @@
 
 #include "frc846/control/calculators/CircuitResistanceCalculator.h"
 #include "ports.h"
+#include "subsystems/SubsystemHelper.h"
 #include "subsystems/robot_constants.h"
 
 AlgalWristSubsystem::AlgalWristSubsystem()
@@ -11,14 +12,20 @@ AlgalWristSubsystem::AlgalWristSubsystem()
               .can_id = ports::algal_ss_::wrist_::kWristMotor_CANID,
               .inverted = true,
               .brake_mode = true,
-              .motor_current_limit = 20_A,
-              .smart_current_limit = 10_A,
+              .motor_current_limit = 50_A,
+              .smart_current_limit = 40_A,
               .voltage_compensation = 12_V,
               .circuit_resistance = robot_constants::algae_ss_::wire_resistance,
               .rotational_inertia = frc846::wpilib::unit_kg_m_sq{3.0}},
           encoder_reduction * encoder_to_subsystem_reduction) {
+  REGISTER_PIDF_CONFIG(0.0, 0.0, 0.0, 0.0);
+  REGISTER_SOFTLIMIT_CONFIG(true, 90_deg, 0_deg, 80_deg, 10_deg, 0.3);
+
+  RegisterPreference("cg_offset", 90_deg);
+  RegisterPreference("flip_position_load_sign", true);
+
   RegisterPreference("use_sensor_threshold", 5_deg_per_s);
-  RegisterPreference("encoder_offset", 10_deg);
+  RegisterPreference("encoder_offset", 0_deg);
 }
 
 WristTarget AlgalWristSubsystem::ZeroTarget() const {

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

@@ -2,22 +2,26 @@
 
 #include "frc846/control/calculators/CircuitResistanceCalculator.h"
 #include "ports.h"
+#include "subsystems/SubsystemHelper.h"
 #include "subsystems/robot_constants.h"
 
 ElevatorSubsystem::ElevatorSubsystem()
     : LinearSubsystem("elevator",
           frc846::control::base::MotorMonkeyType::SPARK_MAX_NEO,
           frc846::control::config::MotorConstructionParameters{
               .can_id = ports::algal_ss_::elevator_::kElevator_CANID,
-              .inverted = false,
+              .inverted = true,
               .brake_mode = true,
-              .motor_current_limit = 40_A,
-              .smart_current_limit = 30_A,
+              .motor_current_limit = 90_A,
+              .smart_current_limit = 120_A,
               .voltage_compensation = 12_V,
               .circuit_resistance =
                   robot_constants::algae_ss_::wire_resistance_base,
               .rotational_inertia = frc846::wpilib::unit_kg_m_sq{1.0}},
-          1.0_in / 1_tr, robot_constants::elevator::elevator_hall_effect) {}
+          37_in / 64.579_tr, robot_constants::elevator::elevator_hall_effect) {
+  REGISTER_PIDF_CONFIG(0.0, 0.0, 0.0, 0.0);
+  REGISTER_SOFTLIMIT_CONFIG(true, 104_in, 40_in, 94_in, 29_in, 0.3);
+}
 
 LinearSubsystemTarget ElevatorSubsystem::ZeroTarget() const {
   return LinearSubsystemTarget{0_in};

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

@@ -2,6 +2,7 @@
 
 #include "frc846/control/calculators/CircuitResistanceCalculator.h"
 #include "ports.h"
+#include "subsystems/SubsystemHelper.h"
 #include "subsystems/robot_constants.h"
 
 ClimberSubsystem::ClimberSubsystem()
@@ -17,6 +18,9 @@ ClimberSubsystem::ClimberSubsystem()
               .circuit_resistance = robot_constants::climber_::wire_resistance,
               .rotational_inertia = frc846::wpilib::unit_kg_m_sq{1.0}},
           225_tr / 1_tr) {
+  REGISTER_PIDF_CONFIG(0.0, 0.0, 0.0, 0.0);
+  REGISTER_SOFTLIMIT_CONFIG(true, 90_deg, 0_deg, 90_deg, 0_deg, 0.3);
+
   RegisterPreference("pre_climb_setpoint", 270_deg);
   RegisterPreference("climb_setpoint", 100_deg);
   RegisterPreference("stow_setpoint", 0_deg);

src/y2025/cpp/subsystems/hardware/coral/coral_end_effector.cc

@@ -11,8 +11,8 @@ CoralEESubsystem::CoralEESubsystem()
           .can_id = ports::coral_ss_::end_effector_::kEE_CANID,
           .inverted = false,
           .brake_mode = true,
-          .motor_current_limit = 10_A,
-          .smart_current_limit = 5_A,
+          .motor_current_limit = 40_A,
+          .smart_current_limit = 30_A,
           .voltage_compensation = 12_V,
           .circuit_resistance = robot_constants::coral_ss_::wire_resistance,
           .rotational_inertia = frc846::wpilib::unit_kg_m_sq{0.5},

src/y2025/cpp/subsystems/hardware/coral/coral_ss.cc

@@ -18,30 +18,30 @@ CoralSuperstructure::CoralSuperstructure()
       telescope(),
       coral_wrist(),
       coral_end_effector() {
-  REGISTER_SETPOINT("stow_no_piece", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("stow_with_piece", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("score_l2", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("score_l3", 0_in, 0_deg, 0.0);
-  REGISTER_SETPOINT("score_l4", 0_in, 0_deg, 0.0);
+  REGISTER_SETPOINT("stow_no_piece", 29_in, 20_deg, -0.7);
+  REGISTER_SETPOINT("stow_with_piece", 35_in, 120_deg, -0.15);
+  REGISTER_SETPOINT("score_l2", 29_in, 220_deg, -0.15);
+  REGISTER_SETPOINT("score_l3", 48_in, 213.9_deg, -0.15);
+  REGISTER_SETPOINT("score_l4", 90_in, 263_deg, -0.15);  // 74.5, 39, 23.5
 
   REGISTER_SETPOINT("dinosaur_A", 0_in, 0_deg, 0.0);
   REGISTER_SETPOINT("dinosaur_B", 0_in, 0_deg, 0.0);
 
   RegisterPreference("score_dc", -0.5);
 
-  RegisterPreference("init_telescope", false);
-  RegisterPreference("init_wrist", false);
+  RegisterPreference("init_telescope", true);
+  RegisterPreference("init_wrist", true);
   RegisterPreference("init_ee", true);
 
   RegisterPreference("disable_distance_sensor", false);
 
-  RegisterPreference("telescope_tolerance", 1.2_in);
-  RegisterPreference("wrist_tolerance", 5_deg);
+  RegisterPreference("telescope_tolerance", 3_in);
+  RegisterPreference("wrist_tolerance", 9_deg);
 
   RegisterPreference("telescope_adjustment", 0.04_in);
   RegisterPreference("wrist_adjustment", 0.2_deg);
 
-  RegisterPreference("autostow", false);
+  RegisterPreference("autostow", true);
   RegisterPreference("stow_no_piece_loop_thresh", 20);
 
   last_state = kCoral_StowNoPiece;

src/y2025/cpp/subsystems/hardware/coral/coral_wrist.cc

@@ -2,6 +2,7 @@
 
 #include "frc846/control/calculators/CircuitResistanceCalculator.h"
 #include "ports.h"
+#include "subsystems/SubsystemHelper.h"
 #include "subsystems/robot_constants.h"
 
 CoralWristSubsystem::CoralWristSubsystem()
@@ -11,14 +12,20 @@ CoralWristSubsystem::CoralWristSubsystem()
               .can_id = ports::coral_ss_::wrist_::kWristMotor_CANID,
               .inverted = false,
               .brake_mode = true,
-              .motor_current_limit = 20_A,
-              .smart_current_limit = 10_A,
+              .motor_current_limit = 60_A,
+              .smart_current_limit = 50_A,
               .voltage_compensation = 12_V,
               .circuit_resistance = robot_constants::coral_ss_::wire_resistance,
               .rotational_inertia = frc846::wpilib::unit_kg_m_sq{1.0}},
           subsystem_reduction) {
-  RegisterPreference("use_sensor_threshold", 5_deg_per_s);
-  RegisterPreference("encoder_offset", 10_deg);
+  REGISTER_PIDF_CONFIG(0.0, 0.0, 0.0, 0.0);
+  REGISTER_SOFTLIMIT_CONFIG(true, 260_deg, 5_deg, 245_deg, 15_deg, 0.3);
+
+  RegisterPreference("cg_offset", -50.0_deg);
+  RegisterPreference("flip_position_load_sign", false);
+
+  RegisterPreference("use_sensor_threshold", 2_deg_per_s);
+  RegisterPreference("encoder_offset", 0_deg);
 }
 
 WristTarget CoralWristSubsystem::ZeroTarget() const {

src/y2025/cpp/subsystems/hardware/coral/telescope.cc

@@ -2,23 +2,27 @@
 
 #include "frc846/control/calculators/CircuitResistanceCalculator.h"
 #include "ports.h"
+#include "subsystems/SubsystemHelper.h"
 #include "subsystems/robot_constants.h"
 
 TelescopeSubsystem::TelescopeSubsystem()
     : LinearSubsystem("telescope",
-          frc846::control::base::MotorMonkeyType::SPARK_FLEX_VORTEX,
+          frc846::control::base::MotorMonkeyType::SPARK_MAX_NEO,
           frc846::control::config::MotorConstructionParameters{
               .can_id = ports::coral_ss_::telescope_::kTelescope_CANID,
               .inverted = true,
               .brake_mode = true,
-              .motor_current_limit = 40_A,
-              .smart_current_limit = 30_A,
+              .motor_current_limit = 90_A,
+              .smart_current_limit = 120_A,
               .voltage_compensation = 12_V,
               .circuit_resistance =
                   robot_constants::coral_ss_::wire_resistance_base,
               .rotational_inertia = frc846::wpilib::unit_kg_m_sq{1.0}},
           52.5_in / 23.85_tr,
-          robot_constants::telescope::telescope_hall_effect) {}
+          robot_constants::telescope::telescope_hall_effect) {
+  REGISTER_PIDF_CONFIG(0.025, 0.0, -0.001, 0.007);
+  REGISTER_SOFTLIMIT_CONFIG(true, 90.5_in, 28.5_in, 78_in, 38.5_in, 0.3);
+}
 
 LinearSubsystemTarget TelescopeSubsystem::ZeroTarget() const {
   return LinearSubsystemTarget{0_in};

src/y2025/cpp/subsystems/hardware/generic/linear_subsystem.cc

@@ -10,9 +10,6 @@ LinearSubsystem::LinearSubsystem(std::string name,
           LinearSubsystemTarget>(name),
       linear_esc_(mmtype, GetCurrentConfig(motor_configs_)),
       hall_effect_loc_(hall_effect_loc_) {
-  REGISTER_PIDF_CONFIG(0.0, 0.0, 0.0, 0.0);
-  REGISTER_SOFTLIMIT_CONFIG(true, 45_in, 2_in, 40_in, 10_in, 0.3);
-
   linear_esc_helper_.SetConversion(conversion);
 
   linear_esc_helper_.bind(&linear_esc_);

src/y2025/cpp/subsystems/hardware/generic/wrist_subsystem.cc

@@ -8,14 +8,8 @@ WristSubsystem::WristSubsystem(std::string name,
     wrist_pos_conv_t conversion)
     : frc846::robot::GenericSubsystem<WristReadings, WristTarget>(name),
       wrist_esc_(mmtype, GetCurrentConfig(motor_configs_)) {
-  REGISTER_PIDF_CONFIG(0.0, 0.0, 0.0, 0.0);
-  REGISTER_SOFTLIMIT_CONFIG(true, 90_deg, 0_deg, 90_deg, 0_deg, 0.3);
-
   wrist_esc_helper_.SetConversion(conversion);
 
-  RegisterPreference("cg_offset", 0.0_deg);
-  RegisterPreference("flip_position_load_sign", false);
-
   RegisterPreference("rezero_thresh", 2_deg);
 
   wrist_esc_helper_.bind(&wrist_esc_);

src/y2025/include/subsystems/robot_container.h

@@ -37,7 +37,7 @@ class RobotContainer : public frc846::robot::GenericRobotContainer {
 
     bool drivetrain_init = (GetPreferenceValue_bool("init_drivetrain"));
     bool leds_init = (GetPreferenceValue_bool("init_leds"));
-    bool gpd_init = true;  //(GetPreferenceValue_bool("init_gpd"));
+    bool gpd_init = (GetPreferenceValue_bool("init_gpd"));
 
     bool coral_ss_init = (GetPreferenceValue_bool("init_coral_ss"));
     bool algal_ss_init = (GetPreferenceValue_bool("init_algal_ss"));