tuning shooter

frc2053 · Oct 22, 2024 · 5928659 · 5928659
1 parent b8451f2
commit 5928659
Show file tree

Hide file tree

Showing 7 changed files with 25 additions and 15 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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<units::inches>().value());
+  frc::SmartDashboard::PutNumber("Distance to Speaker Amp", GetDistanceToSpeaker(SpeakerSide::AMP_SIDE).convert<units::inches>().value());
+  frc::SmartDashboard::PutNumber("Distance to Speaker Center", GetDistanceToSpeaker(SpeakerSide::SOURCE).convert<units::inches>().value());
 }
 
 void SwerveSubsystem::SimulationPeriodic() {

diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-c1d62c7
+b8451f2
diff --git a/src/main/include/str/Vision.h b/src/main/include/str/Vision.h
@@ -29,16 +29,16 @@ class Vision {
   std::array<Camera, 4> 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,