diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp index c97ead0f..93b5b5f3 100644 --- a/src/main/cpp/AlphaArm.cpp +++ b/src/main/cpp/AlphaArm.cpp @@ -3,8 +3,12 @@ // of the MIT License at the root of this project #include "AlphaArm.h" -//fiddle with these values -AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config), _pidWom(_config.path + "/pid", config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/{} + +// fiddle with these values +AlphaArm::AlphaArm(AlphaArmConfig config) + : _config(config), + _pidWom(_config.path + "/pid", + config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/ {} void AlphaArm::OnStart() { started = false; @@ -12,98 +16,101 @@ void AlphaArm::OnStart() { // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition()); } -void AlphaArm::OnUpdate(units::second_t dt){ +void AlphaArm::OnUpdate(units::second_t dt) { _table->GetEntry("Error").SetDouble(_pidWom.GetError().value()); _table->GetEntry("Current Pos").SetDouble(_config.alphaArmGearbox.encoder->GetEncoderPosition().value()); _table->GetEntry("Setpoint").SetDouble(_pidWom.GetSetpoint().value()); _table->GetEntry("State ").SetString(_stateName); - switch(_state){ - case AlphaArmState::kIdle: - _stateName = "Idle"; - // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition()); - - - _setAlphaArmVoltage = 0_V; + switch (_state) { + case AlphaArmState::kIdle: + _stateName = "Idle"; + // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition()); + + _setAlphaArmVoltage = 0_V; - break; - case AlphaArmState::kRaw: - _stateName = "Raw"; - - _setAlphaArmVoltage = _rawArmVoltage; + break; + case AlphaArmState::kRaw: + _stateName = "Raw"; - break; - case AlphaArmState::kAmpAngle: - { - _stateName = "Amp Angle"; + _setAlphaArmVoltage = _rawArmVoltage; - // _pidWom.SetSetpoint(_goal); - // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V); + break; + case AlphaArmState::kAmpAngle: { + _stateName = "Amp Angle"; - if (started) { - if (_controlledRawVoltage.value() == 0) { - if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad/2))) { - // _pidWom.SetSetpoint(_encoderSetpoint); - // _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V); - // _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value()); - // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) { - // _pidWom.SetSetpoint(_encoderSetpoint); - // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V); - // _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value()); - _setAlphaArmVoltage = 0_V; - } else { - _pidWom.SetSetpoint(_encoderSetpoint); - _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V); - // _pidWom.Reset(); - // _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition(); - // _setAlphaArmVoltage = _controlledRawVoltage; - } - } else { - _pidWom.Reset(); - _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition(); - _setAlphaArmVoltage = _controlledRawVoltage; - } - } else { - _pidWom.Reset(); - _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition(); - _setAlphaArmVoltage = _controlledRawVoltage; + // _pidWom.SetSetpoint(_goal); + // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, + // 0_V); - if (std::abs(_controlledRawVoltage.value()) > 0) { - _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition(); - started = true; - } - } - + if (started) { + if (_controlledRawVoltage.value() == 0) { + if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad / 2))) { + // _pidWom.SetSetpoint(_encoderSetpoint); + // _setAlphaArmVoltage = + // -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V); + // _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value()); + // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) { + // _pidWom.SetSetpoint(_encoderSetpoint); + // _setAlphaArmVoltage = + // _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V); + // _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value()); + _setAlphaArmVoltage = 0_V; + } else { + _pidWom.SetSetpoint(_encoderSetpoint); + _setAlphaArmVoltage = + -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V); + // _pidWom.Reset(); + // _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition(); + // _setAlphaArmVoltage = _controlledRawVoltage; } - break; - case AlphaArmState::kSpeakerAngle: - _stateName = "Speaker Angle"; - //_pidWom.SetSetpoint(_goal); - // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V); - break; - case AlphaArmState::kStowed: - _stateName = "Stowed"; - //_pidWom.SetSetpoint(_goal); - //_setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V); - break; - default: - _stateName = "Error"; - std::cout << "oops, wrong state" << std::endl; - break; + } else { + _pidWom.Reset(); + _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition(); + _setAlphaArmVoltage = _controlledRawVoltage; + } + } else { + _pidWom.Reset(); + _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition(); + _setAlphaArmVoltage = _controlledRawVoltage; + + if (std::abs(_controlledRawVoltage.value()) > 0) { + _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition(); + started = true; } - std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() << std::endl; - //std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl; - _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage); + } + } break; + case AlphaArmState::kSpeakerAngle: + _stateName = "Speaker Angle"; + // _pidWom.SetSetpoint(_goal); + // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, + // 0_V); + break; + case AlphaArmState::kStowed: + _stateName = "Stowed"; + // _pidWom.SetSetpoint(_goal); + // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, + // 0_V); + break; + default: + _stateName = "Error"; + std::cout << "oops, wrong state" << std::endl; + break; + } + std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() + << std::endl; + // std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl; + _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage); } void AlphaArm::SetState(AlphaArmState state) { _state = state; } -void AlphaArm::SetGoal(units::radian_t goal){ +void AlphaArm::SetGoal(units::radian_t goal) { _goal = goal; } -void AlphaArm::SetArmRaw(units::volt_t voltage){ +void AlphaArm::SetArmRaw(units::volt_t voltage) { std::cout << "VOLTAGE: " << voltage.value() << std::endl; _rawArmVoltage = voltage; } diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp index d3411a98..9a28b311 100644 --- a/src/main/cpp/AlphaArmBehaviour.cpp +++ b/src/main/cpp/AlphaArmBehaviour.cpp @@ -12,7 +12,6 @@ AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxContro } void AlphaArmManualControl::OnTick(units::second_t dt) { - if (_codriver->GetStartButtonPressed()) { if (_rawControl == true) { _rawControl = false; @@ -23,7 +22,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) { if (_rawControl) { _alphaArm->SetState(AlphaArmState::kRaw); - _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V); + _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V); } else { _alphaArm->SetState(AlphaArmState::kAmpAngle); _alphaArm->setControllerRaw(wom::deadzone(_codriver->GetRightY()) * 12_V); diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp index a96c289b..6110aa7e 100644 --- a/src/main/cpp/Intake.cpp +++ b/src/main/cpp/Intake.cpp @@ -78,4 +78,4 @@ void Intake::setRaw(units::volt_t voltage) { } IntakeState Intake::getState() { return _state; -} \ No newline at end of file +} diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp index 3192ef12..3d738fb3 100644 --- a/src/main/cpp/IntakeBehaviour.cpp +++ b/src/main/cpp/IntakeBehaviour.cpp @@ -60,4 +60,4 @@ IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) { Controls(intake); } -void IntakeAutoControl::OnTick(units::second_t dt) {} \ No newline at end of file +void IntakeAutoControl::OnTick(units::second_t dt) {} diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 25e05917..a1ba628b 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -49,7 +49,6 @@ void Robot::RobotInit() { // shooter->SetDefaultBehaviour( // [this]() { return wom::make(shooter, &robotmap.controllers.codriver); }); - // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser); // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser); @@ -59,20 +58,16 @@ void Robot::RobotInit() { // robotmap.swerveBase.gyro->Reset(); - _swerveDrive = - new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); + _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive); - _swerveDrive->SetDefaultBehaviour([this]() { - return wom::make(_swerveDrive, - &robotmap.controllers.driver); - }); - + _swerveDrive->SetDefaultBehaviour( + [this]() { return wom::make(_swerveDrive, &robotmap.controllers.driver); }); // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config); // wom::BehaviourScheduler::GetInstance()->Register(alphaArm); // alphaArm->SetDefaultBehaviour( // [this]() { return wom::make(alphaArm, &robotmap.controllers.codriver); }); - + // // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field); // // m_driveSim = wom::TempSimSwerveDrive(); @@ -97,21 +92,18 @@ void Robot::RobotInit() { // robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true); // robotmap.swerveBase.moduleConfigs[2].driveMotor.motorController->SetInverted(true); - //robotmap.alphaArmSystem.armEncoder->Reset(); - - + // robotmap.alphaArmSystem.armEncoder->Reset(); + // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad); // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad); // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad); // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad); lastPeriodic = wom::now(); - - } void Robot::RobotPeriodic() { - //double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance(); + // double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance(); auto dt = wom::now() - lastPeriodic; lastPeriodic = wom::now(); @@ -119,16 +111,19 @@ void Robot::RobotPeriodic() { wom::BehaviourScheduler::GetInstance()->Tick(); sched->Tick(); - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value()); - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value()); - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value()); - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value()); + robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ") + .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value()); + robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ") + .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value()); + robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ") + .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value()); + robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ") + .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value()); // shooter->OnUpdate(dt); // intake->OnUpdate(dt); // alphaArm->OnUpdate(dt); _swerveDrive->OnUpdate(dt); - } void Robot::AutonomousInit() { diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index c6c40a02..44326bda 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -1,22 +1,22 @@ // Copyright (c) 2023-2024 CurtinFRC // Open Source Software, you can modify it according to the terms // of the MIT License at the root of this project -#include "Shooter.h" +#include "Shooter.h" -Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {} +Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {} void Shooter::OnStart() { _pid.Reset(); } - void Shooter::OnUpdate(units::second_t dt) { // _pid.SetTolerance(0.5, 4); table->GetEntry("Error").SetDouble(_pid.GetError().value()); // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError()); table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value()); - // table->GetEntry("Current Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value()); + // table->GetEntry("Current + // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value()); // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue()); table->GetEntry("Shooting").SetString(_statename); switch (_state) { @@ -29,13 +29,13 @@ void Shooter::OnUpdate(units::second_t dt) { // if (_shooterSensor.Get()) { // _state = ShooterState::kReverse; // } - } break; case ShooterState::kSpinUp: { _statename = "SpinUp"; std::cout << "KSpinUp" << std::endl; _pid.SetSetpoint(_goal); - units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)}; + units::volt_t pidCalculate = + units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)}; std::cout << "KShooting" << std::endl; if (_pid.IsStable()) { @@ -49,13 +49,13 @@ void Shooter::OnUpdate(units::second_t dt) { } else { _setVoltage = holdVoltage; } - } break; case ShooterState::kShooting: { _statename = "Shooting"; - + _pid.SetSetpoint(_goal); - units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)}; + units::volt_t pidCalculate = + units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)}; std::cout << "KShooting" << std::endl; if (_pid.IsStable()) { @@ -68,7 +68,6 @@ void Shooter::OnUpdate(units::second_t dt) { } else { _setVoltage = holdVoltage; } - } break; case ShooterState::kReverse: { @@ -95,8 +94,8 @@ void Shooter::OnUpdate(units::second_t dt) { } break; } // table->GetEntry("Motor OutPut").SetDouble(_setVoltage.value()); - table->GetEntry("Encoder Output").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value()); - + table->GetEntry("Encoder Output") + .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value()); _config.ShooterGearbox.motorController->SetVoltage(_setVoltage); } @@ -116,6 +115,6 @@ void Shooter::SetRaw(units::volt_t voltage) { _rawVoltage = voltage; _state = ShooterState::kRaw; } -void Shooter::SetPidGoal(units::radians_per_second_t goal) { +void Shooter::SetPidGoal(units::radians_per_second_t goal) { _goal = goal; } diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp index 7b370456..09a1d133 100644 --- a/src/main/cpp/ShooterBehaviour.cpp +++ b/src/main/cpp/ShooterBehaviour.cpp @@ -36,7 +36,7 @@ void ShooterManualControl::OnTick(units::second_t dt) { } else if (_codriver->GetYButton()) { _shooter->SetPidGoal(300_rad_per_s); _shooter->SetState(ShooterState::kSpinUp); - } else if (_codriver->GetLeftTriggerAxis() > 0.1) { + } else if (_codriver->GetLeftTriggerAxis() > 0.1) { _shooter->SetState(ShooterState::kRaw); _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis()); } else if (_codriver->GetRightTriggerAxis() > 0.1) { diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h index 9a5e94dc..db0bc3e6 100644 --- a/src/main/include/AlphaArm.h +++ b/src/main/include/AlphaArm.h @@ -7,72 +7,75 @@ #include #include #include + +#include +#include + #include "Wombat.h" #include "utils/PID.h" struct AlphaArmConfig { - wom::Gearbox alphaArmGearbox; - //wom::Gearbox wristGearbox; - //wom::DutyCycleEncoder* armEncoder; - //wom::CANSparkMaxEncoder* armEncoder; - wom::utils::PIDConfig pidConfigA; - //wom::utils::PIDConfig velocityConfig; - std::string path; - //void WriteNT(std::shared_ptr table); - + wom::Gearbox alphaArmGearbox; + // wom::Gearbox wristGearbox; + // wom::DutyCycleEncoder* armEncoder; + // wom::CANSparkMaxEncoder* armEncoder; + wom::utils::PIDConfig pidConfigA; + // wom::utils::PIDConfig velocityConfig; + std::string path; + // void WriteNT(std::shared_ptr table); }; enum class AlphaArmState { - kIdle, - kIntakeAngle, - kAmpAngle, - kSpeakerAngle, - kStowed, - kRaw - //kForwardWrist, - //kReverseWrist, + kIdle, + kIntakeAngle, + kAmpAngle, + kSpeakerAngle, + kStowed, + kRaw + // kForwardWrist, + // kReverseWrist, }; -class AlphaArm : public::behaviour::HasBehaviour{ - public: - explicit AlphaArm(AlphaArmConfig config); +class AlphaArm : public ::behaviour::HasBehaviour { + public: + explicit AlphaArm(AlphaArmConfig config); + + void OnStart(); + void OnUpdate(units::second_t dt); + void SetArmRaw(units::volt_t voltage); + void SetState(AlphaArmState state); + void setControllerRaw(units::volt_t); + // void setGoal(units::radians_per_second_t); + void SetGoal(units::radian_t); + double GetArmEncoder(); + AlphaArmConfig GetConfig(); + std::shared_ptr table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm"); + // units::radians_per_second_t goal; + // double goal; - void OnStart(); - void OnUpdate(units::second_t dt); - void SetArmRaw(units::volt_t voltage); - void SetState(AlphaArmState state); - void setControllerRaw(units::volt_t); - //void setGoal(units::radians_per_second_t); - void SetGoal(units::radian_t); - double GetArmEncoder(); - AlphaArmConfig GetConfig(); - std::shared_ptr table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm"); - //units::radians_per_second_t goal; - //double goal; - - //frc::DutyCycleEncoder armEncoder{12}; - //void SetRaw(units::volt_t voltage); + // frc::DutyCycleEncoder armEncoder{12}; + // void SetRaw(units::volt_t voltage); - private: - //frc::PIDController _pidFRC; - wom::utils::PIDController _pidWom; - // wom::utils::PIDController _velocityPID; + private: + // frc::PIDController _pidFRC; + wom::utils::PIDController _pidWom; + // wom::utils::PIDController _velocityPID; - std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm"); - AlphaArmConfig _config; - AlphaArmState _state = AlphaArmState::kIdle; - units::radian_t _goal; - units::volt_t _setAlphaArmVoltage = 0_V; - units::volt_t _setWristVoltage = 0_V; + std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm"); + AlphaArmConfig _config; + AlphaArmState _state = AlphaArmState::kIdle; + units::radian_t _goal; + units::volt_t _setAlphaArmVoltage = 0_V; + units::volt_t _setWristVoltage = 0_V; - units::volt_t _rawArmVoltage = 0_V; - units::volt_t _rawWristVoltage = 0_V; - //units::radiant_t maxAngle = 1_radian_t; + units::volt_t _rawArmVoltage = 0_V; + units::volt_t _rawWristVoltage = 0_V; + // units::radiant_t maxAngle = 1_radian_t; - units::radian_t _encoderSetpoint = 0_rad; - units::volt_t _controlledRawVoltage = 0_V; + units::radian_t _encoderSetpoint = 0_rad; + units::volt_t _controlledRawVoltage = 0_V; units::radian_t _startingPos = 0_rad; - std::string _stateName = "Default"; - bool started = false; + std::string _stateName = "Default"; + bool started = false; }; diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h index 1aca262a..8d2a07d3 100644 --- a/src/main/include/Intake.h +++ b/src/main/include/Intake.h @@ -40,4 +40,4 @@ class Intake : public behaviour::HasBehaviour { units::volt_t _setVoltage = 0_V; std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake"); -}; \ No newline at end of file +}; diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h index 5dbe8ae6..5233c3c2 100644 --- a/src/main/include/IntakeBehaviour.h +++ b/src/main/include/IntakeBehaviour.h @@ -31,4 +31,4 @@ class IntakeAutoControl : public behaviour::Behaviour { private: Intake* _intake; -}; \ No newline at end of file +}; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 799ae2c1..a8bc9ba2 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -22,7 +22,6 @@ #include "RobotMap.h" #include "Shooter.h" #include "ShooterBehaviour.h" -#include "RobotMap.h" #include "Wombat.h" class Robot : public frc::TimedRobot { @@ -56,7 +55,7 @@ class Robot : public frc::TimedRobot { wom::SwerveDrive* _swerveDrive; // AlphaArm* alphaArm; - ctre::phoenix6::hardware::TalonFX *frontLeft; + ctre::phoenix6::hardware::TalonFX* frontLeft; // AlphaArm *alphaArm; // ctre::phoenix6::hardware::TalonFX *frontLeft; diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h index b58e3d02..2c80b462 100644 --- a/src/main/include/RobotMap.h +++ b/src/main/include/RobotMap.h @@ -6,15 +6,14 @@ #include #include +#include +#include #include #include -#include -#include #include #include -#include "utils/PID.h" -#include "utils/Encoder.h" +#include #include #include @@ -26,11 +25,8 @@ #include "Intake.h" #include "Shooter.h" #include "Wombat.h" -#include "Wombat.h" -#include "AlphaArm.h" -#include "AlphaArmBehaviour.h" - -//#include "Wombat/Encoder.h" +#include "utils/Encoder.h" +#include "utils/PID.h" struct RobotMap { struct Controllers { @@ -40,80 +36,77 @@ struct RobotMap { }; Controllers controllers; -// struct AlphaArmSystem { -// rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless}; -// wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m); - -// wom::Gearbox alphaArmGearbox{&alphaArmMotor, armEncoder, frc::DCMotor::NEO(1)}; - -// wom::utils::PIDConfig pidConfigA{ -// "/path/to/pid/in/nt/tables", -// 15_V / 180_deg, -// 0_V / (1_deg * 1_s), -// 0_V / (1_deg / 1_s), -// }; - -// AlphaArmConfig config { -// alphaArmGearbox, -// pidConfigA, -// }; - -// }; -// AlphaArmSystem alphaArmSystem; - -// struct IntakeSystem { -// rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed}; -// // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m}; -// frc::DigitalInput intakeSensor{4}; -// // frc::DigitalInput magSensor{0}; -// // frc::DigitalInput shooterSensor{0}; - -// wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)}; - -// IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/}; -// }; -// IntakeSystem intakeSystem; - -// struct Shooter { -// rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};// Port 11 -// // frc::DigitalInput shooterSensor{2}; - -// // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor); -// wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m); -// wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)}; - -// wom::utils::PIDConfig pidConfigS{ -// "/armavator/arm/velocityPID/config", -// 0.1_V / (360_deg / 1_s), -// 0.03_V / 25_deg, -// 0.001_V / (90_deg / 1_s / 1_s), -// 5_rad_per_s, -// 10_rad_per_s / 1_s -// }; - -// ShooterConfig config{ -// "shooterGearbox", -// shooterGearbox, -// pidConfigS -// }; - - - -// }; -// Shooter shooterSystem; - + // struct AlphaArmSystem { + // rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless}; + // wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m); + + // wom::Gearbox alphaArmGearbox{&alphaArmMotor, armEncoder, frc::DCMotor::NEO(1)}; + + // wom::utils::PIDConfig pidConfigA{ + // "/path/to/pid/in/nt/tables", + // 15_V / 180_deg, + // 0_V / (1_deg * 1_s), + // 0_V / (1_deg / 1_s), + // }; + + // AlphaArmConfig config { + // alphaArmGearbox, + // pidConfigA, + // }; + + // }; + // AlphaArmSystem alphaArmSystem; + + // struct IntakeSystem { + // rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed}; + // // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m}; + // frc::DigitalInput intakeSensor{4}; + // // frc::DigitalInput magSensor{0}; + // // frc::DigitalInput shooterSensor{0}; + + // wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)}; + + // IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/}; + // }; + // IntakeSystem intakeSystem; + + // struct Shooter { + // rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};// Port 11 + // // frc::DigitalInput shooterSensor{2}; + + // // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor); + // wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m); + // wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)}; + + // wom::utils::PIDConfig pidConfigS{ + // "/armavator/arm/velocityPID/config", + // 0.1_V / (360_deg / 1_s), + // 0.03_V / 25_deg, + // 0.001_V / (90_deg / 1_s / 1_s), + // 5_rad_per_s, + // 10_rad_per_s / 1_s + // }; + + // ShooterConfig config{ + // "shooterGearbox", + // shooterGearbox, + // pidConfigS + // }; + + // }; + // Shooter shooterSystem; + struct SwerveBase { ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"}; ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"}; ctre::phoenix6::hardware::CANcoder backLeftCancoder{17, "Drivebase"}; ctre::phoenix6::hardware::CANcoder backRightCancoder{19, "Drivebase"}; - ctre::phoenix6::hardware::Pigeon2* gyro = - new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase"); + ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase"); wpi::array turnMotors{ new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"), // front left new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"), // front right - new ctre::phoenix6::hardware::TalonFX(4, "Drivebase"), // back left + new ctre::phoenix6::hardware::TalonFX(4, "Drivebase"), // back left new ctre::phoenix6::hardware::TalonFX(2, "Drivebase")}; // back right wpi::array driveMotors{ new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"), // front left @@ -122,48 +115,38 @@ struct RobotMap { new ctre::phoenix6::hardware::TalonFX(1, "Drivebase")}; // back right wpi::array moduleConfigs{ - wom::SwerveModuleConfig{ //CORRECT + wom::SwerveModuleConfig{ + // CORRECT // front left module frc::Translation2d(-10_in, 9_in), - wom::Gearbox{ - driveMotors[0], - new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[0], - new wom::CanEncoder(16, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[0], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &frontLeftCancoder, 4_in / 2}, - wom::SwerveModuleConfig{ //CORRECT + wom::SwerveModuleConfig{ + // CORRECT // front right module frc::Translation2d(10_in, 9_in), - wom::Gearbox{ - driveMotors[1], - new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[1], - new wom::CanEncoder(18, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[1], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &frontRightCancoder, 4_in / 2}, wom::SwerveModuleConfig{ // back left module frc::Translation2d(-10_in, 9_in), - wom::Gearbox{ - driveMotors[2], - new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[2], - new wom::CanEncoder(17, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[2], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &backRightCancoder, 4_in / 2}, wom::SwerveModuleConfig{ // back right module frc::Translation2d(-10_in, -9_in), - wom::Gearbox{ - driveMotors[3], - new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[3], - new wom::CanEncoder(19, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[3], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &backLeftCancoder, 4_in / 2}, }; @@ -183,19 +166,16 @@ struct RobotMap { wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0}, 0_deg / 1_deg};*/ wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{ - "/drivetrain/pid/pose/position/config", - 0_mps / 1_m, - wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, - 0_m / 1_m, - 0_cm}; + "/drivetrain/pid/pose/position/config", 0_mps / 1_m, + wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, 0_m / 1_m, 0_cm}; // the config for the whole swerve drive wom::SwerveDriveConfig config{"/drivetrain", - //anglePID, + // anglePID, velocityPID, moduleConfigs, // each module gyro, - //poseAnglePID, + // poseAnglePID, posePositionPID, 60_kg, // robot mass (estimate rn) {0.1, 0.1, 0.1}, diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h index 965b3654..566e30be 100644 --- a/src/main/include/Shooter.h +++ b/src/main/include/Shooter.h @@ -1,6 +1,7 @@ // Copyright (c) 2023-2024 CurtinFRC // Open Source Software, you can modify it according to the terms // of the MIT License at the root of this project + #pragma once #include #include @@ -11,7 +12,6 @@ #include "Wombat.h" - struct ShooterConfig { std::string path; wom::Gearbox ShooterGearbox; diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h index 41e598df..12f93baa 100644 --- a/src/main/include/ShooterBehaviour.h +++ b/src/main/include/ShooterBehaviour.h @@ -6,13 +6,14 @@ #include -#include "Robot.h" +#include + #include "Shooter.h" #include "Wombat.h" class ShooterManualControl : public behaviour::Behaviour { public: - ShooterManualControl(Shooter* shooter, frc::XboxController* codriver); + ShooterManualControl(Shooter* shooter, frc::XboxController* codriver); void OnTick(units::second_t dt) override; @@ -21,5 +22,6 @@ class ShooterManualControl : public behaviour::Behaviour { frc::XboxController* _codriver; bool _rawControl = false; - std::shared_ptr table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour"); -}; \ No newline at end of file + std::shared_ptr table = + nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour"); +}; diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp index d21d9e57..7312c922 100644 --- a/wombat/src/main/cpp/behaviour/Behaviour.cpp +++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp @@ -10,9 +10,7 @@ using namespace behaviour; // Behaviour Behaviour::Behaviour(std::string name, units::time::second_t period) - : _bhvr_name(name), - _bhvr_period(period), - _bhvr_state(BehaviourState::INITIALISED) {} + : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {} Behaviour::~Behaviour() { if (!IsFinished()) Interrupt(); @@ -82,8 +80,7 @@ bool Behaviour::Tick() { if (dt > 2 * _bhvr_period) { std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value() - << " Dt(deadline)=" << (2 * _bhvr_period).value() - << ". Bhvr: " << GetName() << std::endl; + << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl; } if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout) @@ -100,8 +97,7 @@ bool Behaviour::IsRunning() const { } bool Behaviour::IsFinished() const { - return _bhvr_state != BehaviourState::INITIALISED && - _bhvr_state != BehaviourState::RUNNING; + return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING; } void Behaviour::Stop(BehaviourState new_state) { @@ -173,8 +169,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) { } std::string ConcurrentBehaviour::GetName() const { - std::string msg = - (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {"); + std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {"); for (auto b : _children) msg += b->GetName() + ", "; msg += "}"; @@ -190,8 +185,8 @@ void ConcurrentBehaviour::OnStart() { using namespace std::chrono_literals; b->Tick(); - std::this_thread::sleep_for(std::chrono::milliseconds( - static_cast(b->GetPeriod().value() * 1000))); + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(b->GetPeriod().value() * 1000))); } if (IsFinished() && !b->IsFinished()) @@ -273,10 +268,8 @@ void WaitFor::OnTick(units::time::second_t dt) { } // WaitTime -WaitTime::WaitTime(units::time::second_t time) - : WaitTime([time]() { return time; }) {} -WaitTime::WaitTime(std::function time_fn) - : _time_fn(time_fn) {} +WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {} +WaitTime::WaitTime(std::function time_fn) : _time_fn(time_fn) {} void WaitTime::OnStart() { _time = _time_fn(); diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp index 26863c79..eb363b3e 100644 --- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp +++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp @@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) { std::lock_guard lk(_active_mtx); behaviour->Tick(); } - std::this_thread::sleep_for(std::chrono::milliseconds( - static_cast(behaviour->GetPeriod().value() * 1000))); + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(behaviour->GetPeriod().value() * 1000))); } }); } diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp index 964522c6..f2f557c5 100644 --- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp +++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp @@ -8,8 +8,7 @@ using namespace behaviour; -void HasBehaviour::SetDefaultBehaviour( - std::function(void)> fn) { +void HasBehaviour::SetDefaultBehaviour(std::function(void)> fn) { _default_behaviour_producer = fn; } diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp deleted file mode 100644 index f3ce364d..00000000 --- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#include "drivetrain/Drivetrain.h" - -#include - -#include "utils/Util.h" - -using namespace frc; -using namespace units; - -wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, - XboxController& driver) - : _config(config), _driver(driver) {} -wom::drivetrain::Drivetrain::~Drivetrain() {} - -wom::drivetrain::DrivetrainConfig* wom::drivetrain::Drivetrain::GetConfig() { - return _config; -} -wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() { - return _state; -} - -void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) { - _state = state; -} - -void wom::drivetrain::Drivetrain::OnStart() { - std::cout << "Starting Tank" << std::endl; -} - -void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) { - switch (_state) { - case DrivetrainState::kIdle: - break; - case DrivetrainState::kTank: { - double rightSpeed = wom::utils::deadzone(_driver.GetRightY()); - double leftSpeed = wom::utils::deadzone(_driver.GetLeftY()); - // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts); - // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts); - // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts); - // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts); - // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts); - // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts); - break; - } - case DrivetrainState::kAuto: - break; - } -} - */ \ No newline at end of file diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp index 553360c9..701fae07 100644 --- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp +++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp @@ -5,20 +5,18 @@ #include "drivetrain/SwerveDrive.h" #include +#include #include #include -#include - -#include "utils/Util.h" +#include +#include -#include #include #include - -#include -#include +#include #include "frc/MathUtil.h" +#include "utils/Util.h" #include "wpimath/MathShared.h" using namespace wom; @@ -26,19 +24,16 @@ using namespace wom; namespace wom { namespace drivetrain { - - -void SwerveModuleConfig::WriteNT( - std::shared_ptr table) const { +void SwerveModuleConfig::WriteNT(std::shared_ptr table) const { std::array pos{position.X().value(), position.Y().value()}; table->GetEntry("position").SetDoubleArray(std::span(pos)); table->GetEntry("wheelRadius").SetDouble(wheelRadius.value()); } SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config, - //SwerveModule::angle_pid_conf_t anglePID, + // SwerveModule::angle_pid_conf_t anglePID, SwerveModule::velocity_pid_conf_t velocityPID) - : //_anglePIDController(path + "/pid/angle", anglePID), + : // _anglePIDController(path + "/pid/angle", anglePID), _anglePIDController{frc::PIDController(5, 0, 0, 0.005_s)}, _config(config), _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)), @@ -49,7 +44,6 @@ SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config, // _anglePIDController.EnableContinuousInput(0, (3.1415 * 2)); // _anglePIDController.EnableContinuousInput(0, (3.1415)); // _anglePIDController.SetWrap(0, 3.1415); - } void SwerveModule::OnStart() { @@ -60,7 +54,6 @@ void SwerveModule::OnStart() { // _anglePIDController.EnableContinuousInput(-3.141592, (3.141592)); _anglePIDController.EnableContinuousInput(0, (2 * 3.141592)); - _velocityPIDController.Reset(); } @@ -68,8 +61,6 @@ void SwerveModule::OnUpdate(units::second_t dt) { units::volt_t driveVoltage{0}; units::volt_t turnVoltage{0}; - - switch (_state) { case SwerveModuleState::kIdle: driveVoltage = 0_V; @@ -77,17 +68,17 @@ void SwerveModule::OnUpdate(units::second_t dt) { break; case SwerveModuleState::kPID: { auto feedforward = _config.driveMotor.motor.Voltage( - 0_Nm, units::radians_per_second_t{ - _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()}); - // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415)); + 0_Nm, + units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()}); + // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad + // * 3.1415)); double input = _config.turnMotor.encoder->GetEncoderPosition().value(); _table->GetEntry("/testing/GetEncoderPos").SetDouble(input); // _velocityPIDController.SetSetpoint(3); - driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())}; // if (_turnOffset == TurnOffsetValues::forward) { - + // } else if (_turnOffset == TurnOffsetValues::reverse) { // input = input - (3.1415/2); // driveVoltage = -driveVoltage; @@ -104,8 +95,7 @@ void SwerveModule::OnUpdate(units::second_t dt) { std::cerr << "Case not handled" << std::endl; } - units::newton_meter_t torqueLimit = - 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit; + units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit; // units::volt_t voltageMax = _config.driveMotor.motor.Voltage( // torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity()); // units::volt_t voltageMin = _config.driveMotor.motor.Voltage( @@ -121,8 +111,7 @@ void SwerveModule::OnUpdate(units::second_t dt) { // units::math::max(driveVoltage, -_driveModuleVoltageLimit), // _driveModuleVoltageLimit); // was originally 10_V units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V)); - turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), - turnVoltageMax); + turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax); // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V); _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value()); @@ -154,13 +143,11 @@ void SwerveModule::TurnOffset() { // return (_config.turnMotor.encoder->GetEncoderPosition().value()); // } -void SwerveModule::SetAccelerationLimit( - units::meters_per_second_squared_t limit) { +void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) { _currentAccelerationLimit = limit; } -void SwerveDrive::SetAccelerationLimit( - units::meters_per_second_squared_t limit) { +void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) { for (int motorNumber = 0; motorNumber < 4; motorNumber++) { _modules[motorNumber].SetAccelerationLimit(limit); } @@ -179,12 +166,9 @@ void SwerveModule::SetZero(units::second_t dt) { _state = SwerveModuleState::kPID; } -void SwerveModule::SetPID(units::radian_t angle, - units::meters_per_second_t speed, - units::second_t dt) { +void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) { _state = SwerveModuleState::kPID; - // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value()); // _table->GetEntry("diff").SetDouble(diff); // if (std::abs(diff) > (3.14159/2)) { @@ -196,14 +180,13 @@ void SwerveModule::SetPID(units::radian_t angle, // _anglePIDController.SetSetpoint(angle.value()); // _velocityPIDController.SetSetpoint(speed.value()); // } - + // if (diff > (3.14159 / 2)) { // speed *= -1; - // _anglePIDController.SetSetpoint((3.14159 - (angle.value() - _config.turnMotor.encoder->GetEncoderPosition().value()))); + // _anglePIDController.SetSetpoint((3.14159 - (angle.value() - + // _config.turnMotor.encoder->GetEncoderPosition().value()))); // _velocityPIDController.SetSetpoint(speed.value()); // } else { - - // double setValue = 3.141592 - (angle.value() - _config.turnMotor.encoder->GetEncoderPosition().value()); @@ -219,12 +202,10 @@ void SwerveModule::SetPID(units::radian_t angle, // _velocityPIDController.SetSetpoint(-speed.value()); // } // } else { - _anglePIDController.SetSetpoint(angle.value()); - _velocityPIDController.SetSetpoint(speed.value()); + _anglePIDController.SetSetpoint(angle.value()); + _velocityPIDController.SetSetpoint(speed.value()); // } - - // double currentAngle = _config.turnMotor.encoder->GetEncoderPosition().value(); // double setpointAngle = closestAngle(currentAngle, _anglePIDController.GetSetpoint()); // double setpointAngleFlipped = closestAngle(currentAngle, _anglePIDController.GetSetpoint() + 3.1415); @@ -251,38 +232,31 @@ void SwerveModule::SetPID(units::radian_t angle, void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) { units::meters_per_second_t xVelocityComponent = - 1_mps * - (speeds.vx.value() + speeds.omega.value() * _config.position.X().value()); + 1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value()); units::meters_per_second_t yVelocityComponent = - 1_mps * - (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value()); + 1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value()); units::meters_per_second_t velocity = - 1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + - std::pow(yVelocityComponent.value(), 2)); - units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), - xVelocityComponent.value()); + 1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2)); + units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value()); _anglePIDController.SetSetpoint(angle.value()); _velocityPIDController.SetSetpoint(velocity.value()); } units::meters_per_second_t SwerveModule::GetSpeed() const { - units::meters_per_second_t returnVal{ - _config.driveMotor.encoder->GetVelocityValue() * - _config.wheelRadius.value()}; - return returnVal; + units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() * + _config.wheelRadius.value()}; + return returnVal; } units::meter_t SwerveModule::GetDistance() const { - return units::meter_t{ - _config.driveMotor.encoder->GetEncoderPosition().value() * - _config.wheelRadius.value()}; + return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() * + _config.wheelRadius.value()}; } frc::SwerveModulePosition SwerveModule::GetPosition() const { - return frc::SwerveModulePosition{ - GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()}; + return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()}; } const SwerveModuleConfig& SwerveModule::GetConfig() const { @@ -302,11 +276,10 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose) _config.modules[1].position /*2*/, _config.modules[2].position /*3*/), _poseEstimator( _kinematics, frc::Rotation2d(0_deg), - wpi::array{ - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}}, + wpi::array{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, + frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, + frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, + frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}}, initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs), _anglePIDController{frc::PIDController(1, 0, 0)}, _xPIDController(config.path + "/pid/x", _config.posePositionPID), @@ -324,10 +297,8 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose) ResetPose(initialPose); } -frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds( - const units::radian_t robotHeading) { - return frc::ChassisSpeeds::FromFieldRelativeSpeeds( - vx, vy, omega, frc::Rotation2d{robotHeading}); +frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) { + return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading}); } void SwerveDrive::OnUpdate(units::second_t dt) { @@ -348,14 +319,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) { case SwerveDriveState::kPose: { _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt); _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt); - _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; + _target_fr_speeds.omega = + units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; } [[fallthrough]]; case SwerveDriveState::kFieldRelativeVelocity: - _target_speed = - _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians()); + _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians()); if (isRotateToMatchJoystick) { - _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; + _target_speed.omega = units::radians_per_second_t{ + _anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; } // _target_fr_speeds.vy.value() << std::endl; [[fallthrough]]; @@ -379,15 +351,13 @@ void SwerveDrive::OnUpdate(units::second_t dt) { } auto target_states = _kinematics.ToSwerveModuleStates(_target_speed); - frc::ChassisSpeeds new_target_speed {_target_speed.vx, _target_speed.vy, -_target_speed.omega}; + frc::ChassisSpeeds new_target_speed{_target_speed.vx, _target_speed.vy, -_target_speed.omega}; auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed); for (size_t i = 0; i < _modules.size(); i++) { - if ( i == 3) { - _modules[i].SetPID(new_target_states[i].angle.Radians(), - new_target_states[i].speed, dt); + if (i == 3) { + _modules[i].SetPID(new_target_states[i].angle.Radians(), new_target_states[i].speed, dt); } else { - _modules[i].SetPID(target_states[i].angle.Radians(), - target_states[i].speed, dt); + _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt); // target_states[i].angle.Radians().value() << std::endl; } @@ -415,13 +385,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) { case SwerveDriveState::kFRVelocityRotationLock: _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt); _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt); - _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; - _target_speed = - _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians()); + _target_speed.omega = + units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; + _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians()); auto target_states = _kinematics.ToSwerveModuleStates(_target_speed); for (size_t i = 0; i < _modules.size(); i++) { - _modules[i].SetPID(target_states[i].angle.Radians(), - target_states[i].speed, dt); + _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt); } break; } @@ -432,12 +401,10 @@ void SwerveDrive::OnUpdate(units::second_t dt) { _poseEstimator.Update( _config.gyro->GetRotation2d(), - wpi::array{ - _modules[0].GetPosition(), _modules[1].GetPosition(), - _modules[2].GetPosition(), _modules[3].GetPosition()}); + wpi::array{_modules[0].GetPosition(), _modules[1].GetPosition(), + _modules[2].GetPosition(), _modules[3].GetPosition()}); - utils::WritePose2NT(_table->GetSubTable("estimatedPose"), - _poseEstimator.GetEstimatedPosition()); + utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition()); _config.WriteNT(_table->GetSubTable("config")); } @@ -474,8 +441,7 @@ void SwerveDrive::OnResetMode() { _anglePIDController.Reset(); } -void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, - FieldRelativeSpeeds speeds) { +void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) { // _state = SwerveDriveState::kFRVelocityRotationLock; // _anglePIDController.SetSetpoint(joystickAngle); // _target_fr_speeds = speeds; @@ -501,16 +467,14 @@ bool SwerveDrive::GetIsFieldRelative() { return _isFieldRelative; } -void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, - units::meters_per_second_t speed) { +void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) { _mod = mod; _angle = angle; _speed = speed; _state = SwerveDriveState::kIndividualTuning; } -void SwerveDrive::SetTuning(units::radian_t angle, - units::meters_per_second_t speed) { +void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) { _angle = angle; _speed = speed; _state = SwerveDriveState::kTuning; @@ -537,9 +501,8 @@ bool SwerveDrive::IsAtSetPose() { void SwerveDrive::ResetPose(frc::Pose2d pose) { _poseEstimator.ResetPosition( _config.gyro->GetRotation2d(), - wpi::array{ - _modules[0].GetPosition(), _modules[1].GetPosition(), - _modules[2].GetPosition(), _modules[3].GetPosition()}, + wpi::array{_modules[0].GetPosition(), _modules[1].GetPosition(), + _modules[2].GetPosition(), _modules[3].GetPosition()}, pose); } @@ -547,9 +510,8 @@ frc::Pose2d SwerveDrive::GetPose() { return _poseEstimator.GetEstimatedPosition(); } -void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, - units::second_t timestamp) { +void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) { _poseEstimator.AddVisionMeasurement(pose, timestamp); } } // namespace drivetrain -} // namespace wom \ No newline at end of file +} // namespace wom diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp index 9479dfe0..58782b81 100644 --- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp +++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp @@ -72,45 +72,41 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) { // if (isZero) { // _swerveDrivebase->SetZeroing(); // } else { - double xVelocity = wom::utils::spow2(-wom::utils::deadzone( - _driverController->GetLeftY(), - driverDeadzone)); // GetLeftY due to x being where y should be on field - double yVelocity = wom::utils::spow2( - -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone)); - - double r_x = wom::utils::spow2( - -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone)); - - double turnX = _driverController->GetRightX(); - double turnY = _driverController->GetRightY(); - double num = std::sqrt(turnX * turnX + turnY * turnY); - if (num < turningDeadzone) { - turnX = 0; - turnY = 0; - } - - // if (isRotateMatch) { - // units::degree_t currentAngle = - // _swerveDrivebase->GetPose().Rotation().Degrees(); - // CalculateRequestedAngle(turnX, turnY, currentAngle); - // _swerveDriveTable->GetEntry("RotateMatch") - // .SetDouble(_requestedAngle.value()); - // _swerveDrivebase->RotateMatchJoystick( - // _requestedAngle, - // wom::drivetrain::FieldRelativeSpeeds{// also field relative - // xVelocity * maxMovementMagnitude, - // yVelocity * maxMovementMagnitude, - // r_x * maxRotationMagnitude}); - // } else { - _swerveDrivebase->SetFieldRelativeVelocity( - wom::drivetrain::FieldRelativeSpeeds{xVelocity * -maxMovementMagnitude, - yVelocity * -maxMovementMagnitude, - r_x * maxRotationMagnitude}); - - // _swerveDrivebase->SetVelocity( - // frc::ChassisSpeeds{xVelocity * maxMovementMagnitude, - // yVelocity * maxMovementMagnitude, - // r_x * maxRotationMagnitude}); + double xVelocity = wom::utils::spow2( + -wom::utils::deadzone(_driverController->GetLeftY(), + driverDeadzone)); // GetLeftY due to x being where y should be on field + double yVelocity = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone)); + + double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone)); + + double turnX = _driverController->GetRightX(); + double turnY = _driverController->GetRightY(); + double num = std::sqrt(turnX * turnX + turnY * turnY); + if (num < turningDeadzone) { + turnX = 0; + turnY = 0; + } + + // if (isRotateMatch) { + // units::degree_t currentAngle = + // _swerveDrivebase->GetPose().Rotation().Degrees(); + // CalculateRequestedAngle(turnX, turnY, currentAngle); + // _swerveDriveTable->GetEntry("RotateMatch") + // .SetDouble(_requestedAngle.value()); + // _swerveDrivebase->RotateMatchJoystick( + // _requestedAngle, + // wom::drivetrain::FieldRelativeSpeeds{// also field relative + // xVelocity * maxMovementMagnitude, + // yVelocity * maxMovementMagnitude, + // r_x * maxRotationMagnitude}); + // } else { + _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{ + xVelocity * -maxMovementMagnitude, yVelocity * -maxMovementMagnitude, r_x * maxRotationMagnitude}); + + // _swerveDrivebase->SetVelocity( + // frc::ChassisSpeeds{xVelocity * maxMovementMagnitude, + // yVelocity * maxMovementMagnitude, + // r_x * maxRotationMagnitude}); // } // } // _swerveDrivebase->SetIndividualTuning(2, 0_deg, 0_mps); @@ -121,8 +117,7 @@ void ManualDrivebase::ResetMode() { resetMode = false; } -void ManualDrivebase::CalculateRequestedAngle(double joystickX, - double joystickY, +void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle) { _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg; if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) { @@ -131,8 +126,7 @@ void ManualDrivebase::CalculateRequestedAngle(double joystickX, } // Code for x-ing the wheels on the drivebase -XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) - : _swerveDrivebase(swerveDrivebase) { +XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) { Controls(swerveDrivebase); } void XDrivebase::OnTick(units::second_t deltaTime) { @@ -174,20 +168,17 @@ void XDrivebase::OnTick(units::second_t deltaTime) { // m_timer.Start(); // } -wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive( - frc::Timer* timer, frc::Field2d* field) +wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field) : m_timer(timer), m_field(field) {} void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() { m_field->SetRobotPose(m_driveSim.GetPose()); // get the current trajectory state - frc::Trajectory::State desired_state = - current_trajectory.Sample(m_timer->Get()); + frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get()); // get the current wheel speeds - wom::utils::WriteTrajectoryState(current_trajectory_state_table, - desired_state); + wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state); // move drivebase position to the desired state m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state)); @@ -205,19 +196,16 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() { return m_driveSim.GetPose(); } -void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath( - std::string path) { +void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) { nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); std::shared_ptr table = inst.GetTable("FMSInfo"); // create a netowrk table for the trajectory std::shared_ptr trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path"); - current_trajectory_table = - nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory"); + current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory"); current_trajectory_state_table = - nt::NetworkTableInstance::GetDefault().GetTable( - "current_trajectory_state"); + nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state"); current_trajectory = m_pathplanner.getTrajectory(path); m_driveSim.SetPose(current_trajectory.Sample(0_s).pose); @@ -225,12 +213,10 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath( m_timer->Start(); } -wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive( - wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, - frc::Field2d* field) +wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, + frc::Timer* timer, frc::Field2d* field) : _swerve(swerve), m_timer(timer), m_field(field) { - _simSwerveDrive = - new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field); + _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field); } void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() { diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp deleted file mode 100644 index 3406be88..00000000 --- a/wombat/src/main/cpp/subsystems/Arm.cpp +++ /dev/null @@ -1,192 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#include "subsystems/Arm.h" - -#include - -using namespace frc; -using namespace wom; - -// creates network table instatnce on shuffleboard -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()); - table->GetEntry("minAngle") - .SetDouble(minAngle.convert().value()); - table->GetEntry("maxAngle") - .SetDouble(maxAngle.convert().value()); - table->GetEntry("initialAngle") - .SetDouble(initialAngle.convert().value()); - table->GetEntry("angleOffset") - .SetDouble(initialAngle.convert().value()); -} - -// arm config is used -wom::subsystems::Arm::Arm(ArmConfig config) - : _config(config), - _pid(config.path + "/pid", config.pidConfig), - _velocityPID(config.path + "/velocityPID", config.velocityConfig), - _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {} - -// the loop that allows the information to be used -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(); - - // sets usable infromation for each state - switch (_state) { - case ArmState::kIdle: - break; - case ArmState::kVelocity: { - units::newton_meter_t torque = - 9.81_m / 1_s / 1_s * _config.armLength * - units::math::cos(angle + _config.angleOffset) * - (0.5 * _config.armMass + _config.loadMass); - // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, - // 0_rad/1_s); - units::volt_t feedforward = - _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint()); - // feedforward = 3.5_V; - // std::cout << "feedforward" << feedforward.value() << std::endl; - voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward); - // std::cout << "arm velocity voltage is: " << voltage.value() << - // std::endl; voltage = 0_V; - } break; - case ArmState::kAngle: { - units::newton_meter_t torque = - 9.81_m / 1_s / 1_s * _config.armLength * - units::math::cos(angle + _config.angleOffset) * - (0.5 * _config.armMass + _config.loadMass); - units::volt_t feedforward = - _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s); - // std::cout << "feedforward" << feedforward.value() << std::endl; - voltage = _pid.Calculate(angle, dt, feedforward); - } break; - case ArmState::kRaw: - voltage = _voltage; - break; - } - - voltage *= armLimit; - - // units::newton_meter_t torqueLimit = 10_kg * 1.4_m * 6_mps_sq; - // units::volt_t voltageMax = _config.leftGearbox.motor.Voltage(torqueLimit, - // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); units::volt_t - // voltageMin = _config.leftGearbox.motor.Voltage(-torqueLimit, - // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); - - // voltage = units::math::max(units::math::min(voltage, voltageMax), - // voltageMin); - units::volt_t voltageMin = -5.5_V; - units::volt_t voltageMax = 5.5_V; - voltage = units::math::max(units::math::min(voltage, voltageMax), voltageMin); - - // std::cout << "voltage: " << voltage.value() << std::endl; - - // _config.leftGearbox.motorController->SetVoltage(voltage); - // _config.rightGearbox.motorController->SetVoltage(voltage); - - // creates network table instances for the angle and config of the arm - _table->GetEntry("angle").SetDouble(angle.convert().value()); - _config.WriteNT(_table->GetSubTable("config")); -} - -void wom::subsystems::Arm::SetArmSpeedLimit(double limit) { - armLimit = limit; -} - -// defines information needed for the functions and connects the states to their -// respective function - -void wom::subsystems::Arm::SetRaw(units::volt_t voltage) { - _voltage = voltage; -} - -void wom::subsystems::Arm::SetVelocity(units::radians_per_second_t velocity) { - _velocityPID.SetSetpoint(velocity); -} - -void wom::subsystems::Arm::SetAngle(units::radian_t angle) { - _pid.SetSetpoint(angle); -} - -void wom::subsystems::Arm::SetState(wom::subsystems::ArmState state) { - _state = state; -} - -wom::subsystems::ArmConfig& wom::subsystems::Arm::GetConfig() { - return _config; -} - -units::radian_t wom::subsystems::Arm::GetAngle() const { - return _config.armEncoder.GetPosition() / 100 * 360 * 1_deg; -} - -units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const { - return _config.leftGearbox.motor.Speed(0_Nm, 12_V); -} - -units::radians_per_second_t wom::subsystems::Arm::GetArmVelocity() const { - return _config.armEncoder.GetVelocity() / 100 * 360 * 1_deg / 60_s; -} - -bool wom::subsystems::Arm::IsStable() const { - return _pid.IsStable(5_deg); -} - -/* SIMULATION */ -// #include - -// ::wom::sim::ArmSim::ArmSim(ArmConfig config) -// : config(config), -// angle(config.initialAngle), -// encoder(config.gearbox.encoder->MakeSimEncoder()), -// lowerLimit(config.lowerLimitSwitch ? new -// frc::sim::DIOSim(*config.lowerLimitSwitch) : nullptr), -// upperLimit(config.upperLimitSwitch ? new -// frc::sim::DIOSim(*config.upperLimitSwitch) : nullptr), -// table(nt::NetworkTableInstance::GetDefault().GetTable(config.path + -// "/sim")) -// {} - -// units::ampere_t wom::sim::ArmSim::GetCurrent() const { -// return current; -// } - -// void ::wom::sim::ArmSim::Update(units::second_t dt) { -// torque = (config.loadMass * config.armLength + config.armMass * -// config.armLength / 2.0) * 9.81_m / 1_s / 1_s * -// units::math::cos(config.angleOffset + angle) + additionalTorque; velocity = -// config.gearbox.motor.Speed(torque, -// config.gearbox.transmission->GetEstimatedRealVoltage()); angle += velocity -// * dt; - -// if (angle <= config.minAngle) { -// angle = config.minAngle; -// velocity = 0_rad / 1_s; -// if (lowerLimit) lowerLimit->SetValue(true); -// } else { -// if (lowerLimit) lowerLimit->SetValue(false); -// } - -// if (angle >= config.maxAngle) { -// angle = config.maxAngle; -// velocity = 0_rad / 1_s; -// if (upperLimit) upperLimit->SetValue(true); -// } else { -// if (upperLimit) upperLimit->SetValue(false); -// } - -// current = config.gearbox.motor.Current(velocity, -// config.gearbox.transmission->GetEstimatedRealVoltage()); - -// if (encoder) encoder->SetEncoderTurns(angle - config.initialAngle); - -// table->GetEntry("angle").SetDouble(angle.convert().value()); -// table->GetEntry("current").SetDouble(current.value()); -// } diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp deleted file mode 100644 index cc840302..00000000 --- a/wombat/src/main/cpp/subsystems/Elevator.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#include "subsystems/Elevator.h" - -#include -#include - -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()); -} - -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); -} - -void wom::subsystems::Elevator::OnUpdate(units::second_t dt) { - units::volt_t voltage{0}; - - units::meter_t height = GetElevatorEncoderPos() * 1_m; - - switch (_state) { - case ElevatorState::kIdle: - voltage = 0_V; - break; - case ElevatorState::kManual: - voltage = _setpointManual; - break; - case ElevatorState::kVelocity: { - units::volt_t feedforward = _config.rightGearbox.motor.Voltage( - (_config.mass * 9.81_mps_sq) * _config.radius, - _velocityPID.GetSetpoint() / - (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad); - // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm, - // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) - // * 1_rad); - feedforward = 1.2_V; - voltage = _velocityPID.Calculate(GetElevatorVelocity(), dt, feedforward); - if (voltage > 6_V) { - voltage = 6_V; - } - std::cout << "elevator feedforward: " << feedforward.value() << std::endl; - // voltage = 0_V; - } break; - case ElevatorState::kPID: { - units::volt_t feedforward = _config.rightGearbox.motor.Voltage( - (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s); - // std::cout << "feed forward" << feedforward.value() << std::endl; - feedforward = 1.2_V; - // voltage = _pid.Calculate(height, dt, feedforward); - voltage = _pid.Calculate(height, dt, feedforward); - if (voltage > 6_V) { - voltage = 6_V; - } - } break; - } - - // Top Sensor Detector - // if(_config.topSensor != nullptr) { - // if(_config.topSensor->Get()) { - // _config.leftGearbox.encoder->SetEncoderPosition(_config.maxHeight / - // _config.radius * 1_rad); - // //voltage = 0_V; - // } - // } - - // //Bottom Sensor Detection - // if (_config.bottomSensor != nullptr) { - // if (_config.bottomSensor->Get()) { - // _config.leftGearbox.encoder->SetEncoderPosition(_config.minHeight / - // _config.radius * 1_rad); - // //voltage = 0_V; - // } - // } - - // std::cout << "elevator: " << elevator.height - - // Set voltage to motors... - voltage *= speedLimit; - // _config.leftGearbox.motorController->SetVoltage(voltage); - // _config.rightGearbox.motorController->SetVoltage(voltage); -} - -void wom::subsystems::Elevator::SetManual(units::volt_t voltage) { - _setpointManual = voltage; -} - -void wom::subsystems::Elevator::SetPID(units::meter_t height) { - _pid.SetSetpoint(height); -} - -void wom::subsystems::Elevator::SetVelocity( - units::meters_per_second_t velocity) { - _velocityPID.SetSetpoint(velocity); -} - -void wom::subsystems::Elevator::SetState(wom::subsystems::ElevatorState state) { - _state = state; -} - -void wom::subsystems::Elevator::SetElevatorSpeedLimit(double limit) { - speedLimit = limit; -} - -wom::subsystems::ElevatorConfig& wom::subsystems::Elevator::GetConfig() { - return _config; -} - -bool wom::subsystems::Elevator::IsStable() const { - return _pid.IsStable(); -} - -wom::subsystems::ElevatorState wom::subsystems::Elevator::GetState() const { - return _state; -} - -double wom::subsystems::Elevator::GetElevatorEncoderPos() { - return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225; -} - -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 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 wom::subsystems::Elevator::MaxSpeed() const { - return _config.leftGearbox.motor.Speed( - (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / - 1_rad * _config.radius; -} - */ \ No newline at end of file diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp deleted file mode 100644 index aa2fe679..00000000 --- a/wombat/src/main/cpp/subsystems/Shooter.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#include "subsystems/Shooter.h" - -#include - -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 wom::subsystems::Shooter::OnUpdate(units::second_t dt) { - units::volt_t voltage{0}; - units::revolutions_per_minute_t currentSpeed = - _params.gearbox.encoder->GetEncoderAngularVelocity(); - - switch (_state) { - case ShooterState::kManual: - voltage = _setpointManual; - break; - case ShooterState::kPID: { - auto feedforward = - _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint()); - voltage = _pid.Calculate(currentSpeed, dt, feedforward); - } break; - case ShooterState::kIdle: - voltage = 0_V; - break; - } - - units::newton_meter_t max_torque_at_current_limit = - _params.gearbox.motor.Torque(_params.currentLimit); - units::volt_t max_voltage_for_current_limit = - _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed); - - voltage = - 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value()); - - // _params.gearbox.motorController->SetVoltage(voltage); - - _table->GetEntry("output_volts").SetDouble(voltage.value()); - _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value()); - _table->GetEntry("setpoint_rpm") - .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value()); - _table->GetEntry("stable").SetBoolean(_pid.IsStable()); -} - -void wom::subsystems::Shooter::SetManual(units::volt_t voltage) { - _setpointManual = voltage; -} - -void wom::subsystems::Shooter::SetPID(units::radians_per_second_t goal) { - _state = ShooterState::kPID; - _pid.SetSetpoint(goal); -} - -void wom::subsystems::Shooter::SetState(ShooterState state) { - _state = state; -} - -bool wom::subsystems::Shooter::IsStable() const { - return _pid.IsStable(); -} - -// Shooter Manual Set - -wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, - units::volt_t setpoint) - : _shooter(s), _setpoint(setpoint) { - Controls(_shooter); -} - -void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) { - _shooter->SetManual(_setpoint); -} - -// ShooterSpinup - -wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, - units::radians_per_second_t speed, - bool hold) - : _shooter(s), _speed(speed), _hold(hold) { - Controls(_shooter); -} - -void wom::subsystems::ShooterSpinup::OnTick(units::second_t dt) { - _shooter->SetPID(_speed); - - if (!_hold && _shooter->IsStable()) - SetDone(); -} - */ \ No newline at end of file diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp index 40b37b74..1120b399 100644 --- a/wombat/src/main/cpp/utils/Encoder.cpp +++ b/wombat/src/main/cpp/utils/Encoder.cpp @@ -6,8 +6,8 @@ #include -wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, - units::meter_t wheelRadius, double reduction) +wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, + double reduction) : _reduction(reduction), _encoderTicksPerRotation(encoderTicksPerRotation), _type(type), @@ -32,8 +32,8 @@ void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) { // _offset = -offset_turns; } -void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { //HERE! - _offset = offset; +void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { // HERE! + _offset = offset; // units::turn_t offset_turns = offset; // _offset = offset_turns.value() * GetEncoderTicksPerRotation(); } @@ -43,16 +43,16 @@ void wom::utils::Encoder::SetReduction(double reduction) { } units::radian_t wom::utils::Encoder::GetEncoderPosition() { - //if (_type == 0) { - // units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()}; - // return n_turns; - //} else if (_type == 2) { - // units::degree_t pos = GetEncoderTicks() * 1_deg; - // return pos; - //} else { - // units::degree_t pos = GetEncoderTicks() * 1_deg; - // return pos - _offset; - //} + // if (_type == 0) { + // units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()}; + // return n_turns; + // } else if (_type == 2) { + // units::degree_t pos = GetEncoderTicks() * 1_deg; + // return pos; + // } else { + // units::degree_t pos = GetEncoderTicks() * 1_deg; + // return pos - _offset; + // } return GetEncoderTicks() * 1_rad; } @@ -63,8 +63,7 @@ double wom::utils::Encoder::GetEncoderDistance() { 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()}; + units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()}; return n_turns_per_s; } @@ -86,12 +85,10 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const { return _nativeEncoder.GetRate(); } -wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, - units::meter_t wheelRadius, +wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction) : wom::utils::Encoder(42, reduction, wheelRadius, 2), - _encoder(controller->GetEncoder( - rev::SparkRelativeEncoder::Type::kHallSensor)) {} + _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {} double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const { return ((_encoder.GetPosition() * 2 * 3.1415) / 200); @@ -121,11 +118,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const { return _encoder.GetVelocity(); } -wom::utils::TalonFXEncoder::TalonFXEncoder( - ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius, - double reduction) - : utils::Encoder(2048, reduction, wheelRadius, 0), - _controller(controller) {} +wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, + units::meter_t wheelRadius, double reduction) + : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {} double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const { return _controller->GetPosition().GetValue().value(); @@ -135,12 +130,9 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const { return _controller->Get() * 10; } -wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, - units::meter_t wheelRadius, - double ticksPerRotation, - double reduction) - : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), - _dutyCycleEncoder(channel) {} +wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius, + double ticksPerRotation, double reduction) + : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {} double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const { return _dutyCycleEncoder.Get().value(); @@ -150,13 +142,11 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const { return 0; } -wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, - double ticksPerRotation, double reduction, - std::string name) +wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation, + double reduction, std::string name) : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) { _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name); // _canEncoder->ConfigAbsoluteEncoderRange(0, 1); - } double wom::utils::CanEncoder::GetEncoderRawTicks() const { @@ -165,4 +155,4 @@ double wom::utils::CanEncoder::GetEncoderRawTicks() const { double wom::utils::CanEncoder::GetEncoderTickVelocity() const { return _canEncoder->GetVelocity().GetValue().value(); -} \ No newline at end of file +} diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h index c1b356a9..f0037344 100644 --- a/wombat/src/main/include/Wombat.h +++ b/wombat/src/main/include/Wombat.h @@ -7,12 +7,8 @@ #include "behaviour/Behaviour.h" #include "behaviour/BehaviourScheduler.h" #include "behaviour/HasBehaviour.h" -#include "drivetrain/Drivetrain.h" #include "drivetrain/SwerveDrive.h" #include "drivetrain/behaviours/SwerveBehaviours.h" -#include "subsystems/Arm.h" -#include "subsystems/Elevator.h" -#include "subsystems/Shooter.h" #include "utils/Encoder.h" #include "utils/Gearbox.h" #include "utils/PID.h" @@ -23,7 +19,6 @@ namespace wom { using namespace wom; using namespace wom::utils; -// using namespace wom::subsystems; using namespace wom::drivetrain; using namespace wom::drivetrain::behaviours; using namespace wom::vision; diff --git a/wombat/src/main/include/drivetrain/Drivetrain.h b/wombat/src/main/include/drivetrain/Drivetrain.h deleted file mode 100644 index b3894217..00000000 --- a/wombat/src/main/include/drivetrain/Drivetrain.h +++ /dev/null @@ -1,58 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#pragma once - -#include -#include -#include - -#include - -#include "behaviour/HasBehaviour.h" -#include "utils/Gearbox.h" - -namespace wom { -namespace drivetrain { -// TODO PID -struct DrivetrainConfig { - std::string path; - - 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 { - kIdle, - kTank, - kAuto, -}; - -class Drivetrain : public behaviour::HasBehaviour { - public: - Drivetrain(DrivetrainConfig* config, frc::XboxController& driver); - ~Drivetrain(); - - DrivetrainConfig* GetConfig(); - DrivetrainState GetState(); - - void SetState(DrivetrainState state); - - void OnStart(); - void OnUpdate(units::second_t dt); - - protected: - private: - DrivetrainConfig* _config; - DrivetrainState _state; - frc::XboxController& _driver; - units::volt_t maxVolts = 9_V; -}; -} // namespace drivetrain -} // namespace wom - */ \ No newline at end of file diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h index 625ef135..d8b8e482 100644 --- a/wombat/src/main/include/drivetrain/SwerveDrive.h +++ b/wombat/src/main/include/drivetrain/SwerveDrive.h @@ -4,9 +4,9 @@ #pragma once +#include #include #include -#include #include #include #include @@ -16,7 +16,12 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include @@ -28,19 +33,9 @@ #include "utils/Gearbox.h" #include "utils/PID.h" -#include -#include - -#include -#include -#include - - namespace wom { namespace drivetrain { - - enum class SwerveModuleState { kZeroing, kIdle, kPID }; enum class TurnOffsetValues { reverse, forward, none }; @@ -59,9 +54,8 @@ struct SwerveModuleConfig { class SwerveModule { public: - //using angle_pid_conf_t = utils::PIDConfig; - using velocity_pid_conf_t = - utils::PIDConfig; + // using angle_pid_conf_t = utils::PIDConfig; + using velocity_pid_conf_t = utils::PIDConfig; SwerveModule(std::string path, SwerveModuleConfig config, /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID); @@ -78,8 +72,7 @@ class SwerveModule { void SetZero(units::second_t dt); void SetIdle(); - void SetPID(units::radian_t angle, units::meters_per_second_t speed, - units::second_t dt); + void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt); void SetZero(); void SetVoltageLimit(units::volt_t driveModuleVoltageLimit); @@ -101,7 +94,7 @@ class SwerveModule { const SwerveModuleConfig& GetConfig() const; - //utils::PIDController _anglePIDController; + // utils::PIDController _anglePIDController; frc::PIDController _anglePIDController; private: @@ -127,20 +120,18 @@ class SwerveModule { struct SwerveDriveConfig { /*using pose_angle_conf_t = utils::PIDConfig;*/ - using pose_position_conf_t = - utils::PIDConfig; - using balance_conf_t = - utils::PIDConfig; + using pose_position_conf_t = utils::PIDConfig; + using balance_conf_t = utils::PIDConfig; std::string path; - //SwerveModule::angle_pid_conf_t anglePID; + // SwerveModule::angle_pid_conf_t anglePID; SwerveModule::velocity_pid_conf_t velocityPID; wpi::array modules; ctre::phoenix6::hardware::Pigeon2* gyro; - //pose_angle_conf_t poseAnglePID; + // pose_angle_conf_t poseAnglePID; pose_position_conf_t posePositionPID; units::kilogram_t mass; @@ -194,8 +185,7 @@ class SwerveDrive : public behaviour::HasBehaviour { * @brief This function switches the state to handle the robot's rotation * matching that of the joystick */ - void RotateMatchJoystick(units::radian_t joystickAngle, - FieldRelativeSpeeds speeds); + void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds); void SetIdle(); @@ -205,8 +195,7 @@ class SwerveDrive : public behaviour::HasBehaviour { void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds); void SetPose(frc::Pose2d pose); bool IsAtSetPose(); - void SetIndividualTuning(int mod, units::radian_t angle, - units::meters_per_second_t speed); + void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed); void SetTuning(units::radian_t angle, units::meters_per_second_t speed); void SetZero(); void SetVoltageLimit(units::volt_t driveVoltageLimit); @@ -262,4 +251,4 @@ class SwerveDrive : public behaviour::HasBehaviour { // double backRightEncoderOffset = -119.619140625; }; } // namespace drivetrain -} // namespace wom \ No newline at end of file +} // namespace wom diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h index d49d3f12..d9f844a5 100644 --- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h +++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h @@ -37,16 +37,14 @@ class ManualDrivebase : public behaviour::Behaviour { * A pointer to the controller that the driver has been allocated (the * allocated memory address that stores the "driver controller" object) */ - ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, - frc::XboxController* driverController); + ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController); void OnTick(units::second_t deltaTime) override; /** * @brief This function handles all of the logic behind the tangent function, * to be able to calculate an angle between 0 andd 360 degrees, inclusively */ - void CalculateRequestedAngle(double joystickX, double joystickY, - units::degree_t defaultAngle); + void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle); void OnStart() override; void ResetMode(); @@ -72,8 +70,8 @@ class ManualDrivebase : public behaviour::Behaviour { const double turningDeadzone = 0.2; // Variables for solution to Anti-tip - double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, - usingJoystickXPos, usingJoystickYPos; + double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos, + usingJoystickYPos; // The speed that the joystick must travel to activate averaging over previous // 3 joystick positions const double smoothingThreshold = 1; @@ -126,8 +124,8 @@ class GoToPose : public behaviour::Behaviour { class FollowTrajectory : public behaviour::Behaviour { public: - FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, - wom::utils::Pathplanner* pathplanner, std::string path); + FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner, + std::string path); void OnTick(units::second_t dt) override; @@ -184,8 +182,7 @@ class TempSimSwerveDrive { class AutoSwerveDrive { public: - AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, - frc::Field2d* field); + AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field); void OnUpdate(); @@ -237,4 +234,4 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour { }; } // namespace behaviours } // namespace drivetrain -} // namespace wom \ No newline at end of file +} // namespace wom diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h deleted file mode 100644 index 62c2d05b..00000000 --- a/wombat/src/main/include/subsystems/Arm.h +++ /dev/null @@ -1,82 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -#include "behaviour/HasBehaviour.h" -#include "utils/Gearbox.h" -#include "utils/PID.h" - -namespace wom { -namespace subsystems { -struct ArmConfig { - std::string path; - - wom::utils::Gearbox leftGearbox; - wom::utils::Gearbox rightGearbox; - wom::utils::CANSparkMaxEncoder armEncoder; - wom::utils::PIDConfig pidConfig; - wom::utils::PIDConfig velocityConfig; - - units::kilogram_t armMass; - units::kilogram_t loadMass; - units::meter_t armLength; - units::radian_t minAngle = 0_deg; - units::radian_t maxAngle = 180_deg; - units::radian_t initialAngle = 0_deg; - units::radian_t angleOffset = 0_deg; - - void WriteNT(std::shared_ptr table); -}; - -enum class ArmState { kIdle, kAngle, kRaw, kVelocity }; - -class Arm : public behaviour::HasBehaviour { - public: - explicit Arm(ArmConfig config); - - void OnUpdate(units::second_t dt); - - void SetIdle(); - void SetAngle(units::radian_t angle); - void SetRaw(units::volt_t voltage); - void SetVelocity(units::radians_per_second_t velocity); - - void SetState(wom::subsystems::ArmState state); - - 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::utils::PIDController _pid; - wom::utils::PIDController _velocityPID; - - std::shared_ptr _table; - - double armLimit = 0.4; - units::radians_per_second_t lastVelocity; - - units::volt_t _voltage{0}; -}; -} // namespace subsystems -} // namespace wom - */ \ No newline at end of file diff --git a/wombat/src/main/include/subsystems/Elevator.h b/wombat/src/main/include/subsystems/Elevator.h deleted file mode 100644 index ccbe2379..00000000 --- a/wombat/src/main/include/subsystems/Elevator.h +++ /dev/null @@ -1,85 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "behaviour/HasBehaviour.h" -#include "utils/Encoder.h" -#include "utils/Gearbox.h" -#include "utils/PID.h" - -namespace wom { -namespace subsystems { -enum class ElevatorState { kIdle, kPID, kManual, kVelocity }; - -struct ElevatorConfig { - std::string path; - wom::utils::Gearbox leftGearbox; - wom::utils::Gearbox rightGearbox; - wom::utils::CANSparkMaxEncoder elevatorEncoder; - frc::DigitalInput* topSensor; - frc::DigitalInput* bottomSensor; - units::meter_t radius; - units::kilogram_t mass; - units::meter_t maxHeight; - units::meter_t minHeight; - units::meter_t initialHeight; - wom::utils::PIDConfig pid; - wom::utils::PIDConfig velocityPID; - - void WriteNT(std::shared_ptr table); -}; - -class Elevator : public behaviour::HasBehaviour { - public: - explicit Elevator(ElevatorConfig params); - - void OnUpdate(units::second_t dt); - - void SetManual(units::volt_t voltage); - void SetPID(units::meter_t height); - - void SetState(ElevatorState state); - - void SetVelocity(units::meters_per_second_t velocity); - - double GetElevatorEncoderPos(); - 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}; - - ElevatorConfig _config; - ElevatorState _state; - double speedLimit = 0.5; - - units::meters_per_second_t _velocity; - - wom::utils::PIDController _pid; - wom::utils::PIDController _velocityPID; - - std::shared_ptr _table; -}; -} // namespace subsystems -} // namespace wom - */ \ No newline at end of file diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h deleted file mode 100644 index 694a6d57..00000000 --- a/wombat/src/main/include/subsystems/Shooter.h +++ /dev/null @@ -1,76 +0,0 @@ -/* // Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#pragma once - -#include -#include -#include - -#include -#include - -#include "behaviour/Behaviour.h" -#include "behaviour/HasBehaviour.h" -#include "utils/Gearbox.h" -#include "utils/PID.h" - -namespace wom { -namespace subsystems { -enum class ShooterState { kPID, kManual, kIdle }; - -struct ShooterParams { - wom::utils::Gearbox gearbox; - wom::utils::PIDConfig pid; - units::ampere_t currentLimit; -}; - -class Shooter : public behaviour::HasBehaviour { - public: - Shooter(std::string path, ShooterParams params); - - void SetManual(units::volt_t voltage); - void SetPID(units::radians_per_second_t goal); - void SetState(ShooterState state); - - void OnUpdate(units::second_t dt); - - bool IsStable() const; - - private: - ShooterParams _params; - ShooterState _state; - - units::volt_t _setpointManual{0}; - - wom::utils::PIDController _pid; - - std::shared_ptr _table; -}; - -class ShooterConstant : public behaviour::Behaviour { - public: - ShooterConstant(Shooter* s, units::volt_t setpoint); - - void OnTick(units::second_t dt) override; - - private: - Shooter* _shooter; - units::volt_t _setpoint; -}; - -class ShooterSpinup : public behaviour::Behaviour { - public: - ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false); - - void OnTick(units::second_t dt) override; - - private: - Shooter* _shooter; - units::radians_per_second_t _speed; - bool _hold; -}; -} // namespace subsystems -} // namespace wom - */ \ No newline at end of file diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h index 073ba6cd..b9aae2a4 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -20,8 +20,7 @@ namespace wom { namespace utils { class Encoder { public: - Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, - double reduction = 1.0); + Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0); virtual double GetEncoderRawTicks() const = 0; virtual double GetEncoderTickVelocity() const = 0; // ticks/s @@ -45,7 +44,8 @@ class Encoder { virtual double GetVelocity() const = 0; double GetVelocityValue() const; - units::radian_t _offset = 0_rad; //bad + units::radian_t _offset = 0_rad; // bad + private: double _encoderTicksPerRotation; int _type = 0; @@ -54,24 +54,23 @@ class Encoder { class DigitalEncoder : public Encoder { public: - DigitalEncoder(int channelA, int channelB, double ticksPerRotation, - units::meter_t wheelRadius, double reduction = 1) - : Encoder(ticksPerRotation, reduction, wheelRadius, 0), - _nativeEncoder(channelA, channelB) {} + DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius, + double reduction = 1) + : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {} double GetEncoderRawTicks() const override; double GetEncoderTickVelocity() const override; double GetPosition() const; double GetVelocity() const override; + private: frc::Encoder _nativeEncoder; }; class CANSparkMaxEncoder : public Encoder { public: - explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, - units::meter_t wheelRadius, double reduction = 1); + explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1); double GetEncoderRawTicks() const override; double GetEncoderTickVelocity() const override; @@ -86,8 +85,8 @@ class CANSparkMaxEncoder : public Encoder { class TalonFXEncoder : public Encoder { public: - TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, - units::meter_t wheelRadius, double reduction = 1); + TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius, + double reduction = 1); double GetEncoderRawTicks() const override; double GetEncoderTickVelocity() const override; @@ -99,8 +98,8 @@ class TalonFXEncoder : public Encoder { class DutyCycleEncoder : public Encoder { public: - DutyCycleEncoder(int channel, units::meter_t wheelRadius, - double ticksPerRotation = 1, double reduction = 1); + DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1, + double reduction = 1); double GetEncoderRawTicks() const override; double GetEncoderTickVelocity() const override; @@ -112,9 +111,8 @@ class DutyCycleEncoder : public Encoder { class CanEncoder : public Encoder { public: - CanEncoder(int deviceNumber, units::meter_t wheelRadius, - double ticksPerRotation = 4095, double reduction = 6.75, - std::string name = "Drivebase"); + CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095, + double reduction = 6.75, std::string name = "Drivebase"); double GetEncoderRawTicks() const override; double GetEncoderTickVelocity() const override; @@ -127,4 +125,4 @@ class CanEncoder : public Encoder { ctre::phoenix6::hardware::CANcoder* _canEncoder; }; } // namespace utils -} // namespace wom \ No newline at end of file +} // namespace wom