Skip to content

Commit

Permalink
Merge pull request #28 from Team846/temp_cg_calcs_merge
Browse files Browse the repository at this point in the history 
Merge master -> cg_calcs
  • Loading branch information
@VyaasBaskar
VyaasBaskar authored Jan 16, 2025
2 parents bb17e13 + 40f081a commit 819f273
Show file tree
Hide file tree
Showing 18 changed files with 416 additions and 57 deletions.
1 change: 1 addition & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,7 @@ To undo the going back:

## CppCheck Warnings
```
src\frc846\cpp\frc846\control\calculators\CurrentTorqueCalculator.cc:49:30: warning: Variable 'duty_cycle_original' is assigned a value that is never used. [unreadVariable]
src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:160:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
src\frc846\cpp\frc846\math\collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction]
Expand Down
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,4 +204,4 @@ runCppcheck.onlyIf { !project.hasProperty('fromCI') }
if (!project.hasProperty('fromCI')) {
check.dependsOn runCppcheck
runCppcheck.dependsOn spotlessApply
}
}
32 changes: 32 additions & 0 deletions networktables.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -230,5 +230,37 @@
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/voltage_min (V)",
"type": "double",
"value": 7.5,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/recal_voltage_thresh (V)",
"type": "double",
"value": 9.5,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/default_max_draw (A)",
"type": "double",
"value": 150.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/battery_cc (A)",
"type": "double",
"value": 600.0,
"properties": {
"persistent": true
}
}
]
72 changes: 72 additions & 0 deletions networktables.json.bck
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,5 +190,77 @@
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/steer_gains/_kP",
"type": "double",
"value": 2.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/steer_gains/_kI",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/steer_gains/_kD",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/steer_gains/_kF",
"type": "double",
"value": 0.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/SwerveDrivetrain/odom_fudge_factor",
"type": "double",
"value": 0.875,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/voltage_min (V)",
"type": "double",
"value": 7.5,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/recal_voltage_thresh (V)",
"type": "double",
"value": 9.5,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/default_max_draw (A)",
"type": "double",
"value": 150.0,
"properties": {
"persistent": true
}
},
{
"name": "/Preferences/MotorMonkey/MotorMonkey/battery_cc (A)",
"type": "double",
"value": 600.0,
"properties": {
"persistent": true
}
}
]
5 changes: 5 additions & 0 deletions src/frc846/cpp/frc846/control/HigherMotorController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,11 @@ void HigherMotorController::WriteDC(double duty_cycle) {
if (soft_limits_.has_value()) {
duty_cycle = soft_limits_.value().LimitDC(duty_cycle, GetPosition());
}
duty_cycle =
frc846::control::calculators::CurrentTorqueCalculator::limit_current_draw(
duty_cycle, constr_params_.smart_current_limit, GetVelocity(), 12.0_V,
constr_params_.circuit_resistance,
frc846::control::base::MotorSpecificationPresets::get(mmtype_));
MotorMonkey::WriteDC(slot_id_, duty_cycle);
}

Expand Down
144 changes: 110 additions & 34 deletions src/frc846/cpp/frc846/control/MotorMonkey.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,6 @@
#include "frc846/control/hardware/simulation/SIMLEVEL.h"
#include "frc846/math/collection.h"

// TODO: Add dynamic can/power management

namespace frc846::control {

#define CHECK_SLOT_ID() \
Expand Down Expand Up @@ -74,15 +72,88 @@ frc846::wpilib::unit_ohm

units::volt_t MotorMonkey::battery_voltage{0_V};

units::volt_t MotorMonkey::last_disabled_voltage{0_V};

frc846::math::DoubleSyncBuffer MotorMonkey::sync_buffer{50U, 15};

units::ampere_t MotorMonkey::max_draw_{0.0_A};

std::queue<MotorMonkey::MotorMessage> MotorMonkey::control_messages{};

void MotorMonkey::Tick(units::ampere_t max_draw) {
void MotorMonkey::Setup() {
loggable_.RegisterPreference("voltage_min", 7.5_V);
loggable_.RegisterPreference("recal_voltage_thresh", 10.5_V);
loggable_.RegisterPreference("default_max_draw", 150.0_A);
loggable_.RegisterPreference("min_max_draw", 40_A);
loggable_.RegisterPreference("max_max_draw", 300_A);
loggable_.RegisterPreference("battery_cc", 400_A);

max_draw_ = loggable_.GetPreferenceValue_unit_type<units::ampere_t>(
"default_max_draw");
}

void MotorMonkey::RecalculateMaxDraw() {
if (!sync_buffer.IsValid()) return;

sync_buffer.Sync();
loggable_.Graph("sync_diff_", sync_buffer.GetSyncDiff());

auto sy_trough = sync_buffer.GetTrough();
units::ampere_t cc_current{sy_trough.first};

loggable_.Graph("cc_current", cc_current);

units::ampere_t current_draw =
loggable_.GetPreferenceValue_unit_type<units::ampere_t>("battery_cc") -
cc_current;
units::volt_t voltage{sy_trough.second};

units::volt_t voltage_min =
loggable_.GetPreferenceValue_unit_type<units::volt_t>("voltage_min");
units::volt_t recal_voltage_thresh =
loggable_.GetPreferenceValue_unit_type<units::volt_t>(
"recal_voltage_thresh");

if (voltage > recal_voltage_thresh) return;

units::ampere_t temp_max_draw_ = current_draw *
(last_disabled_voltage - voltage_min) /
(last_disabled_voltage - voltage);

if (temp_max_draw_ <
loggable_.GetPreferenceValue_unit_type<units::ampere_t>("min_max_draw")) {
return;
} else if (temp_max_draw_ >
loggable_.GetPreferenceValue_unit_type<units::ampere_t>(
"max_max_draw")) {
return;
} else {
max_draw_ = temp_max_draw_;
}
}

void MotorMonkey::Tick(bool disabled) {
battery_voltage = frc::RobotController::GetBatteryVoltage();
loggable_.Graph("battery_voltage", battery_voltage.to<double>());
loggable_.Graph("battery_voltage", battery_voltage);
loggable_.Graph("last_disabled_voltage", last_disabled_voltage);

if (disabled) {
last_disabled_voltage = battery_voltage;
return;
}

units::ampere_t total_pred_draw = WriteMessages(max_draw_);

loggable_.Graph("total_pred_draw", total_pred_draw);

WriteMessages(max_draw);
units::ampere_t cc_current =
loggable_.GetPreferenceValue_unit_type<units::ampere_t>("battery_cc") -
total_pred_draw;
sync_buffer.Add(cc_current.to<double>(), battery_voltage.to<double>());

// TODO: Improve battery voltage estimation for simulation
RecalculateMaxDraw();

loggable_.Graph("max_draw", max_draw_);

for (size_t i = 0; i < CONTROLLER_REGISTRY_SIZE; i++) {
if (controller_registry[i] != nullptr) {
Expand All @@ -97,8 +168,9 @@ void MotorMonkey::Tick(units::ampere_t max_draw) {
}
}

void MotorMonkey::WriteMessages(units::ampere_t max_draw) {
units::ampere_t MotorMonkey::WriteMessages(units::ampere_t max_draw) {
units::ampere_t total_current = 0.0_A;
units::ampere_t second_total_current = 0.0_A;
std::queue<MotorMessage> temp_messages{control_messages};

double scale_factor = 1.0;
Expand All @@ -112,44 +184,32 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) {
units::radians_per_second_t velocity =
controller_registry[msg.slot_id]->GetVelocity();

frc846::control::base::MotorGains gains = gains_registry[msg.slot_id];

double duty_cycle = 0.0;

switch (msg.type) {
case MotorMessage::Type::DC:
duty_cycle = std::get<double>(msg.value);
break;
case MotorMessage::Type::Position: {
duty_cycle =
gains.calculate((std::get<units::radian_t>(msg.value) -
controller_registry[msg.slot_id]->GetPosition())
.to<double>(),
0.0, velocity.to<double>(), 0.0);
duty_cycle = std::clamp(duty_cycle, -1.0, 1.0);
// Onboard control should only be used with low-current draw systems
duty_cycle = 0.0;
break;
}
case MotorMessage::Type::Velocity: {
duty_cycle = gains.calculate(
(std::get<units::radians_per_second_t>(msg.value) - velocity)
.to<double>(),
0.0, 0.0, 0.0);
duty_cycle +=
gains.kFF *
(std::get<units::radians_per_second_t>(msg.value) /
frc846::control::base::MotorSpecificationPresets::get(motor_type)
.free_speed)
.to<double>();
duty_cycle = std::clamp(duty_cycle, -1.0, 1.0);
// Onboard control should only be used with low-current draw systems
duty_cycle = 0.0;
break;
}
default:
throw std::runtime_error(
"Unsupported MotorMessage type in MotorMonkey WriteMessages");
}
total_current += frc846::control::calculators::CurrentTorqueCalculator::
predict_current_draw(duty_cycle, velocity, battery_voltage,
circuit_resistance_registry[msg.slot_id], motor_type);
units::ampere_t pred_draw =
frc846::control::calculators::CurrentTorqueCalculator::
predict_current_draw(duty_cycle, velocity, battery_voltage,
circuit_resistance_registry[msg.slot_id], motor_type);
second_total_current += units::math::max(0_A, pred_draw);
total_current += units::math::abs(pred_draw);
temp_messages.pop();
}

Expand All @@ -168,28 +228,43 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) {
const auto& msg = control_messages.front();
auto* controller = controller_registry[msg.slot_id];

units::radians_per_second_t velocity =
controller_registry[msg.slot_id]->GetVelocity();

frc846::control::base::MotorMonkeyType motor_type =
slot_id_to_type_[msg.slot_id];

size_t slot_id = msg.slot_id;

switch (msg.type) {
case MotorMessage::Type::DC: {
double duty_cycle_ = std::get<double>(msg.value) * scale_factor;
if (!controller->IsDuplicateControlMessage(duty_cycle_)) {
controller->WriteDC(duty_cycle_);
double scaled_duty_cycle =
frc846::control::calculators::CurrentTorqueCalculator::
scale_current_draw(scale_factor, std::get<double>(msg.value),
velocity, battery_voltage, motor_type);
if (!controller->IsDuplicateControlMessage(scaled_duty_cycle) ||
controller->GetLastErrorCode() !=
frc846::control::hardware::ControllerErrorCodes::kAllOK) {
controller->WriteDC(scaled_duty_cycle);
LOG_IF_ERROR("WriteDC");
}
break;
}
case MotorMessage::Type::Position: {
auto pos = std::get<units::radian_t>(msg.value);
if (!controller->IsDuplicateControlMessage(pos)) {
if (!controller->IsDuplicateControlMessage(pos) ||
controller->GetLastErrorCode() !=
frc846::control::hardware::ControllerErrorCodes::kAllOK) {
controller->WritePosition(pos);
LOG_IF_ERROR("WritePosition");
}
break;
}
case MotorMessage::Type::Velocity: {
auto vel = std::get<units::radians_per_second_t>(msg.value);
if (!controller->IsDuplicateControlMessage(vel)) {
if (!controller->IsDuplicateControlMessage(vel) ||
controller->GetLastErrorCode() !=
frc846::control::hardware::ControllerErrorCodes::kAllOK) {
controller->WriteVelocity(vel);
LOG_IF_ERROR("WriteVelocity");
}
Expand All @@ -199,6 +274,7 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) {

control_messages.pop();
}
return second_total_current;
}

bool MotorMonkey::VerifyConnected() {
Expand Down
Loading

0 comments on commit 819f273

Please sign in to comment.