Blue side works -- sketchy fix

src/frc846/cpp/frc846/other/swerve_odometry.cc

@@ -1,5 +1,7 @@
 #include "frc846/other/swerve_odometry.h"
 
+#include <frc/DriverStation.h>
+
 #include <cmath>
 #include <cstdio>
 
@@ -15,6 +17,12 @@ void SwerveOdometry::Update(
     std::array<frc846::math::VectorND<units::foot_t, 2>, kModuleCount>
         wheel_vecs,
     units::radian_t bearing) {
+  bool is_red = true;
+  if (auto alliance = frc::DriverStation::GetAlliance()) {
+    if (alliance.value() == frc::DriverStation::Alliance::kBlue) {
+      is_red = false;
+    }
+  }
   // change in distance from the last odometry update
   for (int i = 0; i < kModuleCount; i++) {
     units::foot_t wheel_dist = wheel_vecs[i].magnitude();
@@ -36,6 +44,7 @@ void SwerveOdometry::Update(
     relative_displacement += wheel_vecs[i] / kModuleCount;
   }
 
+  if (!is_red) bearing += 180_deg;
   position_ += relative_displacement.rotate(bearing, true);
 }
 

src/y2024/cpp/commands/basic/pass_command.cc

@@ -3,14 +3,12 @@
 PassCommand::PassCommand(RobotContainer& container)
     : frc846::robot::GenericCommand<RobotContainer, PassCommand>{
           container, "pass_command"} {
-  AddRequirements({&container_.super_structure_, &container_.intake_,
-                   &container_.shooter_});
+  AddRequirements({&container_.super_structure_});
 }
 
 void PassCommand::OnInit() {}
 
 void PassCommand::Periodic() {
-  container_.intake_.SetTarget({IntakeState::kHold});
   container_.shooter_.SetTarget({ShooterState::kRun});
 
   container_.super_structure_.SetTargetSetpoint(

src/y2024/cpp/subsystems/hardware/bracer.cc

@@ -35,10 +35,10 @@ BracerReadings BracerSubsystem::ReadFromHardware() {
 void BracerSubsystem::WriteToHardware(BracerTarget target) {
   if (target.state == BracerState::kExtend) {
     frc846::util::ShareTables::SetBoolean("is_climb_sequence", true);
-    bracer_.Set(1.0);
+    bracer_.Set(-0.3);
   } else if (target.state == BracerState::kRetract) {
     frc846::util::ShareTables::SetBoolean("is_climb_sequence", false);
-    bracer_.Set(-0.7);
+    bracer_.Set(0.7);
   } else {
     bracer_.Set(0.0);
   }

src/y2024/cpp/subsystems/hardware/drivetrain.cc

@@ -227,7 +227,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
   readings.pose = frc846::math::FieldPoint(odometry_.position(), bearing,
                                            unfiltered_velocity);
 
-  pose_x_graph_.Graph(odometry_.position()[0]);
+    pose_x_graph_.Graph(odometry_.position()[0]);
   pose_y_graph_.Graph(odometry_.position()[1]);
   // pose_bearing_graph.Graph(odometry_.position().bearing);
   v_x_graph_.Graph(readings.pose.velocity[0]);

src/y2024/cpp/subsystems/hardware/pivot.cc

@@ -86,15 +86,15 @@ PivotReadings PivotSubsystem::ReadFromHardware() {
 
 void PivotSubsystem::WriteToHardware(PivotTarget target) {
   if (target.climb_mode) {
-    hard_limits_.OverrideLimits(true);
+    // hard_limits_.OverrideLimits(true);
 
     pivot_one_.WriteDC(climb_duty_cycle_.value());
     pivot_two_.WriteDC(climb_duty_cycle_.value());
     pivot_three_.WriteDC(climb_duty_cycle_.value());
     pivot_four_.WriteDC(climb_duty_cycle_.value());
 
   } else if (auto pos = std::get_if<units::degree_t>(&target.pivot_output)) {
-    hard_limits_.OverrideLimits(false);
+    // hard_limits_.OverrideLimits(false);
 
     double output = dyFPID.calculate(*pos, GetReadings().pivot_position,
                                      pivot_one_.GetVelocityPercentage(),
@@ -107,7 +107,7 @@ void PivotSubsystem::WriteToHardware(PivotTarget target) {
 
     target_pivot_pos_graph.Graph(*pos);
   } else if (auto output = std::get_if<double>(&target.pivot_output)) {
-    hard_limits_.OverrideLimits(false);
+    // hard_limits_.OverrideLimits(false);
 
     pivot_one_.WriteDC(*output);
     pivot_two_.WriteDC(*output);

src/y2024/include/subsystems/abstract/super_structure.h

@@ -99,7 +99,6 @@ class SuperStructureSubsystem
   }
 
   bool CheckValidAdjustment(PTWSetpoint adjusted) {
-    return true;
     return arm_kinematics_calculator.WithinBounds(
         arm_kinematics_calculator.calculate(
             {(currentSetpoint.pivot + adjusted.pivot),

src/y2024/resources/preferences_backup.nform

@@ -1,80 +1,56 @@
-Default.int | 0
 Preferences/MotionTargets/Pivot/amp_position (deg).double | 70.000000
 Preferences/MotionTargets/Pivot/auto_shoot_position (deg).double | 10.000000
 Preferences/MotionTargets/Pivot/intake_position (deg).double | -17.000000
 Preferences/MotionTargets/Pivot/pass_position (deg).double | 70.000000
 Preferences/MotionTargets/Pivot/preclimb_position (deg).double | 83.000000
 Preferences/MotionTargets/Pivot/prescore_position (deg).double | 80.000000
-Preferences/MotionTargets/Pivot/shoot_position (deg).double | 70.000000
+Preferences/MotionTargets/Pivot/shoot_position (deg).double | 7.000000
 Preferences/MotionTargets/Pivot/source_position (deg).double | 36.000000
 Preferences/MotionTargets/Pivot/stow_position (deg).double | -17.000000
 Preferences/MotionTargets/Pivot/trapscore_position (deg).double | 80.000000
 Preferences/MotionTargets/Telescope/amp_position (in).double | 4.000000
 Preferences/MotionTargets/Telescope/trapscore_position (in).double | 3.000000
 Preferences/MotionTargets/Wrist/amp_position (deg).double | 100.000000
-Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 50.500000
+Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 42.500000
 Preferences/MotionTargets/Wrist/intake_position (deg).double | 32.000000
 Preferences/MotionTargets/Wrist/pass_position (deg).double | 80.000000
 Preferences/MotionTargets/Wrist/preclimb_position (deg).double | -30.000000
 Preferences/MotionTargets/Wrist/prescore_position (deg).double | 40.000000
-Preferences/MotionTargets/Wrist/shoot_position (deg).double | 36.500000
+Preferences/MotionTargets/Wrist/shoot_position (deg).double | 41.000000
 Preferences/MotionTargets/Wrist/source_position (deg).double | -10.000000
 Preferences/MotionTargets/Wrist/stow_position (deg).double | -40.000000
 Preferences/MotionTargets/Wrist/trapscore_position (deg).double | 80.000000
-Preferences/field_points/5p_intake_one_y (in).double | 94.000000
-Preferences/field_points/5p_intake_three_deg (deg).double | 0.000000
-Preferences/field_points/5p_intake_three_x (in).double | 153.000000
-Preferences/field_points/5p_intake_three_y (in).double | 94.000000
-Preferences/field_points/5p_intake_two_deg (deg).double | 50.000000
-Preferences/field_points/5p_intake_two_x (in).double | 160.500000
-Preferences/field_points/5p_intake_two_y (in).double | 112.000000
-Preferences/field_points/5p_mid_one_deg (deg).double | 0.000000
-Preferences/field_points/5p_mid_one_x (in).double | 217.500000
-Preferences/field_points/5p_mid_one_y (in).double | 70.000000
-Preferences/field_points/5p_mid_three_deg (deg).double | 0.000000
-Preferences/field_points/5p_mid_three_x (in).double | 117.000000
-Preferences/field_points/5p_mid_three_y (in).double | 70.000000
-Preferences/field_points/5p_mid_two_deg (deg).double | 0.000000
-Preferences/field_points/5p_mid_two_x (in).double | 230.000000
-Preferences/field_points/5p_mid_two_y (in).double | 70.000000
-Preferences/field_points/5p_shoot_one_y (in).double | 51.000000
-Preferences/field_points/5p_shoot_three_x (in).double | 213.000000
-Preferences/field_points/5p_shoot_three_y (in).double | 52.000000
-Preferences/field_points/5p_shoot_two_y (in).double | 52.000000
-Preferences/field_points/testing_point_y (in).double | 10000.000000
 SuperStructure/auto/post_shoot_wait (s).double | 0.100000
 SuperStructure/auto/pre_shoot_wait (s).double | 0.100000
 SuperStructure/manual_control_deadband.double | 0.150000
 SuperStructure/shooting/auto_shooter_height (in).double | 23.000000
 SuperStructure/shooting/auto_shooter_x (in).double | 15.000000
-SuperStructure/shooting/teleop_shooter_height (in).double | 39.000000
-drivetrain/Configs/CurrentLimiting/target_threshold (A).double | 40.000000
-drivetrain/Configs/default_brake_mode.bool | false
-drivetrain/Configs/gear_ratio.double | 16.800000
-drivetrain/Gains/kF.double | 0.000000
-drivetrain/Gains/kP.double | 0.120000
-drivetrain/auto_max_speed (fps).double | 15.000000
+SuperStructure/shooting/teleop_shooter_height (in).double | 23.650000
+SuperStructure/shooting/teleop_shooter_x (in).double | 25.000000
+drivetrain/auto_max_speed (fps).double | 13.000000
 drivetrain/bearing_gains/d.double | -13.500000
 drivetrain/bearing_gains/p.double | 2.000000
-drivetrain/drive_esc/Configs/CurrentLimiting/target_threshold (A).double | 240.000000
+drivetrain/drive_esc/Configs/CurrentLimiting/peak_time_threshold (ms).double | 1000.000000
+drivetrain/drive_esc/Configs/CurrentLimiting/target_threshold (A).double | 480.000000
 drivetrain/drive_esc/Gains/kF.double | 0.205000
 drivetrain/drive_esc/Gains/kP.double | 0.010000
 drivetrain/driver_speed_multiplier.double | 1.000000
-drivetrain/max_acceleration (fps_sq).double | 28.000000
-drivetrain/max_speed (fps).double | 2.400000
-drivetrain/module_BL/cancoder_offset (deg).double | -81.300000
-drivetrain/module_BR/cancoder_offset (deg).double | -173.900000
-drivetrain/module_FL/cancoder_offset (deg).double | 100.400000
-drivetrain/module_FR/cancoder_offset (deg).double | 24.400000
+drivetrain/max_acceleration (fps_sq).double | 16.000000
+drivetrain/max_deceleration (fps_sq).double | 12.000000
+drivetrain/max_speed (fps).double | 15.000000
+drivetrain/module_BL/cancoder_offset (deg).double | -82.700000
+drivetrain/module_BR/cancoder_offset (deg).double | -171.300000
+drivetrain/module_FL/cancoder_offset (deg).double | 99.800000
+drivetrain/module_FR/cancoder_offset (deg).double | 22.900000
 drivetrain/percent_max_omega.double | 0.450000
 drivetrain/pov_control_speed_.double | 10.000000
-drivetrain/tolerated_skid.double | 1.500000
-drivetrain/wheel_radius (in).double | 2.000000
+drivetrain/smart_current_braking/smart_current_limit (A).double | 150.000000
+drivetrain/wheel_radius (in).double | 1.910000
 intake/Configs/ramp_time (s).double | 1.000000
 intake/Configs/use_ramp_rate.bool | true
 intake/Gains/kF.double | 0.000190
 intake/Gains/kP.double | 0.000000
-intake/base_intake_speed_ (fps).double | 3.000000
+intake/base_intake_speed_ (fps).double | 2.000000
 intake/intake_feed_speed_ (fps).double | 6.000000
 intake/release_speed.double | -0.400000
 intake/retract_speed.double | 0.100000
@@ -86,11 +62,11 @@ pivot/Configs/HardLimits/use_position_limits.bool | true
 pivot/Configs/invert.bool | true
 pivot/DynamicFPID/braking_p.double | 0.400000
 pivot/DynamicFPID/smart_current_limit (A).double | 30.000000
-pivot/Gains/kD.double | -1.000000
+pivot/Gains/kD.double | -1.500000
 pivot/Gains/kF.double | 0.044000
 pivot/Gains/kP.double | 0.045000
 pivot/pivot_home_offset (deg).double | -17.000000
-pivot/pivot_tolerance (deg).double | 1.500000
+pivot/pivot_tolerance (deg).double | 2.500000
 robot_container/init_bracers_.bool | false
 robot_container/init_drivetrain.bool | true
 robot_container/init_intake.bool | true
@@ -100,28 +76,35 @@ robot_container/init_telescope.bool | true
 robot_container/init_wrist.bool | true
 shooter/Configs/invert.bool | true
 shooter/Gains/kF.double | 0.008000
-shooter/Gains/kP.double | 0.075000
+shooter/Gains/kP.double | 0.050000
 shooter/shooter_speed (fps).double | 100.000000
 shooter/shooter_speed_tolerance.double | 0.030000
 shooter/shooter_spin.double | 0.000000
-shooter/shooting_exit_velocity.double | 45.000000
+shooter/shooting_exit_velocity.double | 80.000000
 shooter/vFPID/smart_current_limit (A).double | 90.000000
 telescope/Gains/kD.double | -0.100000
 vision/camera_x_offset (in).double | -7.000000
 vision/camera_y_offset (in).double | -10.000000
 vision/can_bus_latency (ms).double | 20.000000
 vision/default_is_red_side.bool | true
-wrist/CloseDynamicFPID/smart_current_limit (A).double | 15.000000
-wrist/Configs/HardLimits/forward (deg).double | 130.000000
-wrist/Configs/HardLimits/reverse (deg).double | -42.000000
+vision/using_vision_autos.bool | false
+wrist/Configs/HardLimits/forward (deg).double | 170.000000
+wrist/Configs/HardLimits/peak_output_reverse.double | -0.500000
+wrist/Configs/HardLimits/reverse (deg).double | -30.000000
 wrist/Configs/HardLimits/use_position_limits.bool | true
 wrist/Configs/default_brake_mode.bool | false
+wrist/Configs/gear_ratio.double | 9.000000
 wrist/Configs/invert.bool | true
-wrist/DynamicFPID/smart_current_limit (A).double | 50.000000
-wrist/Gains/kD.double | -1.650000
-wrist/Gains/kF.double | -0.065000
-wrist/Gains/kP.double | 0.033000
+wrist/DynamicFPID/Gains/kD.double | -2.400000
+wrist/DynamicFPID/Gains/kF.double | -0.130000
+wrist/DynamicFPID/Gains/kP.double | 0.030000
+wrist/DynamicFPID/smart_current_limit (A).double | 40.000000
+wrist/Gains/kD.double | 0.000000
+wrist/Gains/kF.double | 0.000000
+wrist/Gains/kP.double | 0.000600
 wrist/cg_offset_wrist (deg).double | 155.000000
 wrist/home_offset_wrist (deg).double | -42.000000
-wrist/homing_speed.double | -0.350000
+wrist/homing_dc_cut.double | 1.600000
+wrist/homing_max_speed (deg_per_s).double | 55.000000
+wrist/homing_speed.double | -0.100000
 wrist/homing_velocity_tolerance (deg_per_s).double | 0.500000