Skip to content

Commit

Permalink
Code cleanup and reducing compile time
Browse files Browse the repository at this point in the history
  • Loading branch information
VyaasBaskar committed Jan 20, 2025
1 parent e907d35 commit 03b5730
Show file tree
Hide file tree
Showing 30 changed files with 247 additions and 150 deletions.
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,7 @@ To undo the going back:

## CppCheck Warnings
```
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\robot\swerve\drivetrain.cc:173: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]
src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
Expand Down
3 changes: 1 addition & 2 deletions src/frc846/cpp/frc846/base/FunkyLogSystem.cc
Original file line number Diff line number Diff line change
@@ -1,9 +1,8 @@
#include "frc846/base/FunkyLogSystem.h"

#include <fstream>
#include <functional>
#include <future>
#include <iostream>
#include <string>
#include <thread>

#include "frc846/base/compression.h"
Expand Down
8 changes: 3 additions & 5 deletions src/frc846/cpp/frc846/base/compression.cc
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,6 @@

#include <cctype>
#include <cstring>
#include <iostream>
#include <string>

namespace frc846::base {

Expand All @@ -13,11 +11,11 @@ std::pair<uint8_t, bool> Compression::char_conv(char x) {

x = std::tolower(x);

const std::string ENCODING =
const std::string_view ENCODING =
"\nabcdefghijklmnopqrstuvwxyz0123456789!@#$%^&*()[]<>|;:',./?~_- ";

size_t pos = ENCODING.find(x);
if (pos != std::string::npos) {
if (pos != std::string_view::npos) {
val = pos;
} else {
val = ENCODING.size() - 1;
Expand Down Expand Up @@ -47,7 +45,7 @@ std::vector<uint8_t> Compression::pack_bytes(std::vector<uint8_t> conv) {
return output;
}

std::vector<uint8_t> Compression::compress(const std::string& data) {
std::vector<uint8_t> Compression::compress(const std::string_view& data) {
std::vector<uint8_t> conv;
for (char c : data) {
auto [val, is_upper] = char_conv(c);
Expand Down
105 changes: 105 additions & 0 deletions src/frc846/cpp/frc846/base/fserver.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
#include "frc846/base/fserver.h"

#include <string.h>

#include <algorithm>
#include <thread>

namespace frc846::base {

LoggingServer::LoggingServer() : messages{}, clients{} {
#ifdef _WIN32
/*
Configuring Windows Socket API.
*/

WSADATA wsaData;
int result = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (result != 0) {
std::cerr << "WSAStartup failed: " << result << std::endl;
exit(EXIT_FAILURE);
}
#endif
}

LoggingServer::~LoggingServer() {
#ifdef _WIN32
WSACleanup();
#endif
}

void LoggingServer::Start(int port) {
len = sizeof(cliaddr);

sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) throw std::runtime_error("Socket creation failed");

memset(&servaddr, 0, sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = INADDR_ANY;
servaddr.sin_port = htons(port);

if (bind(sockfd, reinterpret_cast<const struct sockaddr *>(&servaddr),
sizeof(servaddr)) < 0)
throw std::runtime_error("Socket bind failed");

std::thread sender([&]() {
while (true) {
do {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
} while (messages.empty() || clients.empty());

msg_mtx.lock();
auto msg = messages.front();
messages.pop();
msg_mtx.unlock();

cli_mtx.lock();
for (const auto &client : clients) {
sendto(sockfd, reinterpret_cast<const char *>(msg.data()), msg.size(),
0, reinterpret_cast<const struct sockaddr *>(&(client.addr)),
sizeof(client.addr));
}
cli_mtx.unlock();
}
});

std::thread receiver([&]() {
char buffer[1024];
std::chrono::milliseconds lastPruneTime{getTime()};
for (std::chrono::milliseconds t{lastPruneTime};; t = getTime()) {
if (recvfrom(sockfd, buffer, sizeof(buffer), 0,
reinterpret_cast<struct sockaddr *>(&cliaddr), &len) > 0) {
cli_mtx.lock();
auto it = std::find_if(
clients.begin(), clients.end(), [&](const LoggingClient &client) {
return client.addr.sin_addr.s_addr == cliaddr.sin_addr.s_addr;
});

if (it != clients.end()) {
it->addr.sin_port = cliaddr.sin_port;
it->lastKeepAlive = getTime();
} else {
clients.push_back({cliaddr, getTime()});
}
cli_mtx.unlock();
}
if (t - lastPruneTime > std::chrono::milliseconds(500)) {
cli_mtx.lock();
for (size_t i = 0; i < clients.size(); i++) {
if (getTime() - clients[i].lastKeepAlive >
std::chrono::milliseconds(5000)) {
clients.erase(clients.begin() + i);
}
}
cli_mtx.unlock();
lastPruneTime = t;
}
}
});

sender.detach();
receiver.detach();
}

} // namespace frc846::base
6 changes: 6 additions & 0 deletions src/frc846/cpp/frc846/control/HigherMotorController.cc
Original file line number Diff line number Diff line change
Expand Up @@ -116,4 +116,10 @@ void HigherMotorController::EnableStatusFrames(
frc846::control::MotorMonkey::EnableStatusFrames(slot_id_, frames);
}

void HigherMotorController::OverrideStatusFramePeriod(
frc846::control::config::StatusFrame frame, units::millisecond_t period) {
frc846::control::MotorMonkey::OverrideStatusFramePeriod(
slot_id_, frame, period);
}

} // namespace frc846::control
19 changes: 15 additions & 4 deletions src/frc846/cpp/frc846/control/MotorMonkey.cc
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <rev/SparkFlex.h>
#include <rev/SparkMax.h>

#include <iostream>
#include <string>

#include "frc846/control/hardware/SparkMXFX_interm.h"
Expand Down Expand Up @@ -83,12 +84,12 @@ std::queue<MotorMonkey::MotorMessage> MotorMonkey::control_messages{};
int MotorMonkey::num_loops_last_brown = 4000;

void MotorMonkey::Setup() {
loggable_.RegisterPreference("voltage_min", 7.5_V);
loggable_.RegisterPreference("voltage_min", 8.0_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);
loggable_.RegisterPreference("min_max_draw", 60_A);
loggable_.RegisterPreference("max_max_draw", 250_A);
loggable_.RegisterPreference("battery_cc", 700_A);
loggable_.RegisterPreference("brownout_perm_loops", 500);

max_draw_ = loggable_.GetPreferenceValue_unit_type<units::ampere_t>(
Expand Down Expand Up @@ -378,6 +379,16 @@ void MotorMonkey::EnableStatusFrames(
LOG_IF_ERROR("EnableStatusFrames");
}

void MotorMonkey::OverrideStatusFramePeriod(size_t slot_id,
frc846::control::config::StatusFrame frame, units::millisecond_t period) {
CHECK_SLOT_ID();

SMART_RETRY(
controller_registry[slot_id]->OverrideStatusFramePeriod(frame, period),
"OverrideStatusFramePeriod");
LOG_IF_ERROR("OverrideStatusFramePeriod");
}

units::volt_t MotorMonkey::GetBatteryVoltage() { return battery_voltage; }

void MotorMonkey::SetLoad(size_t slot_id, units::newton_meter_t load) {
Expand Down
17 changes: 17 additions & 0 deletions src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,23 @@ void SparkMXFX_interm::EnableStatusFrames(
APPLY_CONFIG_NO_RESET();
}

void SparkMXFX_interm::OverrideStatusFramePeriod(
frc846::control::config::StatusFrame frame, units::millisecond_t period) {
if (frame == config::StatusFrame::kFaultFrame ||
frame == config::StatusFrame::kLeader) {
configs.signals.FaultsPeriodMs(period.to<int>());
configs.signals.FaultsAlwaysOn(true);
} else if (frame == config::StatusFrame::kVelocityFrame) {
configs.signals.PrimaryEncoderVelocityPeriodMs(period.to<int>());
} else if (frame == config::StatusFrame::kPositionFrame) {
configs.signals.PrimaryEncoderPositionPeriodMs(period.to<int>());
} else if (frame == config::StatusFrame::kSensorFrame) {
configs.signals.AnalogPositionPeriodMs(period.to<int>());
}

APPLY_CONFIG_NO_RESET();
}

bool SparkMXFX_interm::IsDuplicateControlMessage(double duty_cycle) {
if (double* dc = std::get_if<double>(&last_command_)) {
return *dc == duty_cycle;
Expand Down
20 changes: 18 additions & 2 deletions src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc
Original file line number Diff line number Diff line change
Expand Up @@ -94,15 +94,31 @@ void TalonFX_interm::EnableStatusFrames(
last_status_code = talon_.GetSupplyCurrent().SetUpdateFrequency(10_Hz);
} else if (frame == frc846::control::config::StatusFrame::kPositionFrame) {
last_status_code =
talon_.GetPosition().SetUpdateFrequency(200_Hz, max_wait_time_);
talon_.GetPosition().SetUpdateFrequency(50_Hz, max_wait_time_);
} else if (frame == frc846::control::config::StatusFrame::kVelocityFrame) {
last_status_code = talon_.GetVelocity().SetUpdateFrequency(200_Hz);
last_status_code = talon_.GetVelocity().SetUpdateFrequency(50_Hz);
}
last_error_ = getErrorCode(last_status_code);
if (last_error_ != ControllerErrorCodes::kAllOK) return;
}
}

void TalonFX_interm::OverrideStatusFramePeriod(
frc846::control::config::StatusFrame frame, units::millisecond_t period) {
ctre::phoenix::StatusCode last_status_code = ctre::phoenix::StatusCode::OK;
if (frame == frc846::control::config::StatusFrame::kCurrentFrame) {
last_status_code = talon_.GetSupplyCurrent().SetUpdateFrequency(
1 / period, max_wait_time_);
} else if (frame == frc846::control::config::StatusFrame::kPositionFrame) {
last_status_code =
talon_.GetPosition().SetUpdateFrequency(1 / period, max_wait_time_);
} else if (frame == frc846::control::config::StatusFrame::kVelocityFrame) {
last_status_code =
talon_.GetVelocity().SetUpdateFrequency(1 / period, max_wait_time_);
}
last_error_ = getErrorCode(last_status_code);
}

bool TalonFX_interm::IsDuplicateControlMessage(double duty_cycle) {
if (double* dc = std::get_if<double>(&last_command_)) {
return *dc == duty_cycle;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,6 @@

#include <units/math.h>

#include <iostream>

#include "frc846/control/calculators/CurrentTorqueCalculator.h"
#include "frc846/control/calculators/VelocityPositionEstimator.h"

Expand Down
1 change: 0 additions & 1 deletion src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
#include "frc846/math/DoubleSyncBuffer.h"

#include <iostream>
#include <stdexcept>

namespace frc846::math {
Expand Down
2 changes: 0 additions & 2 deletions src/frc846/cpp/frc846/math/RampRateLimiter.cc
Original file line number Diff line number Diff line change
@@ -1,7 +1,5 @@
#include "frc846/math/RampRateLimiter.h"

#include <iostream>

namespace frc846::math {

RampRateLimiter::RampRateLimiter() {
Expand Down
23 changes: 18 additions & 5 deletions src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#include "frc846/robot/swerve/drivetrain.h"

#include <thread>

#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
#include "frc846/robot/swerve/swerve_module.h"

Expand All @@ -9,13 +11,13 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
: GenericSubsystem{"SwerveDrivetrain"},
configs_{configs},
modules_{},
navX_{configs.navX_connection_mode} {
navX_{configs.navX_connection_mode, studica::AHRS::k200Hz} {
for (int i = 0; i < 4; i++) {
modules_[i] = new SwerveModuleSubsystem{*this,
configs_.module_unique_configs[i], configs_.module_common_config};
}

RegisterPreference("steer_gains/_kP", 2.0);
RegisterPreference("steer_gains/_kP", 4.0);
RegisterPreference("steer_gains/_kI", 0.0);
RegisterPreference("steer_gains/_kD", 0.0);
RegisterPreference("steer_gains/_kF", 0.0);
Expand All @@ -25,6 +27,8 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)

RegisterPreference("odom_fudge_factor", 0.875);

RegisterPreference("steer_lag", 0.05_s);

odometry_.setConstants({});
ol_calculator_.setConstants({
.wheelbase_horizontal_dim = configs.wheelbase_horizontal_dim,
Expand Down Expand Up @@ -91,6 +95,7 @@ void DrivetrainSubsystem::SetCANCoderOffsets() {

DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
units::degree_t bearing = navX_.GetAngle() * 1_deg;
units::degrees_per_second_t yaw_rate = navX_.GetRate() * 1_deg_per_s;

Graph("readings/bearing", bearing);

Expand Down Expand Up @@ -130,7 +135,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
Graph("readings/position_x", new_pose.position[0]);
Graph("readings/position_y", new_pose.position[1]);

return {new_pose};
return {new_pose, yaw_rate};
}

void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
Expand All @@ -140,9 +145,17 @@ void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
Graph("target/ol_velocity_y", ol_target->velocity[1]);
Graph("target/ol_angular_velocity", ol_target->angular_velocity);

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

Graph("target/steer_lag_compensation", steer_lag_compensation);

auto velocity_compensated =
ol_target->velocity.rotate(steer_lag_compensation, true);

auto ol_calc_outputs = ol_calculator_.calculate({ol_target->velocity,
auto ol_calc_outputs = ol_calculator_.calculate({velocity_compensated,
ol_target->angular_velocity, bearing,
GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed")});

Expand Down
Loading

0 comments on commit 03b5730

Please sign in to comment.