Skip to content

Commit

Permalink
Anti-skew, fix preference system
Browse files Browse the repository at this point in the history
  • Loading branch information
@VyaasBaskar
VyaasBaskar committed Jan 21, 2025
1 parent 03b5730 commit aa99cb8
Show file tree
Hide file tree
Showing 8 changed files with 263 additions and 14 deletions.
160 changes: 160 additions & 0 deletions networktables.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,5 +278,165 @@
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/drive_motor_current_limit (A)",
"type": "double",
"value": 120.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/steer_motor_current_limit (A)",
"type": "double",
"value": 120.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/drive_motor_smart_current_limit (A)",
"type": "double",
"value": 80.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/steer_motor_smart_current_limit (A)",
"type": "double",
"value": 80.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/drive_motor_voltage_compensation (V)",
"type": "double",
"value": 16.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/steer_motor_voltage_compensation (V)",
"type": "double",
"value": 12.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/FR/cancoder_offset_ (deg)",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/max_speed (fps)",
"type": "double",
"value": 15.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/max_omega (deg_per_s)",
"type": "double",
"value": 180.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/steer_lag (s)",
"type": "double",
"value": 0.05,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/voltage_min (V)",
"type": "double",
"value": 8.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/recal_voltage_thresh (V)",
"type": "double",
"value": 10.5,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/default_max_draw (A)",
"type": "double",
"value": 150.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/min_max_draw (A)",
"type": "double",
"value": 60.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/max_max_draw (A)",
"type": "double",
"value": 250.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/battery_cc (A)",
"type": "double",
"value": 700.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/brownout_perm_loops",
"type": "int",
"value": 500,
"properties": {
"persistent": true
}
}
]
88 changes: 88 additions & 0 deletions networktables.json.bck
View file
Original file line number Diff line number Diff line change
Expand Up @@ -262,5 +262,93 @@
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/min_max_draw (A)",
"type": "double",
"value": 40.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/max_max_draw (A)",
"type": "double",
"value": 300.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/drive_motor_current_limit (A)",
"type": "double",
"value": 120.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/steer_motor_current_limit (A)",
"type": "double",
"value": 120.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/drive_motor_smart_current_limit (A)",
"type": "double",
"value": 80.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/steer_motor_smart_current_limit (A)",
"type": "double",
"value": 80.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/drive_motor_voltage_compensation (V)",
"type": "double",
"value": 16.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/DrivetrainConstructor/steer_motor_voltage_compensation (V)",
"type": "double",
"value": 12.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/FR/cancoder_offset_ (deg)",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
}
]
1 change: 1 addition & 0 deletions src/frc846/cpp/frc846/base/fserver.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <string.h>

#include <algorithm>
#include <iostream>
#include <thread>

namespace frc846::base {
Expand Down
9 changes: 5 additions & 4 deletions src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
units::degrees_per_second_t yaw_rate = navX_.GetRate() * 1_deg_per_s;

Graph("readings/bearing", bearing);
Graph("readings/yaw_rate", yaw_rate);

frc846::math::VectorND<units::inch, 4> drive_positions{
0_in, 0_in, 0_in, 0_in};
Expand All @@ -123,9 +124,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,
};
Expand All @@ -147,7 +148,7 @@ void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {

units::degree_t bearing = GetReadings().pose.bearing;
units::degree_t steer_lag_compensation =
GetPreferenceValue_unit_type<units::second_t>("steer_lag") *
-GetPreferenceValue_unit_type<units::second_t>("steer_lag") *
GetReadings().yaw_rate;

Graph("target/steer_lag_compensation", steer_lag_compensation);
Expand Down
4 changes: 2 additions & 2 deletions src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,13 +107,13 @@ void SwerveModuleSubsystem::ZeroWithCANcoder() {
GetPreferenceValue_unit_type<units::degree_t>("cancoder_offset_");

if (position.IsAllGood()) {
steer_helper_.OffsetPositionTo(position_zero);
steer_helper_.SetPosition(position_zero);
Log("Zeroed to {}!", position_zero);
return;
} else if (attempts == kMaxAttempts) {
Error("Unable to zero normally after {} attempts - attempting anyways",
kMaxAttempts);
steer_helper_.OffsetPositionTo(position_zero);
steer_helper_.SetPosition(position_zero);
Warn("Unreliably zeroed to {}!", position_zero);
return;
}
Expand Down
6 changes: 3 additions & 3 deletions src/frc846/include/frc846/base/Loggable.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,9 +89,9 @@ class Loggable {

// Returns the value of the preference for a unit-type.
template <typename U> U GetPreferenceValue_unit_type(std::string key) {
std::string fullkey = fmt::format("{} ({})", name_ + "/" + key,
units::abbreviation(units::make_unit<U>(0)));
return units::make_unit<U>(GetPreferenceValue_double(fullkey));
std::string modkey = fmt::format(
"{} ({})", key, units::abbreviation(units::make_unit<U>(0)));
return units::make_unit<U>(GetPreferenceValue_double(modkey));
}

// Returns the value of the preference for a double.
Expand Down
4 changes: 4 additions & 0 deletions src/y2025/cpp/FunkyRobot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,10 @@ void FunkyRobot::InitTeleop() {
}

void FunkyRobot::OnPeriodic() {
// TODO: plug real heights into AntiTippingCalculator
AntiTippingCalculator::SetTelescopeHeight(36_in);
AntiTippingCalculator::SetElevatorHeight(45_in);

auto cg = AntiTippingCalculator::CalculateRobotCG();
Graph("robot_cg_x", cg[0]);
Graph("robot_cg_y", cg[1]);
Expand Down
5 changes: 0 additions & 5 deletions src/y2025/cpp/commands/teleop/drive_command.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,11 +49,6 @@ void DriveCommand::Periodic() {

target.velocity = {translate_x * max_speed, translate_y * max_speed};

// TODO: plug real heights into AntiTippingCalculator

AntiTippingCalculator::SetTelescopeHeight(36_in);
AntiTippingCalculator::SetElevatorHeight(45_in);

auto delta_dir = (frc846::math::VectorND<units::feet_per_second, 2>{
target.velocity[0], target.velocity[1]} -
container_.drivetrain_.GetReadings().pose.velocity);
Expand Down

0 comments on commit aa99cb8

Please sign in to comment.