From 5928659c566888bf58024b6e78277512a6ae69c7 Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Mon, 21 Oct 2024 22:36:10 -0400 Subject: [PATCH] tuning shooter --- src/main/cpp/Robot.cpp | 13 ++++++------- src/main/cpp/RobotContainer.cpp | 4 ++-- src/main/cpp/str/SwerveDrive.cpp | 9 ++++++++- src/main/cpp/subsystems/ShooterSubsystem.cpp | 1 + src/main/cpp/subsystems/SwerveSubsystem.cpp | 3 +++ src/main/deploy/commit.txt | 2 +- src/main/include/str/Vision.h | 8 ++++---- 7 files changed, 25 insertions(+), 15 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index af97a7c..59f0999 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -27,13 +27,12 @@ void Robot::RobotInit() { consts::SWERVE_ODOM_LOOP_PERIOD, 2_ms); m_container.GetShooterSubsystem().SetupLUTs({ - {1_ft, {1000_rpm, 1000_rpm}}, - {2_ft, {1500_rpm, 1500_rpm}}, - {3_ft, {2000_rpm, 2000_rpm}}, - {4_ft, {2500_rpm, 2500_rpm}}, - {5_ft, {3000_rpm, 3000_rpm}}, - {6_ft, {3500_rpm, 3500_rpm}}, - {7_ft, {4000_rpm, 4000_rpm}}, + {65_in, {1200_rpm, 3500_rpm}}, + {78_in, {2500_rpm, 2500_rpm}}, + {102_in, {3000_rpm, 1900_rpm}}, + {114_in, {3400_rpm, 1900_rpm}}, + {127_in, {5000_rpm, 1600_rpm}}, + {143_in, {5000_rpm, 1200_rpm}}, }); } diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 1b3ba31..1922bee 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -100,8 +100,8 @@ void RobotContainer::ConfigureBindings() { operatorController.B().OnFalse(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::OFF; })); - operatorController.X().WhileTrue(shooterSubsystem.RunShooter( - [] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return swerveSubsystem.GetDistanceToSpeaker(swerveSubsystem.GetSpeakerSideFromPosition()); })); + operatorController.X().OnTrue(shooterSubsystem.RunShooter( + [] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return swerveSubsystem.GetDistanceToSpeaker(swerveSubsystem.GetSpeakerSideFromPosition()); }).Repeatedly()); operatorController.X().OnFalse(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::OFF; })); diff --git a/src/main/cpp/str/SwerveDrive.cpp b/src/main/cpp/str/SwerveDrive.cpp index f6be947..7a1f456 100644 --- a/src/main/cpp/str/SwerveDrive.cpp +++ b/src/main/cpp/str/SwerveDrive.cpp @@ -79,9 +79,16 @@ void SwerveDrive::Drive(units::meters_per_second_t xVel, units::radians_per_second_t omega, bool fieldRelative, bool openLoop) { frc::ChassisSpeeds speedsToSend; + + frc::Rotation2d rot = poseEstimator.GetEstimatedPosition().Rotation(); + + if(frc::DriverStation::IsTeleop()) { + rot = frc::Rotation2d{GetYawFromImu()}; + } + if (fieldRelative) { speedsToSend = frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xVel, yVel, omega, poseEstimator.GetEstimatedPosition().Rotation()); + xVel, yVel, omega, rot); } else { speedsToSend.vx = xVel; speedsToSend.vy = yVel; diff --git a/src/main/cpp/subsystems/ShooterSubsystem.cpp b/src/main/cpp/subsystems/ShooterSubsystem.cpp index 97dbd22..6dae8cd 100644 --- a/src/main/cpp/subsystems/ShooterSubsystem.cpp +++ b/src/main/cpp/subsystems/ShooterSubsystem.cpp @@ -58,6 +58,7 @@ frc2::CommandPtr ShooterSubsystem::RunShooter( neutralState = false; break; case consts::shooter::PRESET_SPEEDS::SPEAKER_DIST: + fmt::print("dist: {}\n", distance()); topWheelVelocitySetpoint = consts::shooter::TOP_SHOOTER_LUT[distance()]; bottomWheelVelocitySetpoint = diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index 8a7493d..365ef81 100644 --- a/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -40,6 +40,9 @@ void SwerveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, // This method will be called once per scheduler run void SwerveSubsystem::Periodic() { swerveDrive.UpdateNTEntries(); + frc::SmartDashboard::PutNumber("Distance to Speaker Center", GetDistanceToSpeaker(SpeakerSide::CENTER).convert().value()); + frc::SmartDashboard::PutNumber("Distance to Speaker Amp", GetDistanceToSpeaker(SpeakerSide::AMP_SIDE).convert().value()); + frc::SmartDashboard::PutNumber("Distance to Speaker Center", GetDistanceToSpeaker(SpeakerSide::SOURCE).convert().value()); } void SwerveSubsystem::SimulationPeriodic() { diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index cf05ecd..f770d3e 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -c1d62c7 \ No newline at end of file +b8451f2 \ No newline at end of file diff --git a/src/main/include/str/Vision.h b/src/main/include/str/Vision.h index 711010e..1c092c7 100644 --- a/src/main/include/str/Vision.h +++ b/src/main/include/str/Vision.h @@ -29,16 +29,16 @@ class Vision { std::array cameras{ Camera{consts::vision::FL_CAM_NAME, consts::vision::FL_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, - consts::vision::MULTI_TAG_STD_DEV, false}, + consts::vision::MULTI_TAG_STD_DEV, true}, Camera{consts::vision::FR_CAM_NAME, consts::vision::FR_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, - consts::vision::MULTI_TAG_STD_DEV, false}, + consts::vision::MULTI_TAG_STD_DEV, true}, Camera{consts::vision::BL_CAM_NAME, consts::vision::BL_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, - consts::vision::MULTI_TAG_STD_DEV, false}, + consts::vision::MULTI_TAG_STD_DEV, true}, Camera{consts::vision::BR_CAM_NAME, consts::vision::BR_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, - consts::vision::MULTI_TAG_STD_DEV, false}}; + consts::vision::MULTI_TAG_STD_DEV, true}}; // Camera noteCamera{consts::vision::NOTE_CAM_NAME, // consts::vision::ROBOT_TO_NOTE_CAM, // consts::vision::SINGLE_TAG_STD_DEV,