Vision | Configured the shooter cmds so that they run with vision

Author: NightShadow38

simgui-ds.json

@@ -91,11 +91,17 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     },
     {
-      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
+    },
+    {},
+    {},
+    {},
+    {
+      "guid": "Keyboard0"
     }
   ]
 }

simgui.json

@@ -45,9 +45,6 @@
             "open": true
           }
         },
-        "Intake": {
-          "open": true
-        },
         "Pivot": {
           "Motor": {
             "open": true
@@ -204,6 +201,19 @@
       "window": {
         "visible": false
       }
+    },
+    "Plot <4>": {
+      "plots": [
+        {
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 332
+        }
+      ]
     }
   }
 }

src/main/cpp/RobotContainer.cpp

@@ -26,8 +26,8 @@ void RobotContainer::ConfigureBindings() {
 
   //Triggers
   _driverController.RightTrigger().WhileTrue(cmd::CmdIntake());
-  //Bumpers
   _driverController.LeftTrigger().WhileTrue(cmd::CmdAimAtSpeakerWithVision(_driverController));
+  //Bumpers
 
   //Letters
 
@@ -36,7 +36,8 @@ void RobotContainer::ConfigureBindings() {
   //Operator
 
   //Triggers
-  _operatorController.RightTrigger().WhileTrue(cmd::CmdShootSpeaker());
+  _operatorController.RightTrigger().WhileTrue(cmd::CmdShootSpeaker(_driverController));
+  
   //Bumpers
   _operatorController.RightBumper().WhileTrue(cmd::CmdShootPassing());
   _operatorController.LeftBumper().WhileTrue(cmd::CmdShootNeutral());

src/main/cpp/commands/ShooterCommands.cpp

@@ -22,6 +22,7 @@ frc2::CommandPtr CmdFeedOnceOnTarget() {
     return Sequence(
             WaitUntil([]{return SubPivot::GetInstance().IsOnTarget();}),
             WaitUntil([]{return SubShooter::GetInstance().IsOnTarget();}),
+            WaitUntil([]{return SubVision::GetInstance().IsFacingTarget();}),
             SubFeeder::GetInstance().FeedToShooter()
         );
 }
@@ -30,11 +31,12 @@ frc2::CommandPtr CmdOuttake(){
     return SubIntake::GetInstance().Outtake().AlongWith(SubFeeder::GetInstance().FeedToIntake());
 }
 
-frc2::CommandPtr CmdShootSpeaker(){
+frc2::CommandPtr CmdShootSpeaker(frc2::CommandXboxController& controller){
     return Parallel(
         SubPivot::GetInstance().CmdPivotFromVision([]{    /*default value = 60 degrees(Subwoofer shot)*/
             return SubVision::GetInstance().GetSpeakerPitch().value_or(60_deg);}),
         SubShooter::GetInstance().CmdSetShooterSpeaker(),
+        CmdAimAtSpeakerWithVision(controller),
         CmdFeedOnceOnTarget()
     )
     .FinallyDo([] {SubShooter::GetInstance().CmdSetShooterOff();});

src/main/cpp/subsystems/SubVision.cpp

@@ -76,4 +76,12 @@ std::optional<units::degree_t> SubVision::GetSpeakerPitch(){
   } else {
     return {};
   }
+}
+
+bool SubVision::IsFacingTarget(){
+  if (-2_deg < GetSpeakerYaw() && GetSpeakerYaw() < 2_deg){
+    return true;
+  } else {
+    return false;
+  }
 }

src/main/include/commands/ShooterCommands.h

@@ -19,7 +19,7 @@
 namespace cmd {
   frc2::CommandPtr CmdIntake();
   frc2::CommandPtr CmdOuttake();
-  frc2::CommandPtr CmdShootSpeaker();
+  frc2::CommandPtr CmdShootSpeaker(frc2::CommandXboxController& controller);
   frc2::CommandPtr CmdShootAmp();
   frc2::CommandPtr CmdShootPassing();
   frc2::CommandPtr CmdShootNeutral();

src/main/include/subsystems/SubVision.h

@@ -37,8 +37,8 @@ class SubVision : public frc2::SubsystemBase {
   std::optional<photon::PhotonTrackedTarget> GetSpeakerTarget();
   std::optional<units::degree_t> GetSpeakerYaw();
   std::optional<units::degree_t> GetSpeakerPitch();
-
 
+  bool IsFacingTarget();
 
 private:
   std::string CAM_NAME1 = "arducam";