Improved drive_to_point accuracy

Team846 · Jan 30, 2025 · f0a9aab · f0a9aab
1 parent 85ee25f
commit f0a9aab
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 22 deletions.
diff --git a/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc b/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc
@@ -95,21 +95,21 @@ void TalonFX_interm::EnableStatusFrames(
     ctre::phoenix::StatusCode last_status_code = ctre::phoenix::StatusCode::OK;
     switch (frame) {
     case frc846::control::config::StatusFrame::kCurrentFrame:
-      last_status_code =
-          talon_.GetSupplyCurrent().SetUpdateFrequency(10_Hz, max_wait_time_);
+      last_status_code = talon_.GetSupplyCurrent().SetUpdateFrequency(
+          1.0 / velocity_ms, max_wait_time_);
       break;
     case frc846::control::config::StatusFrame::kPositionFrame:
-      last_status_code =
-          talon_.GetPosition().SetUpdateFrequency(50_Hz, max_wait_time_);
+      last_status_code = talon_.GetPosition().SetUpdateFrequency(
+          1.0 / encoder_position_ms, max_wait_time_);
       if (last_status_code != ctre::phoenix::StatusCode::OK) break;
 
       [[fallthrough]];  // Latency compensation requires vel, acc frames
     case frc846::control::config::StatusFrame::kVelocityFrame:
-      last_status_code =
-          talon_.GetVelocity().SetUpdateFrequency(40_Hz, max_wait_time_);
+      last_status_code = talon_.GetVelocity().SetUpdateFrequency(
+          1 / velocity_ms, max_wait_time_);
       if (last_status_code != ctre::phoenix::StatusCode::OK) break;
-      last_status_code =
-          talon_.GetAcceleration().SetUpdateFrequency(40_Hz, max_wait_time_);
+      last_status_code = talon_.GetAcceleration().SetUpdateFrequency(
+          1 / velocity_ms, max_wait_time_);
       break;
 
     default: break;

diff --git a/src/frc846/cpp/frc846/robot/swerve/drive_to_point_command.cc b/src/frc846/cpp/frc846/robot/swerve/drive_to_point_command.cc
@@ -45,17 +45,13 @@ void DriveToPointCommand::Execute() {
   auto dist_left =
       (target_.point - dt_readings.estimated_pose.position).magnitude();
 
-  if (dist_left <= stopping_distance) {
+  if (dist_left - drivetrain_->GetPreferenceValue_unit_type<units::inch_t>(
+                      "drive_to_subtract") <=
+      stopping_distance) {
     is_decelerating_ = true;
 
-    if (dt_readings.estimated_pose.velocity.magnitude() < 4_fps &&
-        target_.velocity > 1_fps) {
-      dt_target.accel_dir =
-          dt_readings.estimated_pose.velocity.angle(true) + 180_deg;
-    } else {
-      dt_target.accel_dir =
-          (start_point_ - dt_readings.estimated_pose.position).angle(true);
-    }
+    dt_target.accel_dir =
+        dt_readings.estimated_pose.velocity.angle(true) + 180_deg;
 
     double decel_scale_factor = 1.0;
     if (dist_left > 3_in) {

diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
@@ -34,7 +34,7 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
   RegisterPreference("lock_gains/deadband", 2_in);
   RegisterPreference("lock_adj_rate", 0.05_in);
 
-  RegisterPreference("drive_to_subtract", 5_in);
+  RegisterPreference("drive_to_subtract", 2_in);
 
   RegisterPreference("max_speed", 15_fps);
   RegisterPreference("max_omega", units::degrees_per_second_t{180});
@@ -212,9 +212,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,
   };

diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
@@ -141,7 +141,7 @@ DrivetrainConstructor::getDrivetrainConfigs() {
       frc846::robot::swerve::SwerveModuleCommonConfig{drive_params,
           steer_params, mmtype, steer_reduction, drive_reduction, ""};
   configs.module_unique_configs = {FR_config, FL_config, BL_config, BR_config};
-  //   std::cout
+
   configs.camera_x_offsets = {4.5_in, 4_in};
   configs.camera_y_offsets = {-6_in, -12.5_in};
   configs.cams = 2;