From af62b5dfe109697b5a5a43f7711fc4699e1f9d9b Mon Sep 17 00:00:00 2001 From: JoystickMaster-race <97269195+JoystickMaster-race@users.noreply.github.com> Date: Thu, 29 Feb 2024 20:32:35 +0800 Subject: [PATCH] Toby Beta (#151) * setpoints * Arm Good PID States * Cleaned up PID arm * Finished Arm SetStates --- src/main/cpp/AlphaArm.cpp | 117 ++++++++++++------ src/main/cpp/AlphaArmBehaviour.cpp | 33 +++-- src/main/cpp/Robot.cpp | 153 +++++++++++++++++++----- src/main/cpp/Vision.cpp | 12 ++ src/main/include/AlphaArm.h | 48 ++++++-- src/main/include/AlphaArmBehaviour.h | 7 +- src/main/include/ArmAngle.h | 84 +++++++++++++ src/main/include/Robot.h | 38 ++++-- src/main/include/RobotMap.h | 133 +++++++++++++++++++- src/main/include/Vision.h | 111 +++++++++++++++++ wombat/src/main/cpp/utils/Encoder.cpp | 36 +++--- wombat/src/main/include/utils/Encoder.h | 4 +- 12 files changed, 649 insertions(+), 127 deletions(-) create mode 100644 src/main/cpp/Vision.cpp create mode 100644 src/main/include/ArmAngle.h create mode 100644 src/main/include/Vision.h diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp index 848ee616..f10824e7 100644 --- a/src/main/cpp/AlphaArm.cpp +++ b/src/main/cpp/AlphaArm.cpp @@ -4,63 +4,106 @@ #include "AlphaArm.h" -AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {} -AlphaArmConfig AlphaArm::GetConfig() { - return _config; +AlphaArm::AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)} +{ + +} + +void AlphaArm::OnStart(){ + _pidArmStates.Reset(); + _pidIntakeState.Reset(); + } void AlphaArm::OnUpdate(units::second_t dt) { switch (_state) { case AlphaArmState::kIdle: - - // _config.alphaArmGearbox.motorController->SetVoltage(0_V); - // _config.wristGearbox.motorController->SetVoltage(0_V); + _setAlphaArmVoltage = 0_V; - _setWristVoltage = 0_V; - break; case AlphaArmState::kRaw: - _setAlphaArmVoltage = _rawArmVoltage; - _setWristVoltage = _rawWristVoltage; - _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage); - _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage); - + std::cout << "RawControl" << std::endl; + _setAlphaArmVoltage = _rawArmVoltage; + break; - case AlphaArmState::kForwardWrist: - _config.wristGearbox.motorController->SetVoltage(3_V); - _setWristVoltage = 3_V; - - case AlphaArmState::kReverseWrist: - _config.wristGearbox.motorController->SetVoltage(-3_V); - _setWristVoltage = -3_V; - default: - std::cout << "oops, wrong state" << std::endl; + + case AlphaArmState::kHoldAngle: + { + _pidArm.SetSetpoint(-_goal); + //_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition()); + _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())}; + } + break; + case AlphaArmState::kAmpAngle: + std::cout << "Amp Angle" << std::endl; + + _pidArmStates.SetSetpoint(-2.17); + _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())}; + + break; + + case AlphaArmState::kIntakeAngle: + std::cout << "Intake Angle" << std::endl; + _pidIntakeState.SetSetpoint(-0.48); //-0.48 + _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())}; + break; + + case AlphaArmState::kSpeakerAngle: + std::cout << "Speaker Angle" << std::endl; + _pidArmStates.SetSetpoint(-0.82); + _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())}; + + break; + + // case AlphaArmState::kVisionAngle: + // std::cout << "Vision Angle" << std::endl; + + // break; + + case AlphaArmState::kStowed: + std::cout << "Stowed" << std::endl; + _pidArmStates.SetSetpoint(-0.52); + _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())}; + default: + std::cout << "Error: alphaArm in INVALID STATE" << std::endl; break; + } - // transmission translate - // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage); - // _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage); - // _config.wristGearbox.motorController->SetVoltage(setWristVoltage); - _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage); - _config.wristGearbox.motorController->SetVoltage(_setWristVoltage); - - // _config.wristGearbox.motorController->SetVoltage(_setVoltage); + _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage); + _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage); + std::cout << "Encoder Value: " << _config->alphaArmEncoder.GetEncoderPosition().value() << std::endl; + _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError()); + _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint()); + _table->GetEntry("Input").SetDouble(_config->alphaArmEncoder.GetEncoderPosition().value()); + + _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError()); + _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint()); + + _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint()); + _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError()); + + + + std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl; + } void AlphaArm::SetState(AlphaArmState state) { _state = state; } -// void AlphaArm::SetRaw(units::volt_t voltage){ -// _rawArmVoltage = voltage; -// _rawWristVoltage = voltage; -// } - void AlphaArm::SetArmRaw(units::volt_t voltage) { _rawArmVoltage = voltage; } -void AlphaArm::setWristRaw(units::volt_t voltage) { - _rawWristVoltage = voltage; + +void AlphaArm::SetGoal(double goal){ + _goal = goal; +} + +void AlphaArm::SetControllerRaw(units::volt_t voltage){ + _controlledRawVoltage = voltage; } + + diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp index ece50ebb..bc3d33c1 100644 --- a/src/main/cpp/AlphaArmBehaviour.cpp +++ b/src/main/cpp/AlphaArmBehaviour.cpp @@ -11,8 +11,17 @@ AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxContro Controls(alphaArm); } +AlphaArmConfig AlphaArm::GetConfig() { + return *_config; +} + void AlphaArmManualControl::OnTick(units::second_t dt) { - if (_codriver->GetXButtonPressed()) { + + _table->GetEntry("State").SetBoolean(_rawControl); + _table->GetEntry("Goal Value").SetBoolean(_gotValue); + + + if (_codriver->GetBButton()) { if (_rawControl == true) { _rawControl = false; } else { @@ -20,16 +29,18 @@ void AlphaArmManualControl::OnTick(units::second_t dt) { } } - if (_rawControl) { - _alphaArm->SetState(AlphaArmState::kRaw); - _alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V); - _alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V); + if(_codriver->GetLeftTriggerAxis() > 0.1){ + _alphaArm->SetState(AlphaArmState::kSpeakerAngle); + } else if (_codriver->GetLeftBumper()){ + _alphaArm->SetState(AlphaArmState::kAmpAngle); + } else if(_codriver->GetYButton()){ + _alphaArm->SetState(AlphaArmState::kStowed); + } else if(_codriver->GetRightBumper()){ + _alphaArm->SetState(AlphaArmState::kIntakeAngle); } else { - if (_codriver->GetRightBumperPressed()) { - _alphaArm->SetState(AlphaArmState::kForwardWrist); - } - if (_codriver->GetLeftBumperPressed()) { - _alphaArm->SetState(AlphaArmState::kReverseWrist); - } + _alphaArm->SetState(AlphaArmState::kIdle); } } + + + diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 8c5bed35..4f736097 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -3,40 +3,63 @@ // of the MIT License at the root of this project #include "Robot.h" - -// include units -#include +#include "RobotMap.h" +#include +#include +#include +#include +#include +#include +#include #include +// #include #include -#include #include +#include #include -#include -#include -#include + #include "behaviour/HasBehaviour.h" +#include "networktables/NetworkTableInstance.h" + static units::second_t lastPeriodic; void Robot::RobotInit() { + + // shooter = new Shooter(robotmap.shooterSystem.config); + // wom::BehaviourScheduler::GetInstance()->Register(shooter); + // shooter->SetDefaultBehaviour( + // [this]() { return wom::make(shooter, &robotmap.controllers.codriver); }); + + // sched = wom::BehaviourScheduler::GetInstance(); + // m_chooser.SetDefaultOption("Default Auto", "Default Auto"); + + // m_chooser.SetDefaultOption("Default Auto", "Default Auto"); + + // frc::SmartDashboard::PutData("Auto Selector", &m_chooser); sched = wom::BehaviourScheduler::GetInstance(); m_chooser.SetDefaultOption("Default Auto", "Default Auto"); - frc::SmartDashboard::PutData("Auto Selector", &m_chooser); + // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json"); - m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json"); + // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json"); + // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json"); - m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json"); - m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json"); + // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser); - frc::SmartDashboard::PutData("Path Selector", &m_path_chooser); + // frc::SmartDashboard::PutData("Field", &m_field); - frc::SmartDashboard::PutData("Field", &m_field); + // simulation_timer = frc::Timer(); - simulation_timer = frc::Timer(); + // robotmap.swerveBase.gyro->Reset(); + + + // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); + // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive); + // _swerveDrive->SetDefaultBehaviour( + // [this]() { return wom::make(_swerveDrive, &robotmap.controllers.driver); }); - robotmap.swerveBase.gyro->Reset(); _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); @@ -47,13 +70,19 @@ void Robot::RobotInit() { }); - alphaArm = new AlphaArm(robotmap.alphaArmSystem.config); + // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field); + // m_driveSim = wom::TempSimSwerveDrive(); + + + 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(); + // 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); // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.5948_rad); // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6156_rad); @@ -71,7 +100,6 @@ void Robot::RobotInit() { robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(3.01121_rad); robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(4.4524_rad); - // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"); // front left // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"); // front right // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"); // back left @@ -80,6 +108,17 @@ void Robot::RobotInit() { // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"); // front right // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"); // back left // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase"); + lastPeriodic = wom::now(); + + // intake = new Intake(robotmap.intakeSystem.config); + // wom::BehaviourScheduler::GetInstance()->Register(intake); + // intake->SetDefaultBehaviour( + // [this]() { return wom::make(intake, robotmap.controllers.codriver); }); + + // _vision = new Vision("limelight", FMAP("fmap.fmap")); + + //robotmap->vision = new Vision("limelight", FMAP("fmap.fmap")); + } void Robot::RobotPeriodic() { @@ -87,36 +126,65 @@ void Robot::RobotPeriodic() { lastPeriodic = wom::now(); loop.Poll(); - sched->Tick(); + wom::BehaviourScheduler::GetInstance()->Tick(); + //shooter->OnUpdate(dt); + //sched->Tick(); + + // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder") + // .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value()); + // robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder") + // .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value()); + // robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder") + // .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value()); + // robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder") + // .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()); + // _swerveDrive->OnUpdate(dt); + //shooter->OnStart(); + //intake->OnUpdate(dt); + + // _swerveDrive->OnUpdate(dt); + + +// 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()); alphaArm->OnUpdate(dt); - _swerveDrive->OnUpdate(dt); + //_swerveDrive->OnUpdate(dt); + } void Robot::AutonomousInit() { - // m_driveSim->SetPath(m_path_chooser.GetSelected()); - loop.Clear(); sched->InterruptAll(); } -void Robot::AutonomousPeriodic() { - // m_driveSim->OnUpdate(); -} + +void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { loop.Clear(); + + wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance(); sched->InterruptAll(); // frontLeft->SetVoltage(4_V); // frontRight->SetVoltage(4_V); // backLeft->SetVoltage(4_V); // backRight->SetVoltage(4_V); + + + // FMAP("fmap.fmap"); + + // _swerveDrive->OnStart(); + // sched->InterruptAll(); + + //reimplement when vision is reimplemented + + // _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first); } + void Robot::TeleopPeriodic() {} void Robot::DisabledInit() {} @@ -125,5 +193,28 @@ void Robot::DisabledPeriodic() {} void Robot::TestInit() {} void Robot::TestPeriodic() {} -void Robot::SimulationInit() {} +void Robot::SimulationInit() { + /* std::string x = "["; + std::string y = "["; + // _vision->GetDistanceToTarget(16); + for (int i = 1; i < 17; i++) { + for (int j = 0; j < 17; j++) { + frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive); + x += std::to_string(pose.X().value()) + ","; + y += std::to_string(pose.Y().value()) + ","; + } + } + + x += "]"; + y += "]"; + + std::cout << x << std::endl; + std::cout << y << std::endl; */ + // std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl; + //Reimplement when vision is reimplemented + // frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive); + // nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot", + // pose.Rotation().Degrees().value()); +} + void Robot::SimulationPeriodic() {} diff --git a/src/main/cpp/Vision.cpp b/src/main/cpp/Vision.cpp new file mode 100644 index 00000000..8684d8a4 --- /dev/null +++ b/src/main/cpp/Vision.cpp @@ -0,0 +1,12 @@ +// #include "Vision.h" + +// #include +// #include + +// #include "units/length.h" +// #include "units/math.h" +// #include "vision/Limelight.h" + +// //std::pair Vision::AngleToTarget(VisionTargetObjects objects){ + +// //} \ No newline at end of file diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h index 96858caa..8429fa9d 100644 --- a/src/main/include/AlphaArm.h +++ b/src/main/include/AlphaArm.h @@ -4,11 +4,23 @@ #pragma once #include + + #include "Wombat.h" +//#include "vision/Vision.h" +#include "utils/PID.h" +#include +#include +#include struct AlphaArmConfig { wom::Gearbox alphaArmGearbox; - wom::Gearbox wristGearbox; + wom::Gearbox alphaArmGearbox2; + wom::DutyCycleEncoder alphaArmEncoder; + + std::string path; + // Vision *vision; + }; enum class AlphaArmState { @@ -16,28 +28,42 @@ enum class AlphaArmState { kIntakeAngle, kAmpAngle, kSpeakerAngle, - kForwardWrist, - kReverseWrist, + kHoldAngle, + kVisionAngle, + kStowed, kRaw }; -class AlphaArm : public ::behaviour::HasBehaviour { +class AlphaArm : public behaviour::HasBehaviour { public: - explicit AlphaArm(AlphaArmConfig config); + AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/); void OnUpdate(units::second_t dt); void SetArmRaw(units::volt_t voltage); - void setWristRaw(units::volt_t voltage); void SetState(AlphaArmState state); - AlphaArmConfig GetConfig(); - // void SetRaw(units::volt_t voltage); + void SetControllerRaw(units::volt_t voltage); + void SetGoal(double goal); + void OnStart(); + AlphaArmConfig GetConfig(); //{ return _config; } + frc::PIDController GetPID(); private: - AlphaArmConfig _config; + // units::radian_t CalcTargetAngle(); + + AlphaArmConfig *_config; + wom::vision::Limelight* _vision; AlphaArmState _state = AlphaArmState::kIdle; + //wom::utils::PIDController _alphaArmPID; + //frc::DutyCycleEncoder armEncoder{4}; + frc::PIDController _pidArm; + frc::PIDController _pidArmStates; + frc::PIDController _pidIntakeState; + std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm"); units::volt_t _setAlphaArmVoltage = 0_V; - units::volt_t _setWristVoltage = 0_V; + units::volt_t _controlledRawVoltage = 0_V; units::volt_t _rawArmVoltage = 0_V; - units::volt_t _rawWristVoltage = 0_V; + units::volt_t _testRawVoltage = 3_V; + double _goal = 0; + }; diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h index 4c2d03fb..f29714c3 100644 --- a/src/main/include/AlphaArmBehaviour.h +++ b/src/main/include/AlphaArmBehaviour.h @@ -18,7 +18,10 @@ class AlphaArmManualControl : public behaviour::Behaviour { private: AlphaArm* _alphaArm; frc::XboxController* _codriver; - // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05 - // )?_codriver->GetRightY():0) * 2_V; + units::volt_t _setAlphaArmVoltage = 0_V; bool _rawControl; + bool _gotValue = false; + std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem"); + }; + diff --git a/src/main/include/ArmAngle.h b/src/main/include/ArmAngle.h new file mode 100644 index 00000000..051565db --- /dev/null +++ b/src/main/include/ArmAngle.h @@ -0,0 +1,84 @@ +// // 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 "Wombat.h" +// #include "units/angle.h" +// #include "AlphaArm.h" + +// #define APRILTAGS_MAX 16 +// #define APRILTAGS_MIN 0 + +// enum class VisionTarget { +// /* +// Left is toward the blue side of the diagram here: +// https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf + +// The numbers are the numbers on the field diagram (and the numbers on the tags). +// */ +// kBlueAmp = 6, +// kBlueSpeakerCenter = 7, +// kBlueSpeakerOffset = 8, +// kBlueChain1 = 15, +// kBlueChain2 = 16, +// kBlueChain3 = 14, +// kBlueSourceLeft = 1, +// kBlueSourceRight = 2, + +// kRedAmp = 5, +// kRedSpeakerCenter = 4, +// kRedSpeakerOffset = 3, + +// kRedChain1 = 12, +// kRedChain2 = 11, +// kRedChain3 = 13, +// kRedSourceLeft = 9, +// kRedSourceRight = 10 +// }; + +// enum class VisionModes { kAprilTag = 0, kRing = 1 }; + +// enum class VisionTargetObjects { kNote }; + +// struct AprilTag { +// int id; +// double size; +// std::array, 4> transform; +// bool unique; + +// frc::Pose3d pos; +// units::radian_t yaw; +// units::radian_t pitch; +// units::radian_t roll; +// }; + +// class FMAP { +// public: +// explicit FMAP(std::string path); + +// std::vector GetTags(); + +// private: +// std::vector _tags; +// std::string _path; +// std::string deploy_dir; +// }; + +// class ArmVision{ +// public: +// ArmVision(std::string name, FMAP fmap); + +// frc::Twist2d AngleToAprilTag(VisionTarget target, double angleOffset, ); +// }; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 1394b3d5..0f39f1dc 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -3,8 +3,7 @@ // of the MIT License at the root of this project #pragma once - -#include +// #include #include #include #include @@ -16,11 +15,15 @@ #include +#include "AlphaArm.h" +#include "AlphaArmBehaviour.h" #include "RobotMap.h" #include "Wombat.h" class Robot : public frc::TimedRobot { public: + void TestInit() override; + void TestPeriodic() override; void RobotInit() override; void RobotPeriodic() override; void AutonomousInit() override; @@ -29,8 +32,6 @@ class Robot : public frc::TimedRobot { void TeleopPeriodic() override; void DisabledInit() override; void DisabledPeriodic() override; - void TestInit() override; - void TestPeriodic() override; void SimulationInit() override; void SimulationPeriodic() override; @@ -38,19 +39,36 @@ class Robot : public frc::TimedRobot { RobotMap robotmap; wom::BehaviourScheduler* sched; frc::EventLoop loop; - frc::SendableChooser m_chooser; + //Shooter* shooter; + + // Intake* intake; + // frc::SendableChooser m_chooser; - frc::Field2d m_field; + // frc::Field2d m_field; - frc::Timer simulation_timer; + // frc::Timer simulation_timer; - frc::SendableChooser m_path_chooser; + // frc::SendableChooser m_path_chooser; - wom::SwerveDrive* _swerveDrive; + //wom::SwerveDrive* _swerveDrive; + + // rev::CANSparkMax testMotorUp{1, rev::CANSparkMax::MotorType::kBrushless}; + // rev::CANSparkMax testMotorDown{6, rev::CANSparkMax::MotorType::kBrushless}; + // frc::XboxController testdriver = frc::XboxController(1); AlphaArm* alphaArm; - ctre::phoenix6::hardware::TalonFX *frontLeft; + + // ctre::phoenix6::hardware::TalonFX *frontLeft; + // ctre::phoenix6::hardware::TalonFX *frontRight; + // ctre::phoenix6::hardware::TalonFX *backLeft; + // ctre::phoenix6::hardware::TalonFX *backRight; + + //wom::SwerveDrive* _swerveDrive; + + + //ctre::phoenix6::hardware::TalonFX *frontLeft; // ctre::phoenix6::hardware::TalonFX *frontRight; // ctre::phoenix6::hardware::TalonFX *backLeft; // ctre::phoenix6::hardware::TalonFX *backRight; }; + diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h index c1e8753a..f4756c1c 100644 --- a/src/main/include/RobotMap.h +++ b/src/main/include/RobotMap.h @@ -10,12 +10,14 @@ #include #include #include +#include #include #include #include #include +#include "AlphaArm.h" #include #include #include @@ -37,17 +39,135 @@ struct RobotMap { }; Controllers controllers; - struct AlphaArmSystem { - rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless}; - rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless}; +// struct SwerveBase { +// ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19, "Drivebase"}; +// ctre::phoenix6::hardware::CANcoder frontRightCancoder{17, "Driverbase"}; +// ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"}; +// ctre::phoenix6::hardware::CANcoder backRightCancoder{18, "Drivebase"}; + +// ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase"); +// wpi::array turnMotors{ +// new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"), +// new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"), +// new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"), +// new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")}; +// wpi::array driveMotors{ +// new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"), +// new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"), +// new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"), +// new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")}; + +// wpi::array moduleConfigs{ +// wom::SwerveModuleConfig{ +// // front left module +// frc::Translation2d(10.761_in, 9.455_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(19, 0.0445_m, 4096, 12.8), +// frc::DCMotor::Falcon500(1).WithReduction(12.8)}, +// &frontLeftCancoder, 4_in / 2}, +// wom::SwerveModuleConfig{ +// // front right module +// frc::Translation2d(10.761_in, -9.455_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(17, 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.761_in, 9.455_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(16, 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.761_in, -9.455_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(18, 0.0445_m, 4096, 12.8), +// frc::DCMotor::Falcon500(1).WithReduction(12.8)}, +// &backLeftCancoder, 4_in / 2}, +// }; + +// // Setting the PID path and values to be used for SwerveDrive and +// // SwerveModules +// wom::SwerveModule::angle_pid_conf_t anglePID{ +// "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s), +// 0_V / (100_deg / 1_s), 1_deg, 0.5_deg / 2_s}; +// wom::SwerveModule::velocity_pid_conf_t velocityPID{ +// "/drivetrain/pid/velocity/config", +// // 12_V / 4_mps // webers per metre +// }; +// wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{ +// "/drivetrain/pid/pose/angle/config", +// 180_deg / 1_s / 45_deg, +// wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1}, +// 0_deg / 1_deg, +// 10_deg, +// 10_deg / 1_s}; +// wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{ +// "/drivetrain/pid/pose/position/config", +// 3_mps / 1_m, +// wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, +// 0_m / 1_m, +// 20_cm, +// 10_cm / 1_s, +// 10_cm}; + +// // the config for the whole swerve drive +// wom::SwerveDriveConfig config{"/drivetrain", +// anglePID, +// velocityPID, +// moduleConfigs, // each module +// gyro, +// poseAnglePID, +// posePositionPID, +// 60_kg, // robot mass (estimate rn) +// {0.1, 0.1, 0.1}, +// {0.9, 0.9, 0.9}}; + +// // current limiting and setting idle mode of modules to brake mode +// // SwerveBase() { +// // for (size_t i = 0; i < 4; i++) { +// // turnMotors[i]->ConfigSupplyCurrentLimit( +// // SupplyCurrentLimitConfiguration(true, 15, 15, 0)); +// // driveMotors[i]->SetNeutralMode(NeutralMode::Brake); +// // turnMotors[i]->SetNeutralMode(NeutralMode::Brake); +// // driveMotors[i]->SetInverted(true); +// // } +// //} +// }; +// SwerveBase swerveBase; + - wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)}; - wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)}; +// struct SwerveTable { +// std::shared_ptr swerveDriveTable = +// nt::NetworkTableInstance::GetDefault().GetTable("swerve"); +// }; +// SwerveTable swerveTable; + + struct AlphaArmSystem { + rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless}; + rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless}; - AlphaArmConfig config{alphaArmGearbox, wristGearbox}; + wom::DutyCycleEncoder alphaArmEncoder{3, 0.1_m, 2048, 1}; + wom::CANSparkMaxEncoder* alphaArmNeoEncoderUp = new wom::CANSparkMaxEncoder(&alphaArmMotorUp, 0.1_m); + wom::CANSparkMaxEncoder* alphaArmNeoEncoderDown = new wom::CANSparkMaxEncoder(&alphaArmMotorDown, 0.1_m); + + + wom::Gearbox alphaArmGearbox{&alphaArmMotorUp, alphaArmNeoEncoderUp, frc::DCMotor::NEO(1)}; + wom::Gearbox alphaArmGearbox2{&alphaArmMotorDown, alphaArmNeoEncoderDown, frc::DCMotor::NEO(1)}; + + AlphaArmConfig config{alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm" + //, &vision + }; }; AlphaArmSystem alphaArmSystem; + struct SwerveBase { ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"}; ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"}; @@ -164,3 +284,4 @@ struct RobotMap { std::shared_ptr swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve"); }; SwerveTable swerveTable; }; + diff --git a/src/main/include/Vision.h b/src/main/include/Vision.h new file mode 100644 index 00000000..ea953fb9 --- /dev/null +++ b/src/main/include/Vision.h @@ -0,0 +1,111 @@ +// // 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 "Wombat.h" +// #include "units/angle.h" +// #include "AlphaArm.h" + +// #define APRILTAGS_MAX 16 +// #define APRILTAGS_MIN 0 + +// enum class VisionTarget { +// /* +// Left is toward the blue side of the diagram here: +// https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf + +// The numbers are the numbers on the field diagram (and the numbers on the tags). +// */ +// kBlueAmp = 6, +// kBlueSpeakerCenter = 7, +// kBlueSpeakerOffset = 8, +// kBlueChain1 = 15, +// kBlueChain2 = 16, +// kBlueChain3 = 14, +// kBlueSourceLeft = 1, +// kBlueSourceRight = 2, + +// kRedAmp = 5, +// kRedSpeakerCenter = 4, +// kRedSpeakerOffset = 3, + +// kRedChain1 = 12, +// kRedChain2 = 11, +// kRedChain3 = 13, +// kRedSourceLeft = 9, +// kRedSourceRight = 10 +// }; + +// enum class VisionModes { kAprilTag = 0, kRing = 1 }; + +// enum class VisionTargetObjects { kNote }; + +// struct AprilTag { +// int id; +// double size; +// std::array, 4> transform; +// bool unique; + +// frc::Pose3d pos; +// units::radian_t yaw; +// units::radian_t pitch; +// units::radian_t roll; +// }; + +// class FMAP { +// public: +// explicit FMAP(std::string path); + +// std::vector GetTags(); + +// private: +// std::vector _tags; +// std::string _path; +// std::string deploy_dir; +// }; + +// class Vision { +// public: +// Vision(std::string name, FMAP fmap); + +// std::pair GetDistanceToTarget(VisionTarget target); +// std::pair GetDistanceToTarget(int id); +// //void GetAngleToTarget(double angle); + +// void SetMode(VisionModes mode); + +// frc::Pose3d GetPose(); + +// frc::Rotation2d AngleToTarget(VisionTarget target, double angle, AlphaArm* alphaArm); +// std::pair AngleToTarget(VisionTarget target); +// double LockOn(VisionTarget target); + +// //std::pair GetAngleToTarget(Vision) + +// std::pair GetAngleToObject(VisionTargetObjects object); +// units::degree_t LockOn(VisionTargetObjects target); + +// std::vector GetTags(); + +// bool IsAtPose(frc::Pose3d pose, units::second_t dt); + +// bool TargetIsVisible(VisionTargetObjects target); + +// int CurrentAprilTag(); + +// private: +// wom::Limelight* _limelight; +// FMAP _fmap; +// }; \ 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 b6c071c0..62b44cf1 100644 --- a/wombat/src/main/cpp/utils/Encoder.cpp +++ b/wombat/src/main/cpp/utils/Encoder.cpp @@ -43,17 +43,17 @@ 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; - //} - return GetEncoderTicks() * 1_rad; + // 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 * (2 * 3.1415); } double wom::utils::Encoder::GetEncoderDistance() { @@ -90,11 +90,10 @@ 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::kQuadrature)) {} + _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {} double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const { - return _encoder.GetPosition() * _reduction; + return ((_encoder.GetPosition() * 2 * 3.1415) / 200); } double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const { @@ -137,10 +136,13 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const { 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::Encoder(ticksPerRotation, reduction, wheelRadius, 0) { + _dutyCycleEncoder = new frc::DutyCycleEncoder(channel); + + } double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const { - return _dutyCycleEncoder.Get().value(); + return _dutyCycleEncoder->GetAbsolutePosition(); } double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const { @@ -160,4 +162,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/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h index b9aae2a4..d17ba4e2 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -106,7 +106,7 @@ class DutyCycleEncoder : public Encoder { double GetVelocity() const override; private: - frc::DutyCycleEncoder _dutyCycleEncoder; + frc::DutyCycleEncoder* _dutyCycleEncoder; }; class CanEncoder : public Encoder { @@ -125,4 +125,4 @@ class CanEncoder : public Encoder { ctre::phoenix6::hardware::CANcoder* _canEncoder; }; } // namespace utils -} // namespace wom +} // namespace wom \ No newline at end of file