diff --git a/src/deploy/autos/paths/aside_home b/src/deploy/autos/paths/aside_home deleted file mode 100644 index f4dc4d5..0000000 --- a/src/deploy/autos/paths/aside_home +++ /dev/null @@ -1 +0,0 @@ -P,277.15,37.5,55,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_home_2 b/src/deploy/autos/paths/aside_home_2 deleted file mode 100644 index 2af41d9..0000000 --- a/src/deploy/autos/paths/aside_home_2 +++ /dev/null @@ -1,2 +0,0 @@ -P,385.15,-60.5,0,3 -P,354.15,-80.5,50,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_one b/src/deploy/autos/paths/aside_piece_one deleted file mode 100644 index e83b5e5..0000000 --- a/src/deploy/autos/paths/aside_piece_one +++ /dev/null @@ -1 +0,0 @@ -P,290.5,94,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_two b/src/deploy/autos/paths/aside_piece_two deleted file mode 100644 index b12764d..0000000 --- a/src/deploy/autos/paths/aside_piece_two +++ /dev/null @@ -1 +0,0 @@ -P,424,165,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/drive_auto b/src/deploy/autos/paths/drive_auto deleted file mode 100644 index 672ff0f..0000000 --- a/src/deploy/autos/paths/drive_auto +++ /dev/null @@ -1 +0,0 @@ -P,217.5,169,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_center_run b/src/deploy/autos/paths/fp_center_run deleted file mode 100644 index 1871b90..0000000 --- a/src/deploy/autos/paths/fp_center_run +++ /dev/null @@ -1 +0,0 @@ -P,214.5,112.0,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_home b/src/deploy/autos/paths/fp_home deleted file mode 100644 index 660d3f4..0000000 --- a/src/deploy/autos/paths/fp_home +++ /dev/null @@ -1 +0,0 @@ -P,217.5,49.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_home_2 b/src/deploy/autos/paths/fp_home_2 deleted file mode 100644 index 78a70dc..0000000 --- a/src/deploy/autos/paths/fp_home_2 +++ /dev/null @@ -1 +0,0 @@ -P,219.5,54,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_home_3 b/src/deploy/autos/paths/fp_home_3 deleted file mode 100644 index 1f4de86..0000000 --- a/src/deploy/autos/paths/fp_home_3 +++ /dev/null @@ -1 +0,0 @@ -P,214.5,58,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_one b/src/deploy/autos/paths/fp_piece_one deleted file mode 100644 index aaba24a..0000000 --- a/src/deploy/autos/paths/fp_piece_one +++ /dev/null @@ -1 +0,0 @@ -P,217.5,94,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_three b/src/deploy/autos/paths/fp_piece_three deleted file mode 100644 index 73a9e6d..0000000 --- a/src/deploy/autos/paths/fp_piece_three +++ /dev/null @@ -1 +0,0 @@ -P,270.5,92,20,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_two b/src/deploy/autos/paths/fp_piece_two deleted file mode 100644 index c0c5536..0000000 --- a/src/deploy/autos/paths/fp_piece_two +++ /dev/null @@ -1 +0,0 @@ -P,153.5,90,-20,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_exit b/src/deploy/autos/paths/sside_exit deleted file mode 100644 index 8db1591..0000000 --- a/src/deploy/autos/paths/sside_exit +++ /dev/null @@ -1,2 +0,0 @@ -P,150.5,80.5,0,0 -P,150.5,115.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_home b/src/deploy/autos/paths/sside_home deleted file mode 100644 index 0631a84..0000000 --- a/src/deploy/autos/paths/sside_home +++ /dev/null @@ -1 +0,0 @@ -P,205.15,36.75,-55,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_piece_one b/src/deploy/autos/paths/sside_piece_one deleted file mode 100644 index 4b23bfd..0000000 --- a/src/deploy/autos/paths/sside_piece_one +++ /dev/null @@ -1 +0,0 @@ -P,186.5,95.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/points.lst b/src/deploy/autos/points.lst index 8824d81..638c38d 100644 --- a/src/deploy/autos/points.lst +++ b/src/deploy/autos/points.lst @@ -1,6 +1 @@ -N,kOrigin,0,0,0,0 -N,kSpeaker,217.5,-4,0,0 -N,kAmp,0,0,90,0 -N,kPointBlank,217.5,49,0,0 -N,kRightSub,253.15,57.75,60,0 -N,kLeftSub,222.84,57.75,-60,0 \ No newline at end of file +N,kOrigin,0,0,0,0 \ No newline at end of file diff --git a/src/deploy/autos/scripts/amp_side_auto b/src/deploy/autos/scripts/amp_side_auto deleted file mode 100644 index 0779e23..0000000 --- a/src/deploy/autos/scripts/amp_side_auto +++ /dev/null @@ -1,9 +0,0 @@ -0,2 -F,kRightSub -ACT,prep_shoot -ACT,shoot -ACT,deploy_intake -PATH,aside_piece_one -ACT,a_prep_shoot -PATH,aside_home -ACT,shoot \ No newline at end of file diff --git a/src/deploy/autos/scripts/amp_side_nothing b/src/deploy/autos/scripts/amp_side_nothing deleted file mode 100644 index 9292a86..0000000 --- a/src/deploy/autos/scripts/amp_side_nothing +++ /dev/null @@ -1,4 +0,0 @@ -0,2 -F,kRightSub -ACT,prep_shoot -ACT,shoot \ No newline at end of file diff --git a/src/deploy/autos/scripts/drive_auto b/src/deploy/autos/scripts/drive_auto deleted file mode 100644 index 6f87d38..0000000 --- a/src/deploy/autos/scripts/drive_auto +++ /dev/null @@ -1,3 +0,0 @@ -0,0 -F,kPointBlank -PATH,drive_auto \ No newline at end of file diff --git a/src/deploy/autos/scripts/four_piece_auto b/src/deploy/autos/scripts/four_piece_auto deleted file mode 100644 index cd081c7..0000000 --- a/src/deploy/autos/scripts/four_piece_auto +++ /dev/null @@ -1,21 +0,0 @@ -0,2 -F,kPointBlank -ACT,auto_home -ACT,prep_shoot -ACT,shoot -ACT,deploy_intake -PATH,fp_piece_one -ACT,a_prep_shoot -PATH,fp_home -ACT,shoot -ACT,deploy_intake -PATH,fp_piece_two -ACT,a_prep_shoot -PATH,fp_home_2 -ACT,shoot -ACT,deploy_intake -PATH,fp_piece_three -ACT,a_prep_shoot -PATH,fp_home_3 -ACT,shoot -PATH,fp_center_run \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_auto b/src/deploy/autos/scripts/source_side_auto deleted file mode 100644 index 5faab59..0000000 --- a/src/deploy/autos/scripts/source_side_auto +++ /dev/null @@ -1,10 +0,0 @@ -0,2 -F,kLeftSub -ACT,auto_home -ACT,prep_shoot -ACT,shoot -ACT,deploy_intake -PATH,sside_piece_one -ACT,a_prep_shoot -PATH,sside_home -ACT,shoot \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_mobility b/src/deploy/autos/scripts/source_side_mobility deleted file mode 100644 index ed99f1d..0000000 --- a/src/deploy/autos/scripts/source_side_mobility +++ /dev/null @@ -1,5 +0,0 @@ -0,2 -F,kLeftSub -ACT,prep_shoot -ACT,shoot -PATH,sside_exit \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_nothing b/src/deploy/autos/scripts/source_side_nothing deleted file mode 100644 index f27a3f2..0000000 --- a/src/deploy/autos/scripts/source_side_nothing +++ /dev/null @@ -1,4 +0,0 @@ -0,2 -F,kLeftSub -ACT,prep_shoot -ACT,shoot \ No newline at end of file diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc index 0161209..0e865ae 100644 --- a/src/frc846/cpp/frc846/robot/GenericRobot.cc +++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc @@ -15,6 +15,7 @@ #include "frc2/command/ParallelDeadlineGroup.h" #include "frc2/command/WaitCommand.h" #include "frc846/base/Loggable.h" +#include "frc846/control/MotorMonkey.h" #include "frc846/robot/GenericCommand.h" #include "frc846/wpilib/NTAction.h" #include "frc846/wpilib/time.h" @@ -173,6 +174,9 @@ void GenericRobot::StartCompetition() { // Update subsystem hardware generic_robot_container_->UpdateHardware(); + // Tick Motor Monkey + frc846::control::MotorMonkey::Tick(); + // Update dashboards frc::SmartDashboard::UpdateValues(); frc::Shuffleboard::Update(); diff --git a/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc b/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc index 183cc02..d092d7e 100644 --- a/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc +++ b/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc @@ -22,7 +22,7 @@ SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate( kModuleLocationSigns[i].first * constants_.wheelbase_horizontal_dim, kModuleLocationSigns[i].second * constants_.wheelbase_forward_dim}; - units::degree_t rot_direction = location.angle(false); + units::degree_t rot_direction = location.angle(false) - 90_deg; frc846::math::VectorND rotation{ inputs.rotation_target * units::math::cos(rot_direction) * radius / @@ -30,7 +30,8 @@ SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate( inputs.rotation_target * units::math::sin(rot_direction) * radius / 1_rad}; - module_targets[i] = inputs.translation_target + rotation; + module_targets[i] = + inputs.translation_target.rotate(-inputs.bearing, true) + rotation; } units::feet_per_second_t max_mag = 0_fps; diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc index aa5587c..4d10a77 100644 --- a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc +++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc @@ -15,14 +15,16 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs) configs_.module_unique_configs[i], configs_.module_common_config}; } - RegisterPreference("steer_gains/kP", 0.3); - RegisterPreference("steer_gains/kI", 0.0); - RegisterPreference("steer_gains/kD", 0.0); - RegisterPreference("steer_gains/kF", 0.0); + RegisterPreference("steer_gains/_kP", 2.0); + RegisterPreference("steer_gains/_kI", 0.0); + RegisterPreference("steer_gains/_kD", 0.0); + RegisterPreference("steer_gains/_kF", 0.0); RegisterPreference("max_speed", 15_fps); RegisterPreference("max_omega", units::degrees_per_second_t{180}); + RegisterPreference("odom_fudge_factor", 0.875); + odometry_.setConstants({}); ol_calculator_.setConstants({ .wheelbase_horizontal_dim = configs.wheelbase_horizontal_dim, @@ -32,10 +34,10 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs) void DrivetrainSubsystem::Setup() { frc846::control::base::MotorGains steer_gains{ - GetPreferenceValue_double("steer_gains/kP"), - GetPreferenceValue_double("steer_gains/kI"), - GetPreferenceValue_double("steer_gains/kD"), - GetPreferenceValue_double("steer_gains/kF")}; + GetPreferenceValue_double("steer_gains/_kP"), + GetPreferenceValue_double("steer_gains/_kI"), + GetPreferenceValue_double("steer_gains/_kD"), + GetPreferenceValue_double("steer_gains/_kF")}; for (SwerveModuleSubsystem* module : modules_) { module->InitByParent(); module->Setup(); @@ -115,11 +117,13 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { Graph("readings/velocity_y", velocity[1]); frc846::robot::swerve::odometry::SwervePose new_pose{ - .position = - odometry_.calculate({bearing, steer_positions, drive_positions}) - .position, + .position = odometry_ + .calculate({bearing, steer_positions, drive_positions, + GetPreferenceValue_double("odom_fudge_factor")}) + .position, .bearing = bearing, - .velocity = velocity}; + .velocity = velocity, + }; // TODO: consider bearing simulation @@ -139,18 +143,18 @@ void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) { units::degree_t bearing = navX_.GetAngle() * 1_deg; auto ol_calc_outputs = ol_calculator_.calculate({ol_target->velocity, - ol_target->angular_velocity, + ol_target->angular_velocity, bearing, GetPreferenceValue_unit_type("max_speed")}); for (int i = 0; i < 4; i++) { - modules_[i]->SetSteerGains({GetPreferenceValue_double("steer_gains/kP"), - GetPreferenceValue_double("steer_gains/kI"), - GetPreferenceValue_double("steer_gains/kD"), - GetPreferenceValue_double("steer_gains/kF")}); + modules_[i]->SetSteerGains({GetPreferenceValue_double("steer_gains/_kP"), + GetPreferenceValue_double("steer_gains/_kI"), + GetPreferenceValue_double("steer_gains/_kD"), + GetPreferenceValue_double("steer_gains/_kF")}); SwerveModuleOLControlTarget module_target{ .drive = ol_calc_outputs.drive_outputs[i], - .steer = ol_calc_outputs.steer_outputs[i] - bearing}; + .steer = ol_calc_outputs.steer_outputs[i]}; modules_[i]->SetTarget(module_target); } } else if (DrivetrainAccelerationControlTarget* accel_target = diff --git a/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc index 0bb4074..59a19f1 100644 --- a/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc +++ b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc @@ -13,8 +13,10 @@ SwerveOdometryOutput SwerveOdometryCalculator::calculate( for (int i = 0; i < 4; i++) { displacement += - frc846::math::Vector2D{inputs.drive_pos[i], inputs.steer_pos[i], true}; + frc846::math::Vector2D{module_diffs[i], inputs.steer_pos[i], true} / + 4.0; } + displacement *= inputs.odom_ff_; displacement.rotate(inputs.bearing, true); last_position_ += displacement; diff --git a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc index 1844156..c026d9e 100644 --- a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc +++ b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc @@ -95,8 +95,11 @@ void SwerveModuleSubsystem::ZeroWithCANcoder() { auto position = cancoder_.GetAbsolutePosition(); if (position.IsAllGood()) { - steer_helper_.SetPosition(-position.GetValue()); - Log("Zeroed to {}!", -position.GetValue()); + units::degree_t position_zero = + -position.GetValue() + + GetPreferenceValue_unit_type("cancoder_offset_"); + steer_helper_.SetPosition(position_zero); + Log("Zeroed to {}!", position_zero); return; } @@ -130,13 +133,13 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) { Graph("target/ol_steer_target", ol_target->steer); auto [steer_dir, invert_drive] = - calculateSteerPosition(GetReadings().steer_pos, ol_target->steer); + calculateSteerPosition(ol_target->steer, GetReadings().steer_pos); Graph("target/steer_dir", steer_dir); Graph("target/invert_drive", invert_drive); units::dimensionless::scalar_t cosine_comp = - units::math::cos(steer_dir - GetReadings().steer_pos); + units::math::cos(ol_target->steer - GetReadings().steer_pos); Graph("target/cosine_comp", cosine_comp.to()); @@ -145,7 +148,7 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) { drive_helper_.WriteDC(cosine_comp * drive_duty_cycle); if (std::abs(drive_duty_cycle) > 0.002) { - steer_helper_.WritePositionOnController(ol_target->steer); + steer_helper_.WritePositionOnController(steer_dir); } } else if (SwerveModuleTorqueControlTarget* torque_target = std::get_if(&target)) { @@ -159,20 +162,20 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) { std::pair SwerveModuleSubsystem::calculateSteerPosition( units::degree_t target_norm, units::degree_t current) { - bool reverse = false; + bool invert = false; - units::degree_t diff = - frc846::math::CoterminalDifference(target_norm, current); + units::degree_t target = target_norm; - if (diff > 90_deg) { - diff -= 180_deg; - reverse = true; - } else if (diff < -90_deg) { - diff += 180_deg; - reverse = true; + while ((target - current) > 90_deg) { + target -= 180_deg; + invert = !invert; + } + while ((target - current) < -90_deg) { + target += 180_deg; + invert = !invert; } - return {current + diff, reverse}; + return {target, invert}; } void SwerveModuleSubsystem::SetSteerGains( diff --git a/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h b/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h index 1be0927..29c490a 100644 --- a/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h +++ b/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h @@ -20,6 +20,7 @@ struct SwerveOpenLoopCalculatorConstants { struct SwerveOpenLoopCalculatorInputs { frc846::math::VectorND translation_target; units::degrees_per_second_t rotation_target; + units::degree_t bearing; units::feet_per_second_t max_speed; }; diff --git a/src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h b/src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h index 52b36ed..0bf631c 100644 --- a/src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h +++ b/src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h @@ -16,6 +16,7 @@ struct SwerveOdometryInputs { units::degree_t bearing; std::array steer_pos; frc846::math::VectorND drive_pos; + double odom_ff_; }; struct SwerveOdometryOutput { diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc index 6e4b910..685fad2 100644 --- a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc +++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc @@ -82,17 +82,17 @@ DrivetrainConstructor::getDrivetrainConfigs() { num_connectors_BR)}; frc846::robot::swerve::SwerveModuleUniqueConfig FR_config{"FR", - ports::drivetrain_::kFRDrive_CANID, ports::drivetrain_::kFRSteer_CANID, - ports::drivetrain_::kFRCANCoder_CANID, wire_resistance_FR}; + ports::drivetrain_::kFRCANCoder_CANID, ports::drivetrain_::kFRDrive_CANID, + ports::drivetrain_::kFRSteer_CANID, wire_resistance_FR}; frc846::robot::swerve::SwerveModuleUniqueConfig FL_config{"FL", - ports::drivetrain_::kFLDrive_CANID, ports::drivetrain_::kFLSteer_CANID, - ports::drivetrain_::kFLCANCoder_CANID, wire_resistance_FL}; + ports::drivetrain_::kFLCANCoder_CANID, ports::drivetrain_::kFLDrive_CANID, + ports::drivetrain_::kFLSteer_CANID, wire_resistance_FL}; frc846::robot::swerve::SwerveModuleUniqueConfig BL_config{"BL", - ports::drivetrain_::kBLDrive_CANID, ports::drivetrain_::kBLSteer_CANID, - ports::drivetrain_::kBLCANCoder_CANID, wire_resistance_BL}; + ports::drivetrain_::kBLCANCoder_CANID, ports::drivetrain_::kBLDrive_CANID, + ports::drivetrain_::kBLSteer_CANID, wire_resistance_BL}; frc846::robot::swerve::SwerveModuleUniqueConfig BR_config{"BR", - ports::drivetrain_::kBRDrive_CANID, ports::drivetrain_::kBRSteer_CANID, - ports::drivetrain_::kBRCANCoder_CANID, wire_resistance_BR}; + ports::drivetrain_::kBRCANCoder_CANID, ports::drivetrain_::kBRDrive_CANID, + ports::drivetrain_::kBRSteer_CANID, wire_resistance_BR}; frc846::control::config::MotorConstructionParameters drive_params{ .can_id = 999, // overriden by unique config @@ -128,7 +128,7 @@ DrivetrainConstructor::getDrivetrainConfigs() { steer_params, frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60, steer_reduction, drive_reduction, ""}; - configs.module_unique_configs = {FR_config, FL_config, BR_config, BL_config}; + configs.module_unique_configs = {FR_config, FL_config, BL_config, BR_config}; return configs; } \ No newline at end of file