Scrimmage Changes

.vscode/settings.json

@@ -24,7 +24,74 @@
     "memory": "cpp",
     "random": "cpp",
     "future": "cpp",
-    "optional": "cpp"
+    "optional": "cpp",
+    "atomic": "cpp",
+    "bit": "cpp",
+    "*.tcc": "cpp",
+    "cctype": "cpp",
+    "cfenv": "cpp",
+    "chrono": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "codecvt": "cpp",
+    "compare": "cpp",
+    "complex": "cpp",
+    "concepts": "cpp",
+    "condition_variable": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "deque": "cpp",
+    "forward_list": "cpp",
+    "list": "cpp",
+    "map": "cpp",
+    "set": "cpp",
+    "string": "cpp",
+    "unordered_map": "cpp",
+    "unordered_set": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "functional": "cpp",
+    "iterator": "cpp",
+    "memory_resource": "cpp",
+    "numeric": "cpp",
+    "ratio": "cpp",
+    "regex": "cpp",
+    "string_view": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "fstream": "cpp",
+    "initializer_list": "cpp",
+    "iomanip": "cpp",
+    "iosfwd": "cpp",
+    "istream": "cpp",
+    "limits": "cpp",
+    "mutex": "cpp",
+    "new": "cpp",
+    "numbers": "cpp",
+    "ostream": "cpp",
+    "ranges": "cpp",
+    "semaphore": "cpp",
+    "span": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "stop_token": "cpp",
+    "streambuf": "cpp",
+    "thread": "cpp",
+    "cinttypes": "cpp",
+    "typeindex": "cpp",
+    "typeinfo": "cpp",
+    "valarray": "cpp",
+    "variant": "cpp"
   },
   "C_Cpp.errorSquiggles": "enabled"
 }

src/main/cpp/Robot.cpp

@@ -13,6 +13,7 @@
 #include "frc2/command/InstantCommand.h"
 #include "util/NKTrajectoryManager.hpp"
 #include <arm/Arm.h>
+#include <autos/Auto.h>
 
 Robot::Robot() { this->CreateRobot(); }
 
@@ -66,6 +67,8 @@ void Robot::TeleopInit() {
   if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
   }
+  m_arm.arm_Brake_In();
+
 }
 
 /**
@@ -100,6 +103,9 @@ void Robot::SimulationPeriodic() {}
  */
 void Robot::CreateRobot() {
   // Initialize all of your commands and subsystems here
+      frc::SmartDashboard::PutNumber("ARM_Angel",100);
+      frc::SmartDashboard::PutNumber("ARM_Speed",-120);
+
   m_swerveDrive.SetDefaultCommand(frc2::RunCommand(
       [this] {
         auto leftXAxis =
@@ -124,7 +130,7 @@ void Robot::CreateRobot() {
 
     m_arm.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_arm.handle_Setpoint(units::degree_t(70));
+        m_arm.handle_Setpoint(units::degree_t(ARM_Angel));
       },
       {&m_arm}));
 
@@ -151,7 +157,14 @@ void Robot::BindCommands() {
         m_arm.Disable();
       },
       {&m_arm}).ToPtr()))
-      .WhileTrue(Shoot(&m_shooter, &m_indexer, &m_intake, &m_arm, -100, 78).ToPtr());
+      .WhileTrue(Shoot(&m_shooter, &m_indexer, &m_intake, &m_arm, ARM_Speed, ARM_Angel).ToPtr());
+
+  frc2::JoystickButton(&m_operatorController, 5).OnFalse(frc2::CommandPtr(frc2::InstantCommand([this] {
+        // m_arm.SetGoal(units::degree_t(m_arm.GetMeasurement()));
+        m_arm.Disable();
+      },
+      {&m_arm}).ToPtr()))
+      .WhileTrue(Shoot(&m_shooter, &m_indexer, &m_intake, &m_arm, -15, 145).ToPtr());
 
     frc2::JoystickButton(&m_operatorController, 3)
       .WhileTrue(intakeTake(&m_intake, &m_indexer, &m_arm).ToPtr());
@@ -194,14 +207,23 @@ void Robot::BindCommands() {
  * Returns the Autonomous Command
  */
 frc2::CommandPtr Robot::GetAutonomousCommand() {
-  return TrajectoryFollower(&m_swerveDrive,
-                            &NKTrajectoryManager::GetTrajectory("NewPath"))
-      .ToPtr();
-  // return frc2::InstantCommand().ToPtr();
+  // return TrajectoryFollower(&m_swerveDrive,
+  //                           &NKTrajectoryManager::GetTrajectory(autoName))
+  //     .ToPtr();
+  return Auto(&m_swerveDrive, &m_shooter, &m_indexer, &m_intake, &m_arm, 1)
+            .ToPtr();
 }
 
 void Robot::DisabledPeriodic() {
   m_arm.Disable();
+
+  if(autoColor.Get()){
+    autoName = 1;
+  }
+  else{
+    autoName = 2;
+  }
+  frc::SmartDashboard::PutNumber("Auto", autoName);
 }
 
 /**
@@ -217,6 +239,9 @@ void Robot::UpdateDashboard() {
   // frc::SmartDashboard::PutNumber("Swerve Drive Heading",
   //                                m_swerveDrive.GetHeading().Degrees().value());
   frc::SmartDashboard::PutBoolean("Note?", m_indexer.hasNote());
+   ARM_Angel = frc::SmartDashboard::GetNumber("ARM_Angel",100);
+   ARM_Speed = frc::SmartDashboard::GetNumber("ARM_Speed",-120);
+
   // m_swerveDrive.PrintNetworkTableValues();
   m_arm.printLog();
 

src/main/cpp/autos/Auto.cpp

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "autos/Auto.h"
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+Auto::Auto(SwerveDrive* m_swerve,
+            Shooter* shooter, Indexer* indexer, Intake* intaker,
+            ArmSubsystem* armer, int autoName) {
+  // Add your commands here, e.g.
+  // AddCommands(FooCommand{}, BarCommand{});
+  switch(autoName){
+  case 1:
+    AddCommands(
+      TrajectoryFollower(m_swerve, &NKTrajectoryManager::GetTrajectory("Red_Near_Loading"))),
+      Shoot(shooter, indexer, intaker, armer, 120, 78);
+    break;
+  case 2:
+    AddCommands(
+      TrajectoryFollower(m_swerve, &NKTrajectoryManager::GetTrajectory("Blue_Near_Loading"))),
+      Shoot(shooter, indexer, intaker, armer, 120, 78);
+      break;
+  }
+}
+
+frc2::CommandHelper<frc2::SequentialCommandGroup,Auto> Blue_Middle(){
+
+}

src/main/cpp/commands/shooter/Shoot.cpp

@@ -6,6 +6,8 @@
 #include "subsystems/Shooter.h"
 #include "subsystems/Indexer.h"
 #include "Constants.hpp"
+#include <frc/smartdashboard/SmartDashboard.h>
+
 
 // Shooter shoooter;
 // Indexer indexing;
@@ -25,6 +27,9 @@ Shoot::Shoot(Shooter* _shooter, Indexer* _indexer, Intake* _intake, ArmSubsystem
 
 // Called when the command is initially scheduled.
 void Shoot::Initialize() {
+
+  shootSpeed = frc::SmartDashboard::GetNumber("ARM_Speed",-120);
+  shootAngle = frc::SmartDashboard::GetNumber("ARM_Angel",100);
   m_state = SPINUP;
 }
 
@@ -37,7 +42,7 @@ void Shoot::Execute() {
   {
     shoooter->Shoot(shootSpeed);//angle is 78
     arm->handle_Setpoint(units::angle::degree_t(shootAngle));
-    if(shoooter->atSetpoint() && arm->GetController().AtGoal())
+    if(shoooter->atSetpoint() && arm->m_ArmState == ArmConstants::DONE)
     {
       m_state = SHOOTING;
     }

src/main/deploy/choreo/Blue_Near_Loading.traj

@@ -0,0 +1,85 @@
+{
+    "samples": [
+        {
+            "x": 0.6893710494041443,
+            "y": 2.369027853012085,
+            "heading": 2.1014725097839467e-34,
+            "angularVelocity": -3.1053705614490077e-34,
+            "velocityX": 2.6938513034065912e-34,
+            "velocityY": -2.1218903755886332e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7673626052514745,
+            "y": 2.3641533698285855,
+            "heading": -2.1870854019937907e-19,
+            "angularVelocity": -2.2591913272463132e-18,
+            "velocityX": 0.8056287393881562,
+            "velocityY": -0.05035190925000365,
+            "timestamp": 0.09680830888252791
+        },
+        {
+            "x": 0.9233457132545645,
+            "y": 2.3544044036923104,
+            "heading": -5.011292711588876e-19,
+            "angularVelocity": -2.9173184450054465e-18,
+            "velocityX": 1.6112574406435274,
+            "velocityY": -0.1007038161167029,
+            "timestamp": 0.19361661776505582
+        },
+        {
+            "x": 1.1573203647997508,
+            "y": 2.339780955141615,
+            "heading": -7.355528752986962e-19,
+            "angularVelocity": -2.4215226546503946e-18,
+            "velocityX": 2.4168860529224094,
+            "velocityY": -0.1510557174223591,
+            "timestamp": 0.29042492664758374
+        },
+        {
+            "x": 1.4692865168187492,
+            "y": 2.320283026868273,
+            "heading": -1.8637538079608215e-18,
+            "angularVelocity": -1.1653968016514798e-17,
+            "velocityX": 3.2225142203191828,
+            "velocityY": -0.20140759092282107,
+            "timestamp": 0.38723323553011163
+        },
+        {
+            "x": 1.7032611683639356,
+            "y": 2.3056595783175773,
+            "heading": -1.9343105694131068e-18,
+            "angularVelocity": -7.288305817701476e-19,
+            "velocityX": 2.4168860529224094,
+            "velocityY": -0.15105571742235913,
+            "timestamp": 0.4840415444126395
+        },
+        {
+            "x": 1.8592442763670256,
+            "y": 2.295910612181302,
+            "heading": -5.638194959631801e-19,
+            "angularVelocity": 1.415674956387752e-17,
+            "velocityX": 1.6112574406435274,
+            "velocityY": -0.10070381611670291,
+            "timestamp": 0.5808498532951675
+        },
+        {
+            "x": 1.9372358322143557,
+            "y": 2.2910361289978027,
+            "heading": 1.210282756012044e-34,
+            "angularVelocity": 5.8240814613095776e-18,
+            "velocityX": 0.8056287393881562,
+            "velocityY": -0.050351909250003664,
+            "timestamp": 0.6776581621776954
+        },
+        {
+            "x": 1.9372358322143557,
+            "y": 2.2910361289978027,
+            "heading": 1.210282756012044e-34,
+            "angularVelocity": 0,
+            "velocityX": -5.864539020364988e-35,
+            "velocityY": 1.7387944579786932e-34,
+            "timestamp": 0.7744664710602233
+        }
+    ]
+}