shooter distance button

frc2053 · Oct 21, 2024 · b8451f2 · b8451f2
1 parent c1d62c7
commit b8451f2
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 23 deletions.
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -60,10 +60,10 @@ void RobotContainer::ConfigureBindings() {
       .OnTrue(feederSubsystem.Feed())
       .OnFalse(feederSubsystem.Stop());
 
-  // intakeSubsystem.TouchedNote().OnTrue(RumbleDriver([] { return .1_s; }));
-  // feederSubsystem.GotNote().OnTrue(frc2::cmd::Parallel(
-  //     intakeSubsystem.Stop(), feederSubsystem.Stop(),
-  //     RumbleDriver([] { return .5_s; }), RumbleOperator([] { return .5_s; })));
+  (intakeSubsystem.TouchedNote() && frc2::RobotModeTriggers::Teleop()).OnTrue(RumbleDriver([] { return .1_s; }));
+  (feederSubsystem.GotNote() && frc2::RobotModeTriggers::Teleop()).OnTrue(frc2::cmd::Parallel(
+      intakeSubsystem.Stop(), feederSubsystem.Stop(),
+      RumbleDriver([] { return .5_s; }), RumbleOperator([] { return .5_s; })));
 
   driverController.A().WhileTrue(swerveSubsystem.AlignToAmp());
 
@@ -85,20 +85,25 @@ void RobotContainer::ConfigureBindings() {
   // controller.Start().WhileTrue(
   //     swerveSubsystem.WheelRadius(frc2::sysid::Direction::kForward));
 
-  // operatorController.A().WhileTrue(shooterSubsystem.RunShooter(
-  //     [] { return consts::shooter::PRESET_SPEEDS::AMP; }));
-  // operatorController.A().OnFalse(shooterSubsystem.RunShooter(
-  //     [] { return consts::shooter::PRESET_SPEEDS::OFF; }));
+  operatorController.A().WhileTrue(shooterSubsystem.RunShooter(
+      [] { return consts::shooter::PRESET_SPEEDS::AMP; }));
+  operatorController.A().OnFalse(shooterSubsystem.RunShooter(
+      [] { return consts::shooter::PRESET_SPEEDS::OFF; }));
 
-  // operatorController.Y().WhileTrue(shooterSubsystem.RunShooter(
-  //     [] { return consts::shooter::PRESET_SPEEDS::PASS; }));
-  // operatorController.Y().OnFalse(shooterSubsystem.RunShooter(
-  //     [] { return consts::shooter::PRESET_SPEEDS::OFF; }));
+  operatorController.Y().WhileTrue(shooterSubsystem.RunShooter(
+      [] { return consts::shooter::PRESET_SPEEDS::PASS; }));
+  operatorController.Y().OnFalse(shooterSubsystem.RunShooter(
+      [] { return consts::shooter::PRESET_SPEEDS::OFF; }));
 
-  // operatorController.B().WhileTrue(shooterSubsystem.RunShooter(
-  //     [] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; }));
-  // operatorController.B().OnFalse(shooterSubsystem.RunShooter(
-  //     [] { return consts::shooter::PRESET_SPEEDS::OFF; }));
+  operatorController.B().WhileTrue(shooterSubsystem.RunShooter(
+      [] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; }));
+  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().OnFalse(shooterSubsystem.RunShooter(
+      [] { return consts::shooter::PRESET_SPEEDS::OFF; }));
 
   operatorController.Start().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::TUNING; }));
   operatorController.Start().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; }));
@@ -120,10 +125,10 @@ void RobotContainer::ConfigureBindings() {
   // (operatorController.Y() &&
   // operatorController.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdDynamic(frc2::sysid::Direction::kReverse));
 
-  operatorController.A().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kForward));
-  operatorController.B().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kReverse));
-  operatorController.X().WhileTrue(swerveSubsystem.SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction::kForward));
-  operatorController.Y().WhileTrue(swerveSubsystem.SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction::kReverse));
+  // operatorController.A().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kForward));
+  // operatorController.B().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kReverse));
+  // operatorController.X().WhileTrue(swerveSubsystem.SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction::kForward));
+  // operatorController.Y().WhileTrue(swerveSubsystem.SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction::kReverse));
 
   // controller.A().WhileTrue(swerveSubsystem.SysIdSteerQuasistaticVoltage(frc2::sysid::Direction::kForward));
   // controller.B().WhileTrue(swerveSubsystem.SysIdSteerQuasistaticVoltage(frc2::sysid::Direction::kReverse));

diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp
@@ -67,6 +67,45 @@ units::ampere_t SwerveSubsystem::GetSimulatedCurrentDraw() const {
   return swerveDrive.GetSimulatedCurrentDraw();
 }
 
+SpeakerSide SwerveSubsystem::GetSpeakerSideFromPosition() {
+    frc::Translation2d speakerLocation = consts::yearSpecific::speakerLocationCenter;
+
+    frc::Pose2d robotPose = GetRobotPose();
+    frc::Translation2d robotPosition = robotPose.Translation();
+    frc::Translation2d line1Point{0_m, 5.38_m};
+    frc::Rotation2d line1Dir{30_deg};
+    frc::Translation2d line2Point{0_m, 5.70_m};
+    frc::Rotation2d line2Dir{-30_deg};
+
+    bool isInCenter = false;
+    bool isAmpSide = false;
+    bool isSourceSide = false;
+    if (str::IsOnRed()) {
+      speakerLocation = pathplanner::FlippingUtil::flipFieldPosition(speakerLocation);
+      line1Point = pathplanner::FlippingUtil::flipFieldPosition(line1Point);
+      line2Point = pathplanner::FlippingUtil::flipFieldPosition(line2Point);
+      isInCenter = str::math::LineBetweenChecker::IsBetweenLines(robotPosition, line1Point, -line1Dir, line2Point, -line2Dir);
+      isAmpSide = str::math::LineBetweenChecker::IsPastLine(robotPosition, line2Point, -line2Dir, true);
+      isSourceSide = str::math::LineBetweenChecker::IsPastLine(robotPosition, line1Point, -line1Dir, false);
+    }
+    else {
+      isInCenter = str::math::LineBetweenChecker::IsBetweenLines(robotPosition, line1Point, line1Dir, line2Point, line2Dir);
+      isAmpSide = str::math::LineBetweenChecker::IsPastLine(robotPosition, line2Point, line2Dir, true);
+      isSourceSide = str::math::LineBetweenChecker::IsPastLine(robotPosition, line1Point, line1Dir, false);
+    }
+
+    if(!isInCenter) {
+      if(isAmpSide) {
+        return SpeakerSide::SOURCE;
+      } else if (isSourceSide) {
+        return SpeakerSide::AMP_SIDE;
+      }
+    }
+    else {
+      return SpeakerSide::CENTER;
+    }
+}
+
 frc2::CommandPtr SwerveSubsystem::FaceSpeaker(
   std::function<units::meters_per_second_t()> xVel,
   std::function<units::meters_per_second_t()> yVel) {
@@ -114,8 +153,6 @@ frc2::CommandPtr SwerveSubsystem::FaceSpeaker(
       }
     }
 
-    fmt::print("Is source side: {} Is amp side: {}, Is center: {}\n", isSourceSide, isAmpSide, isInCenter);
-
     if(str::IsOnRed()) {
       speakerLocation = pathplanner::FlippingUtil::flipFieldPosition(speakerLocation);
     }

diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-2a7514b
+c1d62c7
diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h
@@ -58,6 +58,7 @@ class SwerveSubsystem : public frc2::SubsystemBase {
                          bool fieldRelative, bool openLoop = false);
   frc2::CommandPtr PIDToPose(std::function<frc::Pose2d()> goalPose);
   frc2::CommandPtr AlignToAmp();
+  SpeakerSide GetSpeakerSideFromPosition();
   frc2::CommandPtr FaceSpeaker(std::function<units::meters_per_second_t()> xVel, std::function<units::meters_per_second_t()> yVel);
   frc2::CommandPtr SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction dir);
   frc2::CommandPtr SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction dir);