Merge branch 'master' of https://github.com/FRC830/2024Robot

FRC830 · Oct 20, 2024 · 7a77453 · 7a77453
2 parents a455a94 + 27f4a5b
commit 7a77453
Show file tree

Hide file tree

Showing 36 changed files with 315 additions and 109 deletions.
diff --git a/src/main/cpp/AutoAimer.cpp b/src/main/cpp/AutoAimer.cpp
@@ -3,11 +3,12 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 
 AutoAimer::AutoAimer()
+
 {
     m_lookup.emplace_back(VisionSetPoint{59.34, 50.0, 75.0, -51.9});
     m_lookup.emplace_back(VisionSetPoint{107.4769, 30, 130.0, -89.5});
-    m_lookup.emplace_back(VisionSetPoint{155.16, 22.3, 175.0, -104.5});
-    m_lookup.emplace_back(VisionSetPoint{172.71, 21.2, 205.3, -104.7}); 
+    m_lookup.emplace_back(VisionSetPoint{155.16, 21.3, 130.0, -89.5});
+    m_lookup.emplace_back(VisionSetPoint{172.71, 20.25, 130.3, -89.5}); 
     m_lookup.emplace_back(VisionSetPoint{200.58, 20.5, 250, -106.5});
 
 

diff --git a/src/main/cpp/ControllerInterface.cpp b/src/main/cpp/ControllerInterface.cpp
@@ -36,6 +36,7 @@ void ControllerInterface::UpdateLauncherInput(RobotControlData &controlData)
     controlData.launcherInput.goToStowPos = m_copilot.GetAButton();
     controlData.launcherInput.goToSubPos = m_copilot.GetYButton();
     controlData.launcherInput.runIndexerBackward = m_copilot.GetLeftTriggerAxis() >= 0.2;
+    controlData.launcherInput.goToHoardePos = m_copilot.GetBButton();
     // controlData.launcherInput.runIndexerForward = m_copilot.GetRightTriggerAxis() >= 0.2;
     //controlData.launcherInput.runIndexerForward = m_copilot.GetXButton();
     //controlData.launcherInput.runIndexerBackward = m_copilot.GetBButton();

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -55,6 +55,10 @@ void Robot::RobotInit() {
 
   SwerveInit();
 
+  frc::CameraServer::StartAutomaticCapture();
+
+  // m_cvSink = frc::CameraServer::GetVideo();
+  // m_cvSource = frc::CameraServer::PutVideo("Camera", 640, 480);
 }
 
 /**

diff --git a/src/main/cpp/commands/SmartIntakeCommand.cpp b/src/main/cpp/commands/SmartIntakeCommand.cpp
@@ -8,7 +8,11 @@ SmartIntakeCommand::SmartIntakeCommand(RobotControlData &controlData)
 
 void SmartIntakeCommand::Initialize()
 {
-    m_controlData.smartIntakeInput.smartIntake = true;
+    if (!m_controlData.smartIntakeOutput.smartIntakeFlag)
+    {
+        m_controlData.smartIntakeInput.smartIntake = true;
+    }
+
     m_state = 0;
 }
 

diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp
@@ -134,7 +134,18 @@ LauncherHAL::LauncherHAL()
     m_FlywheelBottom.SetControl(m_FlywheelTopFollower);
 
     m_FlywheelTop.GetConfigurator().Apply(configs);
-    m_FlywheelBottom.GetConfigurator().Apply(configs);
+
+    {
+        bool successful = false;
+        int retries = 0;
+
+        while (!successful || retries <= 10)
+        {
+            retries++;
+            auto status = m_FlywheelBottom.GetConfigurator().Apply(configs);
+            successful = status == ctre::phoenix::StatusCode::OK;
+        }
+    }
 
     m_currentAngle = -100.0;
 }
@@ -175,6 +186,7 @@ void LauncherHAL::SetIndexerSpeed(double speed)
     m_IndMotor.Set(m_indexerSpeed);
 }
 
+
 void LauncherHAL::SetAngle(double angle)
 {
     if (angle > MAX_PIVOT_ANGLE)
@@ -191,19 +203,15 @@ void LauncherHAL::SetAngle(double angle)
 
 void LauncherHAL::ProfiledMoveToAngle(double angle)
 {
-    if (std::fabs(m_currentAngle - angle) >= 1e-8)
-    {
-        m_profileState = 0;
-        m_currentAngle = angle;
-    }
-
     switch(m_profileState)
     {
      case 0: 
         {
             m_ProfileStartPos = GetAngle();
 
-            m_Timer.Restart();
+            m_Timer.Stop();
+            m_Timer.Reset();
+            m_Timer.Start();
 
             m_profileState++;
 
@@ -259,5 +267,5 @@ double LauncherHAL::GetFlywheelSpeed()
 
 void LauncherHAL::ResetProfiledMoveState()
 {
-    // m_profileState = 0;
+    m_profileState = 0;
 }
diff --git a/src/main/cpp/subsystems/SmartIntake.cpp b/src/main/cpp/subsystems/SmartIntake.cpp
@@ -211,7 +211,7 @@ void SmartIntake::HandleInput(RobotControlData& input){
         }
         case 3:
         {
-            if (m_timer.Get() > units::second_t(0.035))
+            if (m_timer.Get() > units::second_t(0.025))
             {
                 m_SmartOutTakeFlag = false;
                 m_OutTakeState = 0;
@@ -255,4 +255,6 @@ void SmartIntake::HandleInput(RobotControlData& input){
     m_prevSmartIntake = input.smartIntakeInput.smartIntake;
     m_prevSmartOutTake = input.smartIntakeInput.smartOutTake;
     m_prevSwitchMode = input.smartIntakeInput.switchMode;
+    input.smartIntakeOutput.smartIntakeFlag = m_SmartIntakeFlag;
+    input.smartIntakeOutput.smartOuttakeFlag = m_SmartOutTakeFlag;
 }
diff --git a/src/main/cpp/subsystems/subsystemManagers/IntakeManager.cpp b/src/main/cpp/subsystems/subsystemManagers/IntakeManager.cpp
@@ -2,8 +2,9 @@
 
 namespace
 {
-    const double NORMAL_INTAKE_SPEED = 0.80;
-    const double AMP_POS = 19.361;
+    const double NORMAL_INTAKE_SPEED = 0.95;
+    const double OUTAKE_SPEED = 0.7;
+    const double AMP_POS = 25.361;
     const double GROUND_POS = 129.000;
     const double STOW_POS = 68.144;
     const double PSEUDO_STOW_POS = 104.671;
@@ -23,15 +24,15 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
 {
     if (input.runIntakeOutSlow)
     {
-        m_intake.RunIntake(-0.1);
+        m_intake.RunIntake(-0.15);
     }
     else if (input.runIntakeIn && !input.runIntakeOut)
     {
         m_intake.RunIntake(NORMAL_INTAKE_SPEED);
     }
     else if (input.runIntakeOut && !input.runIntakeIn)
     {
-        m_intake.RunIntake(-NORMAL_INTAKE_SPEED);
+        m_intake.RunIntake(-OUTAKE_SPEED);
     } 
     else 
     {

diff --git a/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp b/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp
@@ -5,6 +5,7 @@ void LauncherManager::ResetLauncher()
 {
     m_goToStowPos = false;
     m_goToSubPos = false;
+    m_goToHoardePos = false;
     m_visionResetProfiledMoveState = false;
 }
 
@@ -19,6 +20,7 @@ void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &
             m_visionResetProfiledMoveState = true;
             m_goToStowPos = false;
             m_goToSubPos = false;
+            m_goToHoardePos = false;
         }
 
         m_launcher.SetFlywheelSpeed(launcherInput.visionSpeedSetpoint);
@@ -37,13 +39,23 @@ void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &
         if(launcherInput.goToStowPos){
             m_goToStowPos = true;
             m_goToSubPos = false;
+            m_goToHoardePos = false;
             m_launcher.ResetProfiledMoveState();
         }
         else if (launcherInput.goToSubPos) {
             m_goToStowPos = false;
             m_goToSubPos = true;
+            m_goToHoardePos = false;
 
             m_launcher.ResetProfiledMoveState();
+        } else if (launcherInput.goToHoardePos) {
+
+            m_goToHoardePos = true; 
+            m_goToStowPos = false;
+            m_goToSubPos = false;
+
+            m_launcher.ResetProfiledMoveState();
+
         }
 
         if (m_goToStowPos)
@@ -64,6 +76,12 @@ void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &
             m_launcher.ProfiledMoveToAngle(SUB_ANGLE);
             m_launcher.SetFlywheelSpeed(SUB_SPEED);
         }
+        if (m_goToHoardePos) {
+
+            m_launcher.ProfiledMoveToAngle(HOARDE_ANG);
+            m_launcher.SetFlywheelSpeed(HOARDE_SPD);
+
+        }
     }
 
     if (launcherInput.runIndexerBackwardSlow)

diff --git a/src/main/deploy/pathplanner/autos/BLUE_Top_4_Piece.auto b/src/main/deploy/pathplanner/autos/BLUE_Top_4_Piece.auto
@@ -36,6 +36,12 @@
             "pathName": "BLUE_3_Piece_Path_1"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.5
+          }
+        },
         {
           "type": "named",
           "data": {

diff --git a/src/main/deploy/pathplanner/autos/Red_Bottom_4_Piece.auto b/src/main/deploy/pathplanner/autos/Red_Bottom_4_Piece.auto
@@ -11,6 +11,25 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeDown"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SubShoot"
+                }
+              }
+            ]
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -23,6 +42,12 @@
             "pathName": "RED_Path5"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "VisionShoot"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -34,6 +59,12 @@
           "data": {
             "pathName": "RED_Path3"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "VisionShoot"
+          }
         }
       ]
     }

diff --git a/src/main/deploy/pathplanner/autos/Red_Top_4_Piece.auto b/src/main/deploy/pathplanner/autos/Red_Top_4_Piece.auto
@@ -11,12 +11,43 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeDown"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SubShoot"
+                }
+              }
+            ]
+          }
+        },
         {
           "type": "path",
           "data": {
             "pathName": "RED_3_Piece_Path_1"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.5
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "VisionShoot"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -29,6 +60,12 @@
             "pathName": "RED_3_Piece_Path_3"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "VisionShoot"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -40,6 +77,12 @@
           "data": {
             "pathName": "RED_3_Piece_Path_5"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "VisionShoot"
+          }
         }
       ]
     }

diff --git a/src/main/deploy/pathplanner/paths/BLUE_3_Piece_Path_1.path b/src/main/deploy/pathplanner/paths/BLUE_3_Piece_Path_1.path
@@ -33,7 +33,7 @@
   "eventMarkers": [
     {
       "name": "SmartIntakeCommand",
-      "waypointRelativePos": 0,
+      "waypointRelativePos": 0.05,
       "command": {
         "type": "parallel",
         "data": {
@@ -58,7 +58,7 @@
   "goalEndState": {
     "velocity": 0,
     "rotation": 25.0,
-    "rotateFast": false
+    "rotateFast": true
   },
   "reversed": false,
   "folder": "BLUEPATH",

diff --git a/src/main/deploy/pathplanner/paths/BLUE_3_Piece_Path_2.path b/src/main/deploy/pathplanner/paths/BLUE_3_Piece_Path_2.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [
     {
-      "name": "New Event Marker",
-      "waypointRelativePos": 0.45,
+      "name": "SmartIntake",
+      "waypointRelativePos": 0.05,
       "command": {
         "type": "parallel",
         "data": {

diff --git a/src/main/deploy/pathplanner/paths/BLUE_3_Piece_Path_4.path b/src/main/deploy/pathplanner/paths/BLUE_3_Piece_Path_4.path
@@ -48,16 +48,16 @@
   "constraintZones": [],
   "eventMarkers": [
     {
-      "name": "New Event Marker",
-      "waypointRelativePos": 0.35,
+      "name": "SmartIntake",
+      "waypointRelativePos": 0.05,
       "command": {
         "type": "parallel",
         "data": {
           "commands": [
             {
               "type": "named",
               "data": {
-                "name": "VisionShoot"
+                "name": "SmartIntakeCommand"
               }
             }
           ]

diff --git a/src/main/deploy/pathplanner/paths/BLUE_BottomNote_Note5.path b/src/main/deploy/pathplanner/paths/BLUE_BottomNote_Note5.path
@@ -33,7 +33,7 @@
   "eventMarkers": [
     {
       "name": "Intake",
-      "waypointRelativePos": 0.5,
+      "waypointRelativePos": 0.05,
       "command": {
         "type": "parallel",
         "data": {