diff --git a/README.md b/README.md index 5116872..4c147c4 100644 --- a/README.md +++ b/README.md @@ -184,4 +184,5 @@ src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:94:44: warning: Variable 'ol_ta src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:108:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable] src\frc846\cpp\frc846\math\collection.cc:11:0: warning: The function 'HorizontalDeadband' is never used. [unusedFunction] src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] +src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction] ``` \ No newline at end of file diff --git a/build.gradle b/build.gradle index aa81f60..28e8734 100644 --- a/build.gradle +++ b/build.gradle @@ -36,7 +36,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcCpp def includeDesktopSupport = false // Set to true to run simulation in debug mode -wpi.cpp.debugSimulation = false +wpi.cpp.debugSimulation = true // Default enable simgui wpi.sim.addGui().defaultEnabled = false diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..903bd45 --- /dev/null +++ b/networktables.json @@ -0,0 +1,138 @@ +[ + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_current_limit (A)", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_current_limit (A)", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_smart_current_limit (A)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_smart_current_limit (A)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/FR/SwerveDrivetrain/FR/cancoder_offset_ (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/FL/SwerveDrivetrain/FL/cancoder_offset_ (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/BR/SwerveDrivetrain/BR/cancoder_offset_ (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/BL/SwerveDrivetrain/BL/cancoder_offset_ (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/steer_gains/kP", + "type": "double", + "value": 0.3, + "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/robot_container/init_drivetrain", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_leds", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/control_input/trigger_threshold", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + } +] diff --git a/networktables.json.bck b/networktables.json.bck new file mode 100644 index 0000000..c07936a --- /dev/null +++ b/networktables.json.bck @@ -0,0 +1,58 @@ +[ + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_current_limit (A)", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_current_limit (A)", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_smart_current_limit (A)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_smart_current_limit (A)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/FR/SwerveDrivetrain/FR/cancoder_offset_ (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + } +] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..272f1ce --- /dev/null +++ b/simgui.json @@ -0,0 +1,13 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/get_prune_list": "Command", + "/SmartDashboard/prune_prefs": "Command", + "/SmartDashboard/verify_hardware": "Command" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/frc846/cpp/frc846/control/HigherMotorController.cc b/src/frc846/cpp/frc846/control/HigherMotorController.cc index c6e2b0f..9cc972c 100644 --- a/src/frc846/cpp/frc846/control/HigherMotorController.cc +++ b/src/frc846/cpp/frc846/control/HigherMotorController.cc @@ -11,10 +11,6 @@ HigherMotorController::HigherMotorController( : mmtype_(mmtype), constr_params_(params) {} void HigherMotorController::Setup() { - if (!called_enable_status_frames_) { - throw std::runtime_error( - "HigherMotorController: Call EnableStatusFrames before Setup"); - } slot_id_ = frc846::control::MotorMonkey::ConstructController( mmtype_, constr_params_); } @@ -113,7 +109,6 @@ void HigherMotorController::SetSoftLimits( void HigherMotorController::EnableStatusFrames( std::vector frames) { frc846::control::MotorMonkey::EnableStatusFrames(slot_id_, frames); - called_enable_status_frames_ = true; } } // namespace frc846::control diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index 171cc1c..5171dc7 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -17,10 +17,11 @@ namespace frc846::control { -#define CHECK_SLOT_ID() \ - if (slot_id >= CONTROLLER_REGISTRY_SIZE || \ - controller_registry[slot_id] == nullptr) \ - throw std::runtime_error("Invalid MotorMonkey slot ID") +#define CHECK_SLOT_ID() \ + if (controller_registry[slot_id] == nullptr) \ + throw std::runtime_error( \ + "Invalid MotorMonkey slot ID: " + std::to_string(slot_id) + " in " + \ + "[" + __func__ + "]"); #define LOG_IF_ERROR(action_name) \ { \ @@ -35,20 +36,22 @@ namespace frc846::control { #define NUM_RETRIES 5 #define INITIAL_RETRY_DELAY_MS 10 -#define SMART_RETRY(action, action_name) \ - for (int i = 0; i < NUM_RETRIES; i++) { \ - action; \ - hardware::ControllerErrorCodes err = \ - controller_registry[slot_id]->GetLastErrorCode(); \ - if (err == hardware::ControllerErrorCodes::kAllOK) \ - break; \ - else { \ - loggable_.Warn( \ - "Error [{}] while attempting [{}] for slot ID {}. Retrying...", \ - parseError(err), action_name, slot_id); \ - std::this_thread::sleep_for( \ - std::chrono::milliseconds(INITIAL_RETRY_DELAY_MS * (1 << i))); \ - } \ +#define SMART_RETRY(action, action_name) \ + for (int i = 0; i < NUM_RETRIES; i++) { \ + action; \ + hardware::ControllerErrorCodes err = \ + controller_registry[slot_id]->GetLastErrorCode(); \ + if (err == hardware::ControllerErrorCodes::kAllOK) \ + break; \ + else if (i == NUM_RETRIES - 1) { \ + loggable_.Error("Failed [{}] for slot ID {}.", action_name, slot_id); \ + } else { \ + loggable_.Warn( \ + "Error [{}] while attempting [{}] for slot ID {}. Retrying...", \ + parseError(err), action_name, slot_id); \ + std::this_thread::sleep_for( \ + std::chrono::milliseconds(INITIAL_RETRY_DELAY_MS * (1 << i))); \ + } \ } frc846::base::Loggable MotorMonkey::loggable_{"MotorMonkey"}; @@ -106,7 +109,10 @@ size_t MotorMonkey::ConstructController( frc846::control::hardware::IntermediateController* this_controller = nullptr; if (frc::RobotBase::IsSimulation() && - MOTOR_SIM_LEVEL == MOTOR_SIM_LEVEL_SIM_HARDWARE) { + MOTOR_SIM_LEVEL == MOTOR_SIM_LEVEL_SIM_PHYSICS) { + std::cout << "Constructing simulation controller" << std::endl; + loggable_.Log( + "Constructing physics simulation controller for slot ID {}.", slot_id); slot_id_to_sim_[slot_id] = true; this_controller = controller_registry[slot_id] = new frc846::control::simulation::MCSimulator{ @@ -126,9 +132,13 @@ size_t MotorMonkey::ConstructController( this_controller = controller_registry[slot_id] = new frc846::control::hardware::SparkFLEX_interm{ params.can_id, params.max_wait_time}; + } else { + throw std::runtime_error("Invalid MotorMonkeyType [" + + std::to_string((int)type) + + "]: not constructing controller"); } - if (this_controller == nullptr) return slot_id; + if (this_controller == nullptr) { return slot_id; } SMART_RETRY(this_controller->SetInverted(params.inverted), "SetInverted"); LOG_IF_ERROR("SetInverted"); diff --git a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc index de3c8f1..9a02496 100644 --- a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc +++ b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc @@ -50,18 +50,16 @@ SwerveModuleSubsystem::getMotorParams(SwerveModuleUniqueConfig unique_config, } void SwerveModuleSubsystem::Setup() { + drive_.Setup(); drive_.EnableStatusFrames( {frc846::control::config::StatusFrame::kPositionFrame, frc846::control::config::StatusFrame::kVelocityFrame}); - - drive_.Setup(); drive_helper_.SetPosition(0_ft); + steer_.Setup(); steer_.EnableStatusFrames( {frc846::control::config::StatusFrame::kPositionFrame}); - steer_.Setup(); - ZeroWithCANcoder(); } diff --git a/src/frc846/include/frc846/control/HigherMotorController.h b/src/frc846/include/frc846/control/HigherMotorController.h index 4173d8b..7308263 100644 --- a/src/frc846/include/frc846/control/HigherMotorController.h +++ b/src/frc846/include/frc846/control/HigherMotorController.h @@ -104,8 +104,6 @@ class HigherMotorController { std::optional soft_limits_; size_t slot_id_ = 999; - - bool called_enable_status_frames_ = false; }; } // namespace frc846::control \ No newline at end of file diff --git a/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h b/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h index c42ef26..04ec905 100644 --- a/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h +++ b/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h @@ -5,6 +5,6 @@ namespace frc846::control::simulation { #define MOTOR_SIM_LEVEL_SIM_HARDWARE 1 #define MOTOR_SIM_LEVEL_SIM_PHYSICS 2 -#define MOTOR_SIM_LEVEL MOTOR_SIM_LEVEL_SIM_HARDWARE +#define MOTOR_SIM_LEVEL MOTOR_SIM_LEVEL_SIM_PHYSICS } // namespace frc846::control::simulation \ No newline at end of file diff --git a/src/frc846/include/frc846/math/constants.h b/src/frc846/include/frc846/math/constants.h index da72d7f..49ebb07 100644 --- a/src/frc846/include/frc846/math/constants.h +++ b/src/frc846/include/frc846/math/constants.h @@ -9,7 +9,7 @@ struct constants { static constexpr units::feet_per_second_squared_t g{32.17405}; }; struct geometry { - static constexpr double pi = 3.14159265; + static constexpr double pi = 3.14159265358979323846; }; }; diff --git a/src/y2025/cpp/FunkyRobot.cc b/src/y2025/cpp/FunkyRobot.cc index 079a629..08f648d 100644 --- a/src/y2025/cpp/FunkyRobot.cc +++ b/src/y2025/cpp/FunkyRobot.cc @@ -63,7 +63,7 @@ void FunkyRobot::InitTest() {} #ifndef RUNNING_FRC_TESTS int main() { - // configureSignalHandlers(); + if (frc::RobotBase::IsSimulation()) configureSignalHandlers(); return frc::StartRobot(); } #endif \ No newline at end of file diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc index 1e31d2f..6e4b910 100644 --- a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc +++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc @@ -1,6 +1,7 @@ #include "subsystems/hardware/DrivetrainConstructor.h" #include "frc846/control/calculators/CircuitResistanceCalculator.h" +#include "frc846/math/constants.h" #include "ports.h" DrivetrainConstructor::DrivetrainConstructor() @@ -39,7 +40,7 @@ DrivetrainConstructor::getDrivetrainConfigs() { unsigned int num_connectors_BR = 3; frc846::robot::swerve::drive_conv_unit drive_reduction = - (M_PI * wheel_diameter) / 6.12_tr; + (frc846::math::constants::geometry::pi * wheel_diameter) / 6.12_tr; frc846::robot::swerve::steer_conv_unit steer_reduction = 7_tr / 150_tr; configs.wheelbase_forward_dim = 26_in; diff --git a/src/y2025/include/rsighandler.h b/src/y2025/include/rsighandler.h index 77d82f1..b421818 100644 --- a/src/y2025/include/rsighandler.h +++ b/src/y2025/include/rsighandler.h @@ -45,7 +45,7 @@ void handler(int sig) { } else { std::cerr << "? Unknown Exception Occured" << std::endl; } - // exit(1); + exit(1); } #endif