From 62ff2212c29186c09a3081082aa2948bd1e0b1c8 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Sun, 31 Dec 2023 06:08:54 +0000 Subject: [PATCH] [wombat] Refactor to use namespaces (#34) * [wombat] Refactor to use namespaces * apply changes from review --- src/main/cpp/Main.cpp | 2 +- vendordeps/Phoenix.json | 423 ++++++++++++++++++ vendordeps/REVLib.json | 73 +++ wombat/src/main/cpp/drivetrain/Drivetrain.cpp | 19 +- wombat/src/main/cpp/subsystems/Arm.cpp | 36 +- wombat/src/main/cpp/subsystems/Elevator.cpp | 35 +- wombat/src/main/cpp/subsystems/Shooter.cpp | 30 +- wombat/src/main/cpp/utils/Encoder.cpp | 68 ++- wombat/src/main/cpp/utils/RobotStartup.cpp | 4 +- wombat/src/main/cpp/utils/Util.cpp | 10 +- .../src/main/cpp/utils/VoltageController.cpp | 16 +- wombat/src/main/include/Wombat.h | 26 ++ .../src/main/include/drivetrain/Drivetrain.h | 16 +- wombat/src/main/include/subsystems/Arm.h | 20 +- wombat/src/main/include/subsystems/Elevator.h | 24 +- wombat/src/main/include/subsystems/Shooter.h | 8 +- wombat/src/main/include/utils/Encoder.h | 16 +- wombat/src/main/include/utils/Gearbox.h | 6 +- wombat/src/main/include/utils/PID.h | 8 +- wombat/src/main/include/utils/RobotStartup.h | 11 +- wombat/src/main/include/utils/Util.h | 10 +- .../main/include/utils/VoltageController.h | 2 + 22 files changed, 691 insertions(+), 172 deletions(-) create mode 100644 vendordeps/Phoenix.json create mode 100644 vendordeps/REVLib.json create mode 100644 wombat/src/main/include/Wombat.h diff --git a/src/main/cpp/Main.cpp b/src/main/cpp/Main.cpp index c6df2c7f..33e1ec61 100644 --- a/src/main/cpp/Main.cpp +++ b/src/main/cpp/Main.cpp @@ -3,8 +3,8 @@ Go to Robot.cpp */ +#include "Wombat.h" #include "Robot.h" -#include "utils/RobotStartup.h" WOMBAT_ROBOT_MAIN(Robot); diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 00000000..4bd44248 --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,423 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.30.2", + "frcYear": 2023, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.30.2" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.30.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.30.2", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.30.2", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.2", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.30.2", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.30.2", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.2", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.1", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 00000000..989b8e64 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2023.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2023.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2023.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp index df7baa6e..40bfb694 100644 --- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp +++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp @@ -1,29 +1,28 @@ #include "drivetrain/Drivetrain.h" -using namespace wom; using namespace frc; using namespace units; -Drivetrain::Drivetrain(DrivetrainConfig *config, XboxController &driver): _config(config), _driver(driver) {} -Drivetrain::~Drivetrain() {} +wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig *config, XboxController &driver): _config(config), _driver(driver) {} +wom::drivetrain::Drivetrain::~Drivetrain() {} -DrivetrainConfig *Drivetrain::GetConfig() { return _config; } -DrivetrainState Drivetrain::GetState() { return _state; } +wom::drivetrain::DrivetrainConfig *wom::drivetrain::Drivetrain::GetConfig() { return _config; } +wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() { return _state; } -void Drivetrain::SetState(DrivetrainState state) { _state = state; } +void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) { _state = state; } -void Drivetrain::OnStart() { +void wom::drivetrain::Drivetrain::OnStart() { std::cout << "Starting Tank" << std::endl; } -void Drivetrain::OnUpdate(second_t dt) { +void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) { switch(_state) { case DrivetrainState::kIdle: break; case DrivetrainState::kTank: { - double rightSpeed = deadzone(_driver.GetRightY()); - double leftSpeed = deadzone(_driver.GetLeftY()); + double rightSpeed = wom::utils::deadzone(_driver.GetRightY()); + double leftSpeed = wom::utils::deadzone(_driver.GetLeftY()); _config->left1.transmission->SetVoltage(leftSpeed * maxVolts); _config->left2.transmission->SetVoltage(leftSpeed * maxVolts); _config->left3.transmission->SetVoltage(leftSpeed * maxVolts); diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp index 82fb2113..896baa98 100644 --- a/wombat/src/main/cpp/subsystems/Arm.cpp +++ b/wombat/src/main/cpp/subsystems/Arm.cpp @@ -6,7 +6,7 @@ using namespace frc; using namespace wom; //creates network table instatnce on shuffleboard -void ArmConfig::WriteNT(std::shared_ptr table) { +void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr table) { table->GetEntry("armMass").SetDouble(armMass.value()); table->GetEntry("loadMass").SetDouble(loadMass.value()); table->GetEntry("armLength").SetDouble(armLength.value()); @@ -17,7 +17,7 @@ void ArmConfig::WriteNT(std::shared_ptr table) { } //arm config is used -Arm::Arm(ArmConfig config) +wom::subsystems::Arm::Arm(ArmConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig), _velocityPID(config.path + "/velocityPID", config.velocityConfig), @@ -26,7 +26,7 @@ Arm::Arm(ArmConfig config) } //the loop that allows the information to be used -void Arm::OnUpdate(units::second_t dt) { +void wom::subsystems::Arm::OnUpdate(units::second_t dt) { //sets the voltage and gets the current angle units::volt_t voltage = 0_V; auto angle = GetAngle(); @@ -60,14 +60,6 @@ void Arm::OnUpdate(units::second_t dt) { break; } - // if ( - // (((_config.minAngle + _config.angleOffset) < 75_deg && units::math::abs(_pid.GetSetpoint() - _config.minAngle) <= 1_deg) - // || ((_config.maxAngle + _config.angleOffset) > 105_deg && units::math::abs(_pid.GetSetpoint() - _config.maxAngle) <= 1_deg)) && - // units::math::abs(_pid.GetError()) <= 1_deg - // ) { - // voltage = 0_V; - // } - voltage *= armLimit; // units::newton_meter_t torqueLimit = 10_kg * 1.4_m * 6_mps_sq; @@ -89,48 +81,48 @@ void Arm::OnUpdate(units::second_t dt) { _config.WriteNT(_table->GetSubTable("config")); } -void Arm::SetArmSpeedLimit(double limit) { +void wom::subsystems::Arm::SetArmSpeedLimit(double limit) { armLimit = limit; } //defines information needed for the functions and connects the states to their respective function -void Arm::SetIdle() { +void wom::subsystems::Arm::SetIdle() { _state = ArmState::kIdle; } -void Arm::SetRaw(units::volt_t voltage) { +void wom::subsystems::Arm::SetRaw(units::volt_t voltage) { _state = ArmState::kRaw; _voltage = voltage; } -void Arm::SetAngle(units::radian_t angle) { +void wom::subsystems::Arm::SetAngle(units::radian_t angle) { _state = ArmState::kAngle; _pid.SetSetpoint(angle); } -void Arm::SetVelocity(units::radians_per_second_t velocity) { +void wom::subsystems::Arm::SetVelocity(units::radians_per_second_t velocity) { _state = ArmState::kVelocity; _velocityPID.SetSetpoint(velocity); } -ArmConfig &Arm::GetConfig() { +wom::subsystems::ArmConfig &wom::subsystems::Arm::GetConfig() { return _config; } -units::radian_t Arm::GetAngle() const { +units::radian_t wom::subsystems::Arm::GetAngle() const { return _config.armEncoder.GetPosition() / 100 * 360 * 1_deg; } -units::radians_per_second_t Arm::MaxSpeed() const { +units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const { return _config.leftGearbox.motor.Speed(0_Nm, 12_V); } -units::radians_per_second_t Arm::GetArmVelocity() const { +units::radians_per_second_t wom::subsystems::Arm::GetArmVelocity() const { return _config.armEncoder.GetVelocity() / 100 * 360 * 1_deg / 60_s; } -bool Arm::IsStable() const { +bool wom::subsystems::Arm::IsStable() const { return _pid.IsStable(5_deg); } @@ -138,7 +130,7 @@ bool Arm::IsStable() const { /* SIMULATION */ // #include -// ::wom::sim::ArmSim::ArmSim(ArmConfig config) +// ::wom::sim::ArmSim::ArmSim(ArmConfig config) // : config(config), // angle(config.initialAngle), // encoder(config.gearbox.encoder->MakeSimEncoder()), diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp index 74406b00..2da63b0c 100644 --- a/wombat/src/main/cpp/subsystems/Elevator.cpp +++ b/wombat/src/main/cpp/subsystems/Elevator.cpp @@ -1,26 +1,23 @@ #include "subsystems/Elevator.h" #include -#include #include -using namespace wom; - -void ElevatorConfig::WriteNT(std::shared_ptr table) { +void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr table) { table->GetEntry("radius").SetDouble(radius.value()); table->GetEntry("mass").SetDouble(mass.value()); table->GetEntry("maxHeight").SetDouble(maxHeight.value()); } -Elevator::Elevator(ElevatorConfig config) +wom::subsystems::Elevator::Elevator(ElevatorConfig config) : _config(config), _state(ElevatorState::kIdle), _pid{config.path + "/pid", config.pid}, _velocityPID{config.path + "/velocityPID", config.velocityPID}, _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) { - // _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad); + _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad); } -void Elevator::OnUpdate(units::second_t dt) { +void wom::subsystems::Elevator::OnUpdate(units::second_t dt) { units::volt_t voltage{0}; units::meter_t height = GetElevatorEncoderPos() * 1_m; @@ -83,55 +80,55 @@ void Elevator::OnUpdate(units::second_t dt) { _config.rightGearbox.transmission->SetVoltage(voltage); } -void Elevator::SetManual(units::volt_t voltage) { +void wom::subsystems::Elevator::SetManual(units::volt_t voltage) { _state = ElevatorState::kManual; _setpointManual = voltage; } -void Elevator::SetPID(units::meter_t height) { +void wom::subsystems::Elevator::SetPID(units::meter_t height) { _state = ElevatorState::kPID; _pid.SetSetpoint(height); } -void Elevator::SetElevatorSpeedLimit(double limit) { +void wom::subsystems::Elevator::SetElevatorSpeedLimit(double limit) { speedLimit = limit; } -void Elevator::SetVelocity(units::meters_per_second_t velocity) { +void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) { _velocityPID.SetSetpoint(velocity); _state = ElevatorState::kVelocity; } -void Elevator::SetIdle() { +void wom::subsystems::Elevator::SetIdle() { _state = ElevatorState::kIdle; } -ElevatorConfig &Elevator::GetConfig() { +wom::subsystems::ElevatorConfig &wom::subsystems::Elevator::GetConfig() { return _config; } -bool Elevator::IsStable() const { +bool wom::subsystems::Elevator::IsStable() const { return _pid.IsStable(); } -ElevatorState Elevator::GetState() const { +wom::subsystems::ElevatorState wom::subsystems::Elevator::GetState() const { return _state; } -double Elevator::GetElevatorEncoderPos() { +double wom::subsystems::Elevator::GetElevatorEncoderPos() { return _config.elevatorEncoder.GetPosition() * 14/60 * 2 * 3.1415 * 0.02225; } -units::meter_t Elevator::GetHeight() const { +units::meter_t wom::subsystems::Elevator::GetHeight() const { // std::cout << "elevator position"<< _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; // return _config.rightGearbox.encoder->GetEncoderDistance() * 1_m; return _config.elevatorEncoder.GetPosition() * 14/60 * 2 * 3.1415 * 0.02225 * 1_m; } -units::meters_per_second_t Elevator::GetElevatorVelocity() const { +units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const { return _config.elevatorEncoder.GetVelocity() / 60_s * 14/60 * 2 * 3.1415 * 0.02225 * 1_m; } -units::meters_per_second_t Elevator::MaxSpeed() const { +units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const { return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad * _config.radius; } diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp index 2e04d917..e829cc9a 100644 --- a/wombat/src/main/cpp/subsystems/Shooter.cpp +++ b/wombat/src/main/cpp/subsystems/Shooter.cpp @@ -2,14 +2,12 @@ #include -using namespace wom; - -Shooter::Shooter(std::string path, ShooterParams params) - : _params(params), _state(ShooterState::kIdle), - _pid{path + "/pid", params.pid}, +wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params) + : _params(params), _state(ShooterState::kIdle), + _pid{path + "/pid", params.pid}, _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {} -void Shooter::OnUpdate(units::second_t dt) { +void wom::subsystems::Shooter::OnUpdate(units::second_t dt) { units::volt_t voltage{0}; units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity(); @@ -41,44 +39,44 @@ void Shooter::OnUpdate(units::second_t dt) { _table->GetEntry("stable").SetBoolean(_pid.IsStable()); } -void Shooter::SetManual(units::volt_t voltage) { +void wom::subsystems::Shooter::SetManual(units::volt_t voltage) { _state = ShooterState::kManual; _setpointManual = voltage; } -void Shooter::SetPID(units::radians_per_second_t goal) { +void wom::subsystems::Shooter::SetPID(units::radians_per_second_t goal) { _state = ShooterState::kPID; _pid.SetSetpoint(goal); } -void Shooter::SetIdle() { +void wom::subsystems::Shooter::SetIdle() { _state = ShooterState::kIdle; } -bool Shooter::IsStable() const { +bool wom::subsystems::Shooter::IsStable() const { return _pid.IsStable(); } -//Shooter Manual Set +//Shooter Manual Set -ShooterConstant::ShooterConstant(Shooter *s, units::volt_t setpoint) +wom::subsystems::ShooterConstant::ShooterConstant(Shooter *s, units::volt_t setpoint) : _shooter(s), _setpoint(setpoint) { Controls(_shooter); } - -void ShooterConstant::OnTick(units::second_t dt) { + +void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) { _shooter->SetManual(_setpoint); } // ShooterSpinup -ShooterSpinup::ShooterSpinup(Shooter *s, units::radians_per_second_t speed, bool hold) +wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter *s, units::radians_per_second_t speed, bool hold) : _shooter(s), _speed(speed), _hold(hold) { Controls(_shooter); } -void ShooterSpinup::OnTick(units::second_t dt) { +void wom::subsystems::ShooterSpinup::OnTick(units::second_t dt) { _shooter->SetPID(_speed); if (!_hold && _shooter->IsStable()) diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp index b40b6c5b..63711841 100644 --- a/wombat/src/main/cpp/utils/Encoder.cpp +++ b/wombat/src/main/cpp/utils/Encoder.cpp @@ -1,38 +1,35 @@ #include "utils/Encoder.h" -#include -using namespace wom; - -double Encoder::GetEncoderTicks() const { +double wom::utils::Encoder::GetEncoderTicks() const { return GetEncoderRawTicks(); } -double Encoder::GetEncoderTicksPerRotation() const { +double wom::utils::Encoder::GetEncoderTicksPerRotation() const { return _encoderTicksPerRotation * _reduction; } -void Encoder::ZeroEncoder() { +void wom::utils::Encoder::ZeroEncoder() { _offset = GetEncoderRawTicks() * 1_rad; } -void Encoder::SetEncoderPosition(units::degree_t position) { +void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) { // units::radian_t offset_turns = position - GetEncoderPosition(); units::degree_t offset = position - (GetEncoderRawTicks() * 360 * 1_deg); _offset = offset; // _offset = -offset_turns; } -void Encoder::SetEncoderOffset(units::radian_t offset) { +void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { _offset = offset; // units::turn_t offset_turns = offset; // _offset = offset_turns.value() * GetEncoderTicksPerRotation(); } -void Encoder::SetReduction(double reduction) { +void wom::utils::Encoder::SetReduction(double reduction) { _reduction = reduction; } -units::radian_t Encoder::GetEncoderPosition() { +units::radian_t wom::utils::Encoder::GetEncoderPosition() { if (_type == 0) { units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()}; return n_turns; @@ -45,88 +42,85 @@ units::radian_t Encoder::GetEncoderPosition() { } } -double Encoder::GetEncoderDistance() { +double wom::utils::Encoder::GetEncoderDistance() { return (GetEncoderTicks() /*- _offset.value()*/) * 0.02032; // return (GetEncoderTicks() - _offset.value()) * 2 * 3.141592 * 0.0444754; // return (GetEncoderTicks() - _offset.value()); } -units::radians_per_second_t Encoder::GetEncoderAngularVelocity() { +units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() { // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2 * 3.1415926; units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()}; return n_turns_per_s; } -double DigitalEncoder::GetEncoderRawTicks() const { - // Encoder.encoderType = 0; +double wom::utils::DigitalEncoder::GetEncoderRawTicks() const { return _nativeEncoder.Get(); } -double DigitalEncoder::GetEncoderTickVelocity() const { +double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const { return _nativeEncoder.GetRate(); } -CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax *controller, double reduction) - : Encoder(42, reduction, 2), _encoder(controller->GetEncoder()) {} +wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax *controller, double reduction) + : wom::utils::Encoder(42, reduction, 2), _encoder(controller->GetEncoder()) {} -double CANSparkMaxEncoder::GetEncoderRawTicks() const { +double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const { return _encoder.GetPosition() * _reduction; } -double CANSparkMaxEncoder::GetEncoderTickVelocity() const { +double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const { return _encoder.GetVelocity() * GetEncoderTicksPerRotation() / 60; } -TalonFXEncoder::TalonFXEncoder(ctre::phoenix::motorcontrol::can::TalonFX *controller, double reduction) - : Encoder(2048, reduction, 0), _controller(controller) { +wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix::motorcontrol::can::TalonFX *controller, double reduction) + : wom::utils::Encoder(2048, reduction, 0), _controller(controller) { controller->ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::TalonFXFeedbackDevice::IntegratedSensor); } -double TalonFXEncoder::GetEncoderRawTicks() const { +double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const { return _controller->GetSelectedSensorPosition(); } -double TalonFXEncoder::GetEncoderTickVelocity() const { +double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const { return _controller->GetSelectedSensorVelocity() * 10; } -TalonSRXEncoder::TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX *controller, double ticksPerRotation, double reduction) - : Encoder(ticksPerRotation, reduction, 0), _controller(controller) { +wom::utils::TalonSRXEncoder::TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX *controller, double ticksPerRotation, double reduction) + : wom::utils::Encoder(ticksPerRotation, reduction, 0), _controller(controller) { controller->ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::TalonSRXFeedbackDevice::QuadEncoder); } -double TalonSRXEncoder::GetEncoderRawTicks() const { +double wom::utils::TalonSRXEncoder::GetEncoderRawTicks() const { return _controller->GetSelectedSensorPosition(); } -double TalonSRXEncoder::GetEncoderTickVelocity() const { +double wom::utils::TalonSRXEncoder::GetEncoderTickVelocity() const { return _controller->GetSelectedSensorVelocity() * 10; } +wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, double ticksPerRotation, double reduction) + : wom::utils::Encoder(ticksPerRotation, reduction, 0), _dutyCycleEncoder(channel) {} - -DutyCycleEncoder::DutyCycleEncoder(int channel, double ticksPerRotation, double reduction) - : Encoder(ticksPerRotation, reduction, 0), _dutyCycleEncoder(channel) {} - -double DutyCycleEncoder::GetEncoderRawTicks() const { +double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const { return _dutyCycleEncoder.Get().value(); } -double DutyCycleEncoder::GetEncoderTickVelocity() const { +double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const { return 0; } -CanEncoder::CanEncoder(int deviceNumber, double ticksPerRotation, double reduction) - : Encoder(ticksPerRotation, reduction, 1) { +wom::utils::CanEncoder::CanEncoder(int deviceNumber, double ticksPerRotation, double reduction) + : wom::utils::Encoder(ticksPerRotation, reduction, 1) { _canEncoder = new CANCoder(deviceNumber, "Drivebase"); } -double CanEncoder::GetEncoderRawTicks() const { +double wom::utils::CanEncoder::GetEncoderRawTicks() const { return _canEncoder->GetAbsolutePosition(); } -double CanEncoder::GetEncoderTickVelocity() const { +double wom::utils::CanEncoder::GetEncoderTickVelocity() const { return _canEncoder->GetVelocity(); } diff --git a/wombat/src/main/cpp/utils/RobotStartup.cpp b/wombat/src/main/cpp/utils/RobotStartup.cpp index e58e3bff..e8253f47 100644 --- a/wombat/src/main/cpp/utils/RobotStartup.cpp +++ b/wombat/src/main/cpp/utils/RobotStartup.cpp @@ -1,5 +1,5 @@ #include "utils/RobotStartup.h" -void wom::RobotStartup::Start(std::function robotFunc) { +void wom::utils::RobotStartup::Start(std::function robotFunc) { robotFunc(); -} \ No newline at end of file +} diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp index b59563c1..d69cec7b 100644 --- a/wombat/src/main/cpp/utils/Util.cpp +++ b/wombat/src/main/cpp/utils/Util.cpp @@ -1,17 +1,17 @@ #include "utils/Util.h" -units::second_t wom::now() { +units::second_t wom::utils::now() { uint64_t now = frc::RobotController::GetFPGATime(); return static_cast(now) / 1000000 * 1_s; } -void wom::WritePose2NT(std::shared_ptr table, frc::Pose2d pose) { +void wom::utils::WritePose2NT(std::shared_ptr table, frc::Pose2d pose) { table->GetEntry("x").SetDouble(pose.X().value()); table->GetEntry("y").SetDouble(pose.Y().value()); table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value()); } -void wom::WritePose3NT(std::shared_ptr table, frc::Pose3d pose) { +void wom::utils::WritePose3NT(std::shared_ptr table, frc::Pose3d pose) { table->GetEntry("x").SetDouble(pose.X().value()); table->GetEntry("y").SetDouble(pose.Y().value()); table->GetEntry("z").SetDouble(pose.Z().value()); @@ -19,11 +19,11 @@ void wom::WritePose3NT(std::shared_ptr table, frc::Pose3d pose table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert().value()); } -double wom::deadzone(double val, double deadzone) { +double wom::utils::deadzone(double val, double deadzone) { return std::fabs(val) > deadzone ? val : 0; } -double wom::spow2(double val) { +double wom::utils::spow2(double val) { return val*val*(val > 0 ? 1 : -1); } diff --git a/wombat/src/main/cpp/utils/VoltageController.cpp b/wombat/src/main/cpp/utils/VoltageController.cpp index b71af805..710971ed 100644 --- a/wombat/src/main/cpp/utils/VoltageController.cpp +++ b/wombat/src/main/cpp/utils/VoltageController.cpp @@ -3,32 +3,30 @@ #include -using namespace wom; - -units::volt_t VoltageController::GetEstimatedRealVoltage() const { +units::volt_t wom::utils::VoltageController::GetEstimatedRealVoltage() const { units::volt_t vb = frc::RobotController::GetBatteryVoltage(); return units::math::min(units::math::max(-vb, GetVoltage()), vb); } -VoltageController::VoltageController(frc::MotorController *MotorController) : _MotorController(MotorController) +wom::utils::VoltageController::VoltageController(frc::MotorController *MotorController) : _MotorController(MotorController) {} -void VoltageController::SetVoltage(units::volt_t voltage) { +void wom::utils::VoltageController::SetVoltage(units::volt_t voltage) { _MotorController->Set(voltage / GetBusVoltage()); } -units::volt_t VoltageController::GetVoltage() const { +units::volt_t wom::utils::VoltageController::GetVoltage() const { return _MotorController->Get() * GetBusVoltage(); } -units::volt_t VoltageController::GetBusVoltage() const { +units::volt_t wom::utils::VoltageController::GetBusVoltage() const { return frc::RobotController::GetInputVoltage() * 1_V; } -void VoltageController::SetInverted(bool invert) { +void wom::utils::VoltageController::SetInverted(bool invert) { _MotorController->SetInverted(invert); } -bool VoltageController::GetInverted() const { +bool wom::utils::VoltageController::GetInverted() const { return frc::RobotController::GetInputVoltage(); } diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h new file mode 100644 index 00000000..563b784b --- /dev/null +++ b/wombat/src/main/include/Wombat.h @@ -0,0 +1,26 @@ +#pragma once + +#include "behaviour/Behaviour.h" +#include "behaviour/HasBehaviour.h" +#include "behaviour/BehaviourScheduler.h" + +#include "utils/PID.h" +#include "utils/Util.h" +#include "utils/Encoder.h" +#include "utils/Gearbox.h" +#include "utils/RobotStartup.h" +#include "utils/VoltageController.h" + +#include "subsystems/Arm.h" +#include "subsystems/Shooter.h" +#include "subsystems/Elevator.h" + +#include "drivetrain/Drivetrain.h" + +namespace wom { + using namespace wom; + using namespace wom::utils; + using namespace wom::subsystems; + using namespace wom::drivetrain; + using namespace behaviour; +} diff --git a/wombat/src/main/include/drivetrain/Drivetrain.h b/wombat/src/main/include/drivetrain/Drivetrain.h index 14ecefdb..1e8036e1 100644 --- a/wombat/src/main/include/drivetrain/Drivetrain.h +++ b/wombat/src/main/include/drivetrain/Drivetrain.h @@ -12,16 +12,17 @@ #include namespace wom { +namespace drivetrain { // TODO PID struct DrivetrainConfig { std::string path; - wom::Gearbox left1; - wom::Gearbox left2; - wom::Gearbox left3; - wom::Gearbox right1; - wom::Gearbox right2; - wom::Gearbox right3; + wom::utils::Gearbox left1; + wom::utils::Gearbox left2; + wom::utils::Gearbox left3; + wom::utils::Gearbox right1; + wom::utils::Gearbox right2; + wom::utils::Gearbox right3; }; enum DrivetrainState { @@ -35,7 +36,7 @@ namespace wom { Drivetrain(DrivetrainConfig *config, frc::XboxController &driver); ~Drivetrain(); - DrivetrainConfig *GetConfig(); DrivetrainState GetState(); + DrivetrainConfig *GetConfig(); DrivetrainState GetState(); void SetState(DrivetrainState state); @@ -50,4 +51,5 @@ namespace wom { units::volt_t maxVolts = 9_V; }; } +} diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h index 30832cd5..d79fb0cc 100644 --- a/wombat/src/main/include/subsystems/Arm.h +++ b/wombat/src/main/include/subsystems/Arm.h @@ -12,14 +12,15 @@ #include namespace wom { +namespace subsystems { struct ArmConfig { std::string path; - wom::Gearbox leftGearbox; - wom::Gearbox rightGearbox; + wom::utils::Gearbox leftGearbox; + wom::utils::Gearbox rightGearbox; rev::SparkMaxRelativeEncoder armEncoder; - wom::PIDConfig pidConfig; - wom::PIDConfig velocityConfig; + wom::utils::PIDConfig pidConfig; + wom::utils::PIDConfig velocityConfig; units::kilogram_t armMass; units::kilogram_t loadMass; @@ -50,21 +51,21 @@ namespace wom { void SetRaw(units::volt_t voltage); void SetVelocity(units::radians_per_second_t velocity); - void SetArmSpeedLimit(double limit); //units, what are they?? + void SetArmSpeedLimit(double limit); //units, what are they?? ArmConfig &GetConfig(); units::radian_t GetAngle() const; units::radians_per_second_t MaxSpeed() const; units::radians_per_second_t GetArmVelocity() const; - + bool IsStable() const; private: ArmConfig _config; ArmState _state = ArmState::kIdle; - wom::PIDController _pid; - wom::PIDController _velocityPID; - + wom::utils::PIDController _pid; + wom::utils::PIDController _velocityPID; + std::shared_ptr _table; double armLimit = 0.4; @@ -72,4 +73,5 @@ namespace wom { units::volt_t _voltage{0}; }; +} }; diff --git a/wombat/src/main/include/subsystems/Elevator.h b/wombat/src/main/include/subsystems/Elevator.h index a1e6ef55..90a2b86d 100644 --- a/wombat/src/main/include/subsystems/Elevator.h +++ b/wombat/src/main/include/subsystems/Elevator.h @@ -1,4 +1,4 @@ -#pragma once +#pragma once #include "utils/Gearbox.h" #include "utils/PID.h" @@ -15,8 +15,9 @@ #include namespace wom { +namespace subsystems { enum class ElevatorState { - kIdle, + kIdle, kPID, kManual, kVelocity @@ -24,8 +25,8 @@ namespace wom { struct ElevatorConfig { std::string path; - wom::Gearbox leftGearbox; - wom::Gearbox rightGearbox; + wom::utils::Gearbox leftGearbox; + wom::utils::Gearbox rightGearbox; rev::SparkMaxRelativeEncoder elevatorEncoder; frc::DigitalInput *topSensor; frc::DigitalInput *bottomSensor; @@ -34,14 +35,14 @@ namespace wom { units::meter_t maxHeight; units::meter_t minHeight; units::meter_t initialHeight; - PIDConfig pid; - PIDConfig velocityPID; + wom::utils::PIDConfig pid; + wom::utils::PIDConfig velocityPID; void WriteNT(std::shared_ptr table); }; class Elevator : public behaviour::HasBehaviour { - public: + public: Elevator(ElevatorConfig params); void OnUpdate(units::second_t dt); @@ -59,14 +60,14 @@ namespace wom { void SetElevatorSpeedLimit(double limit); ElevatorConfig &GetConfig(); - + bool IsStable() const; ElevatorState GetState() const; units::meter_t GetHeight() const; units::meters_per_second_t MaxSpeed() const; units::meters_per_second_t GetElevatorVelocity() const; - + private: units::volt_t _setpointManual{0}; @@ -76,9 +77,10 @@ namespace wom { units::meters_per_second_t _velocity; - PIDController _pid; - PIDController _velocityPID; + wom::utils::PIDController _pid; + wom::utils::PIDController _velocityPID; std::shared_ptr _table; }; +} }; diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h index fe985074..9dc50b55 100644 --- a/wombat/src/main/include/subsystems/Shooter.h +++ b/wombat/src/main/include/subsystems/Shooter.h @@ -12,6 +12,7 @@ #include namespace wom { +namespace subsystems { enum class ShooterState { kPID, kManual, @@ -19,8 +20,8 @@ namespace wom { }; struct ShooterParams { - Gearbox gearbox; - PIDConfig pid; + wom::utils::Gearbox gearbox; + wom::utils::PIDConfig pid; units::ampere_t currentLimit; }; @@ -42,7 +43,7 @@ namespace wom { units::volt_t _setpointManual{0}; - PIDController _pid; + wom::utils::PIDController _pid; std::shared_ptr _table; }; @@ -68,3 +69,4 @@ namespace wom { bool _hold; }; } +} diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h index 5deae642..3cbcfd14 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -11,6 +11,7 @@ #include "utils/Util.h" namespace wom { +namespace utils { class Encoder { public: Encoder(double encoderTicksPerRotation, double reduction, int type) : _encoderTicksPerRotation(encoderTicksPerRotation), _reduction(reduction), _type(type) {}; @@ -75,29 +76,29 @@ namespace wom { }; class TalonSRXEncoder : public Encoder { - public: + public: TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX *controller, double ticksPerRotation, double reduction = 1); - + double GetEncoderRawTicks() const override; double GetEncoderTickVelocity() const override; - private: + private: ctre::phoenix::motorcontrol::can::TalonSRX *_controller; }; class DutyCycleEncoder : public Encoder { - public: + public: DutyCycleEncoder(int channel, double ticksPerRotation = 1, double reduction = 1); double GetEncoderRawTicks() const override; double GetEncoderTickVelocity() const override; - private: + private: frc::DutyCycleEncoder _dutyCycleEncoder; }; class CanEncoder : public Encoder { - public: + public: CanEncoder(int deviceNumber, double ticksPerRotation = 4095, double reduction = 1); double GetEncoderRawTicks() const override; @@ -106,7 +107,8 @@ namespace wom { const double constantValue = 0.0; - private: + private: CANCoder *_canEncoder; }; +} } // namespace wom diff --git a/wombat/src/main/include/utils/Gearbox.h b/wombat/src/main/include/utils/Gearbox.h index 5a18e828..5a677776 100644 --- a/wombat/src/main/include/utils/Gearbox.h +++ b/wombat/src/main/include/utils/Gearbox.h @@ -1,10 +1,11 @@ -#pragma once +#pragma once #include "utils/Encoder.h" #include "VoltageController.h" #include namespace wom { +namespace utils { using DCMotor = frc::DCMotor; /** * Struct for motor and encoder pairs. @@ -29,4 +30,5 @@ struct Gearbox { */ frc::DCMotor motor = frc::DCMotor::CIM(2); }; -} //ns wom +} +} //ns wom diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h index 6f802034..0acecec0 100644 --- a/wombat/src/main/include/utils/PID.h +++ b/wombat/src/main/include/utils/PID.h @@ -12,6 +12,7 @@ #include namespace wom { +namespace utils { template struct PIDConfig { using in_t = units::unit_t; @@ -64,7 +65,7 @@ namespace wom { config_t config; - PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0}) + PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0}) : config(initialGains), _setpoint(setpoint), _posFilter(frc::LinearFilter::MovingAverage(20)), _velFilter(frc::LinearFilter::MovingAverage(20)), @@ -98,7 +99,7 @@ namespace wom { _integralSum += error * dt; if (config.izone.value() > 0 && (error > config.izone || error < -config.izone)) _integralSum = sum_t{0}; - + typename config_t::deriv_t deriv{0}; if (_iterations > 0) @@ -157,7 +158,7 @@ namespace wom { in_t _last_pv{0}, _last_error{0}; std::optional _wrap_range; - + int _iterations = 0; frc::LinearFilter _posFilter; @@ -169,3 +170,4 @@ namespace wom { std::shared_ptr _table; }; } +} diff --git a/wombat/src/main/include/utils/RobotStartup.h b/wombat/src/main/include/utils/RobotStartup.h index c29d9131..6b398f42 100644 --- a/wombat/src/main/include/utils/RobotStartup.h +++ b/wombat/src/main/include/utils/RobotStartup.h @@ -1,12 +1,13 @@ -#pragma once +#pragma once #include #include #include -namespace wom { +namespace wom { +namespace utils { class RobotStartup { - public: + public: static void Start(std::function func); }; @@ -18,8 +19,8 @@ int StartRobot() { #ifndef RUNNING_FRC_TESTS #define WOMBAT_ROBOT_MAIN(RobotClz) int main() { wom::StartRobot(); } -#else +#else #define WOMBAT_ROBOT_MAIN(RobotClz) #endif - +} } // ns wom diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h index 248c8d5f..65e0d6b7 100644 --- a/wombat/src/main/include/utils/Util.h +++ b/wombat/src/main/include/utils/Util.h @@ -14,6 +14,7 @@ #include namespace wom { +namespace utils { template T&& invert(T &&system) { system.SetInverted(true); @@ -35,16 +36,16 @@ namespace wom { this->_onUpdate(event.GetValueEventData()->value); })); } - - NTBound(const NTBound &other) + + NTBound(const NTBound &other) : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) { - + _listener = _table->AddListener(_name, nt::EventFlags::kValueAll, ([this](nt::NetworkTable *table, std::string_view key, const nt::Event &event) { std::cout << "NT UPDATE" << std::endl; this->_onUpdate(event.GetValueEventData()->value); })); } - + ~NTBound() { _table->RemoveListener(_listener); } @@ -75,3 +76,4 @@ namespace wom { double deadzone(double val, double deadzone = 0.05); double spow2(double val); } +} diff --git a/wombat/src/main/include/utils/VoltageController.h b/wombat/src/main/include/utils/VoltageController.h index bad03620..196ed022 100644 --- a/wombat/src/main/include/utils/VoltageController.h +++ b/wombat/src/main/include/utils/VoltageController.h @@ -7,6 +7,7 @@ #include "utils/Util.h" namespace wom { +namespace utils { /** * A VoltageController is analagous to a MotorController, but in terms of voltage instead * of speed. @@ -57,4 +58,5 @@ namespace wom { private: frc::MotorController *_MotorController; }; +} } // namespace wom