Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Drivetrain ip testing #10

Merged
merged 4 commits into from
Jan 3, 2025
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion src/deploy/autos/paths/aside_home

This file was deleted.

2 changes: 0 additions & 2 deletions src/deploy/autos/paths/aside_home_2

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/aside_piece_one

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/aside_piece_two

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/drive_auto

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/fp_center_run

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/fp_home

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/fp_home_2

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/fp_home_3

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/fp_piece_one

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/fp_piece_three

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/fp_piece_two

This file was deleted.

2 changes: 0 additions & 2 deletions src/deploy/autos/paths/sside_exit

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/sside_home

This file was deleted.

1 change: 0 additions & 1 deletion src/deploy/autos/paths/sside_piece_one

This file was deleted.

7 changes: 1 addition & 6 deletions src/deploy/autos/points.lst
Original file line number Diff line number Diff line change
@@ -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
N,kOrigin,0,0,0,0
9 changes: 0 additions & 9 deletions src/deploy/autos/scripts/amp_side_auto

This file was deleted.

4 changes: 0 additions & 4 deletions src/deploy/autos/scripts/amp_side_nothing

This file was deleted.

3 changes: 0 additions & 3 deletions src/deploy/autos/scripts/drive_auto

This file was deleted.

21 changes: 0 additions & 21 deletions src/deploy/autos/scripts/four_piece_auto

This file was deleted.

10 changes: 0 additions & 10 deletions src/deploy/autos/scripts/source_side_auto

This file was deleted.

5 changes: 0 additions & 5 deletions src/deploy/autos/scripts/source_side_mobility

This file was deleted.

4 changes: 0 additions & 4 deletions src/deploy/autos/scripts/source_side_nothing

This file was deleted.

4 changes: 4 additions & 0 deletions src/frc846/cpp/frc846/robot/GenericRobot.cc
Original file line number Diff line number Diff line change
@@ -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();
Original file line number Diff line number Diff line change
@@ -22,15 +22,16 @@ 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<units::feet_per_second, 2> rotation{
inputs.rotation_target * units::math::cos(rot_direction) * radius /
1_rad,
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;
40 changes: 22 additions & 18 deletions src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
Original file line number Diff line number Diff line change
@@ -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<units::feet_per_second_t>("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 =
Original file line number Diff line number Diff line change
@@ -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;
33 changes: 18 additions & 15 deletions src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
Original file line number Diff line number Diff line change
@@ -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<units::degree_t>("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<double>());

@@ -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<SwerveModuleTorqueControlTarget>(&target)) {
@@ -159,20 +162,20 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) {

std::pair<units::degree_t, bool> 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(
Original file line number Diff line number Diff line change
@@ -20,6 +20,7 @@ struct SwerveOpenLoopCalculatorConstants {
struct SwerveOpenLoopCalculatorInputs {
frc846::math::VectorND<units::feet_per_second, 2> translation_target;
units::degrees_per_second_t rotation_target;
units::degree_t bearing;
units::feet_per_second_t max_speed;
};

Original file line number Diff line number Diff line change
@@ -16,6 +16,7 @@ struct SwerveOdometryInputs {
units::degree_t bearing;
std::array<units::degree_t, 4> steer_pos;
frc846::math::VectorND<units::inch, 4> drive_pos;
double odom_ff_;
};

struct SwerveOdometryOutput {
18 changes: 9 additions & 9 deletions src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
Original file line number Diff line number Diff line change
@@ -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;
}
Loading