Tested point traversal

src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc

@@ -55,6 +55,8 @@ SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate(
 
   if (inputs.cut_excess_steering) {
     units::radians_per_second_t rotation_target = inputs.rotation_target;
+    units::radians_per_second_t abs_rotation_target =
+        units::math::abs(inputs.rotation_target);
     units::feet_per_second_t max_mag;
 
     do {
@@ -63,8 +65,10 @@ SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate(
       max_mag = get_max_mag();
 
       if (max_mag > inputs.max_speed) {
-        rotation_target -= constants_.rotation_iter_dec;
-        if (rotation_target < 0_rad_per_s) {
+        abs_rotation_target -= constants_.rotation_iter_dec;
+        rotation_target =
+            units::math::copysign(abs_rotation_target, rotation_target);
+        if (abs_rotation_target < 0_rad_per_s) {
           calculate_module_targets(0_rad_per_s);
           max_mag = get_max_mag();
           if (max_mag > inputs.max_speed) { rescale_outputs(get_max_mag()); }

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

@@ -27,7 +27,7 @@ void DriveToPointCommand::Execute() {
   DrivetrainAccelerationControlTarget dt_target{
       .linear_acceleration = max_acceleration_,
       .accel_dir = (target_.point - start_point_).angle(true),
-      .angular_velocity = 0_deg_per_s,  // TODO: add bearing control
+      .angular_velocity = 0_deg_per_s,
   };
 
   units::second_t t_decel =
@@ -37,7 +37,10 @@ void DriveToPointCommand::Execute() {
       (dt_readings.pose.velocity.magnitude() * t_decel) -
       ((max_deceleration_ * t_decel * t_decel) / 2.0);
 
-  if ((target_.point - start_point_).magnitude() <= stopping_distance) {
+  if ((target_.point - start_point_).magnitude() <=
+      stopping_distance -
+          drivetrain_->GetPreferenceValue_unit_type<units::inch_t>(
+              "drive_to_subtract")) {
     if (dt_readings.pose.velocity.magnitude() < 4_fps &&
         target_.velocity > 1_fps) {
       dt_target.accel_dir = dt_readings.pose.velocity.angle(true) + 180_deg;
@@ -46,7 +49,7 @@ void DriveToPointCommand::Execute() {
           (dt_readings.pose.position - start_point_).angle(true);
     }
     dt_target.linear_acceleration = max_deceleration_;
-  } else if (dt_readings.pose.velocity.magnitude() < target_.velocity) {
+  } else if (dt_readings.pose.velocity.magnitude() < max_speed_) {
     dt_target.linear_acceleration = max_acceleration_;
     dt_target.accel_dir =
         (target_.point - dt_readings.pose.position).angle(true);
@@ -56,6 +59,8 @@ void DriveToPointCommand::Execute() {
         (target_.point - dt_readings.pose.position).angle(true);
   }
 
+  dt_target.angular_velocity = drivetrain_->ApplyBearingPID(target_.bearing);
+
   drivetrain_->SetTarget(dt_target);
 }
 

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

@@ -32,8 +32,11 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
   RegisterPreference("lock_gains/_kD", 0.0);
   RegisterPreference("lock_gains/deadband", 2_in);
 
+  RegisterPreference("drive_to_subtract", 5_in);
+
   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);
 
@@ -109,7 +112,7 @@ units::degrees_per_second_t DrivetrainSubsystem::ApplyBearingPID(
   units::degrees_per_second_t yaw_rate = GetReadings().yaw_rate;
 
   units::degree_t error =
-      frc846::math::CoterminalDifference(bearing, target_bearing);
+      frc846::math::CoterminalDifference(target_bearing, bearing);
 
   Graph("bearing_pid/bearing", bearing);
   Graph("bearing_pid/target", target_bearing);
@@ -171,9 +174,9 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
 
   frc846::robot::swerve::odometry::SwervePose new_pose{
       .position = odometry_
-                      .calculate({bearing, steer_positions, drive_positions,
-                          GetPreferenceValue_double("odom_fudge_factor")})
-                      .position,
+          .calculate({bearing, steer_positions, drive_positions,
+              GetPreferenceValue_double("odom_fudge_factor")})
+          .position,
       .bearing = bearing,
       .velocity = velocity,
   };
@@ -189,13 +192,14 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
 frc846::math::VectorND<units::feet_per_second, 2>
 DrivetrainSubsystem::compensateForSteerLag(
     frc846::math::VectorND<units::feet_per_second, 2> uncompensated) {
-  units::degree_t steer_lag_compensation =
-      -GetPreferenceValue_unit_type<units::second_t>("steer_lag") *
-      GetReadings().yaw_rate;
+  // units::degree_t steer_lag_compensation =
+  //     -GetPreferenceValue_unit_type<units::second_t>("steer_lag") *
+  //     GetReadings().yaw_rate;
 
-  Graph("target/steer_lag_compensation", steer_lag_compensation);
+  // Graph("target/steer_lag_compensation", steer_lag_compensation);
 
-  return uncompensated.rotate(steer_lag_compensation, true);
+  // return uncompensated.rotate(steer_lag_compensation, true);
+  return uncompensated;
 }
 
 void DrivetrainSubsystem::WriteVelocitiesHelper(
@@ -250,12 +254,18 @@ void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
                           frc846::math::VectorND<units::feet_per_second, 2>{
                               accel_buffer, accel_target->accel_dir, true};
 
+    units::degrees_per_second_t angular_vel = units::math::min(
+        units::math::max(accel_target->angular_velocity,
+            -GetPreferenceValue_unit_type<units::degrees_per_second_t>(
+                "max_omega_cut")),
+        GetPreferenceValue_unit_type<units::degrees_per_second_t>(
+            "max_omega_cut"));
+
     units::feet_per_second_t speed_limit =
         GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed");
     if (accel_target->speed_limit >= 1_fps)
       speed_limit = accel_target->speed_limit;
-    WriteVelocitiesHelper(
-        vel_new_target, accel_target->angular_velocity, true, speed_limit);
+    WriteVelocitiesHelper(vel_new_target, angular_vel, true, speed_limit);
   }
 
   for (int i = 0; i < 4; i++)

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

@@ -14,6 +14,8 @@ void LockToPointCommand::Initialize() { Log("LockToPointCommand initialized"); }
 void LockToPointCommand::Execute() {
   frc846::math::Vector2D r_vec =
       target_.point - drivetrain_->GetReadings().pose.position;
+  Graph("lock_to_point/x_err", r_vec[0]);
+  Graph("lock_to_point/y_err", r_vec[1]);
   if (r_vec.magnitude() <
       drivetrain_->GetPreferenceValue_unit_type<units::inch_t>(
           "lock_gains/deadband")) {
@@ -32,7 +34,7 @@ void LockToPointCommand::Execute() {
 
     drivetrain_->SetTarget(DrivetrainOLControlTarget{
         frc846::math::VectorND<units::feet_per_second, 2>{
-            speed_target, r_vec.angle(true), true},
+            speed_target, r_vec.angle(true) + 180_deg, true},
         drivetrain_->ApplyBearingPID(target_.bearing)});
   }
 }

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

@@ -98,6 +98,8 @@ void SwerveModuleSubsystem::ZeroWithCANcoder() {
   constexpr int kMaxAttempts = 5;
   constexpr int kSleepTimeMs = 500;
 
+  last_rezero = 0;
+
   for (int attempts = 1; attempts <= kMaxAttempts; ++attempts) {
     Log("CANCoder zero attempt {}/{}", attempts, kMaxAttempts);
     auto position = cancoder_.GetAbsolutePosition();
@@ -109,14 +111,12 @@ void SwerveModuleSubsystem::ZeroWithCANcoder() {
     if (position.IsAllGood()) {
       steer_helper_.SetPosition(position_zero);
       Log("Zeroed to {}!", position_zero);
-      just_zeroed = true;
       return;
     } else if (attempts == kMaxAttempts) {
       Error("Unable to zero normally after {} attempts - attempting anyways",
           kMaxAttempts);
       steer_helper_.SetPosition(position_zero);
       Warn("Unreliably zeroed to {}!", position_zero);
-      just_zeroed = true;
       return;
     }
 
@@ -161,9 +161,9 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) {
 
   drive_helper_.WriteDC(cosine_comp * drive_duty_cycle);
 
-  if (std::abs(drive_duty_cycle) > 0.002 || just_zeroed) {
+  if (std::abs(drive_duty_cycle) > 0.002 || last_rezero < 50) {
     steer_helper_.WritePositionOnController(steer_dir);
-    just_zeroed = false;
+    last_rezero += 1;
   }
 }
 

src/frc846/include/frc846/robot/swerve/swerve_module.h

@@ -94,7 +94,7 @@ class SwerveModuleSubsystem
   void SetSteerGains(frc846::control::base::MotorGains gains);
 
 private:
-  bool just_zeroed = false;
+  int last_rezero = 101;
 
   /*
   getMotorParams()

src/y2025/cpp/control_triggers.cc

@@ -25,7 +25,7 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
 
   test_move_10_ft_trigger.WhileTrue(
       frc846::robot::swerve::DriveToPointCommand{&container.drivetrain_,
-          {{0_ft, 10_ft}, 0_deg, 0_fps}, 4_fps, 20_fps_sq, 15_fps_sq}
+          {{0_ft, 9_ft}, 0_deg, 0_fps}, 35_fps, 20_fps_sq, 15_fps_sq}
           .ToPtr());
 
   frc2::Trigger test_bearing_pid_trigger{[&] {
@@ -40,9 +40,8 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
   }};
 
   test_lock_trigger.WhileTrue(frc846::robot::swerve::LockToPointCommand{
-      &container.drivetrain_,
-      {{0_ft, 10_ft}, 0_deg,
-          0_fps}}.ToPtr());
+      &container.drivetrain_, {{0_ft, 0_ft}, 0_deg, 0_fps}}
+          .ToPtr());
 
   // END FAKE
 }