Intake command

src/main/cpp/RobotContainer.cpp

@@ -16,11 +16,14 @@
 #include <frc2/command/CommandScheduler.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/RunCommand.h>
+#include "commands/IntakeCommand.h"
 #include "commands/ClimberLowerCommand.h"
 #include "commands/ClimberRaiseCommand.h"
 #include "commands/ClimberStopCommand.h"
 #include "commands/ShootCommands.h"
 #include "commands/PositionTransferArmCommand.h"
+#include "commands/PIDTransferArmCommand.h"
+#include "commands/IntakeEjectCommand.h"
 
 #include <cmath>
 #include <cstdio>
@@ -263,15 +266,19 @@ void RobotContainer::ConfigureBindings() noexcept
                            {})
           .ToPtr());
 
+  /* Commented out, changing A to run the IntakeCommand for picking up a note
   m_xbox.A().OnTrue(frc2::InstantCommand([&]() -> void
                                          { m_intakeSubsystem.SetSpinMotorVoltagePercent(intake::kIntakeSpinMotorVoltagePercent); },
                                          {&m_intakeSubsystem})
                         .ToPtr());
   m_xbox.A().OnFalse(frc2::InstantCommand([&]() -> void
                                           { m_intakeSubsystem.StopIntake(); },
                                           {&m_intakeSubsystem})
-                         .ToPtr());
+                         .ToPtr());  */
+  m_xbox.A().OnTrue(IntakeCommand(&m_intakeSubsystem).ToPtr());
+
 
+  /* Commented out, shooter functions are in Y now.  Reusing B for the intake eject command
   m_xbox.B().OnTrue(frc2::InstantCommand([&]() -> void
                                          { m_shooterSubsystem.SetShooterMotorVoltagePercent(shooter::kShooterMotorVoltagePercent); },
                                          {&m_shooterSubsystem})
@@ -280,20 +287,33 @@ void RobotContainer::ConfigureBindings() noexcept
                                           { m_shooterSubsystem.StopShooter(); },
                                           {&m_shooterSubsystem})
                          .ToPtr());
+                         */
+
+  m_xbox.B().OnTrue(IntakeEjectCommand(&m_intakeSubsystem).ToPtr());
+  /*                  
+  m_xbox.B().OnTrue(frc2::InstantCommand([&]() -> void
+                                         { m_intakeSubsystem.SetSpinMotorVoltagePercent(intake::kIntakeSpinMotorEjectVoltagePercent); },
+                                         {&m_intakeSubsystem})
+                        .ToPtr());
+  m_xbox.B().OnFalse(frc2::InstantCommand([&]() -> void
+                                          { m_intakeSubsystem.StopIntake(); },
+                                          {&m_intakeSubsystem})
+                         .ToPtr());
+*/
 
+  // Runs shoot command to move arm into postion, start up the shooting motors and eject the note                     
   m_xbox.Y().OnTrue(ShootCommands(&m_shooterSubsystem).ToPtr());
-                         
+
   m_xbox.X().OnTrue(PositionTransferArm(&m_transferArmSubsystem, 90_deg).ToPtr()); // Example Only
+
+  m_xbox.LeftBumper().OnTrue(PIDPositionTransferArm(0_deg, &m_transferArmSubsystem).ToPtr()); // Intake
+  m_xbox.RightBumper().OnTrue(PIDPositionTransferArm(arm::kIntakeToShooterAngle, &m_transferArmSubsystem).ToPtr()); // Shooter
 
-  m_xbox.LeftBumper().WhileTrue(ClimberRaiseCommand(&m_climberSubsystem).ToPtr()); // Raise the climber while button is pressed.
-  m_xbox.LeftBumper().OnFalse(ClimberStopCommand(&m_climberSubsystem).ToPtr());   // on false stop the climber motor
+  m_xbox.RightTrigger().WhileTrue(ClimberRaiseCommand(&m_climberSubsystem).ToPtr()); // Raise the climber while button is pressed.
+  m_xbox.RightTrigger().OnFalse(ClimberStopCommand(&m_climberSubsystem).ToPtr());   // on false stop the climber motor
 
   m_xbox.LeftTrigger().WhileTrue(ClimberLowerCommand(&m_climberSubsystem).ToPtr()); //Lower the climber while button is pressed
   m_xbox.LeftTrigger().OnFalse(ClimberStopCommand(&m_climberSubsystem).ToPtr());   // on false stop the climber motor
-
-
-
-
 }
 #pragma endregion
 

src/main/cpp/commands/IntakeCommand.cpp

@@ -0,0 +1,51 @@
+// 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 "commands/IntakeCommand.h"
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/DataLogManager.h>
+#include <frc/RobotController.h>
+#include <frc/shuffleboard/BuiltInWidgets.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/shuffleboard/ShuffleboardLayout.h>
+#include <frc/shuffleboard/ShuffleboardTab.h>
+#include <frc/shuffleboard/SimpleWidget.h>
+
+// Called when the command is initially scheduled.
+void IntakeCommand::Initialize() {
+  /* Start the intake motors.
+    Set finished to false to allow multiple calls of this command.
+  */
+  intakeSubsystem->SetSpinMotorVoltagePercent(intake::kIntakeSpinMotorVoltagePercent);
+  finished = false;
+
+  frc::SmartDashboard::PutBoolean("Limit1 Boolean Value: ", limit1.Get());
+  frc::SmartDashboard::PutBoolean("Limit2 Boolean Value: ", limit2.Get());
+}
+
+// Called repeatedly when this Command is scheduled to run
+void IntakeCommand::Execute() {
+  /* Checks if the limitswitches have been activated.  If so sets finished to true and intake to stop.
+    Need to add code for moving arm pack to home position. */
+  frc::SmartDashboard::PutBoolean("Limit1 Boolean Value: ", limit1.Get());
+  frc::SmartDashboard::PutBoolean("Limit2 Boolean Value: ", limit2.Get());
+
+  if (limit1.Get() or limit2.Get()){
+
+    finished = true;
+    intakeSubsystem->StopIntake();
+    //Add code to raise the arm to the home position
+  }
+
+}
+
+// Called once the command ends or is interrupted.
+void IntakeCommand::End(bool interrupted) {
+
+}
+
+// Returns true when the command should end.
+bool IntakeCommand::IsFinished() {
+  return finished;
+}

src/main/cpp/commands/IntakeEjectCommand.cpp

@@ -0,0 +1,37 @@
+// 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 "commands/IntakeEjectCommand.h"
+
+
+// Called when the command is initially scheduled.
+void IntakeEjectCommand::Initialize() {
+  /* Start the intake motors in the reverse direction
+     Set finished to false to allow multiple calls of this command
+  */
+  intakeSubsystem->SetSpinMotorVoltagePercent(intake::kIntakeSpinMotorEjectVoltagePercent);
+  finished = false;
+  timer.Reset();
+  timer.Start();
+}
+
+// Called repeatedly when this Command is scheduled to run
+void IntakeEjectCommand::Execute() {
+  //Run the intake motors in reverse for 2 seconds then stop the intake
+  if (timer.HasElapsed(2_s)){
+    finished = true;
+    intakeSubsystem->StopIntake();
+  }
+}
+
+// Called once the command ends or is interrupted.
+void IntakeEjectCommand::End(bool interrupted) {
+  //Stop the intake motors
+  intakeSubsystem->StopIntake();
+}
+
+// Returns true when the command should end.
+bool IntakeEjectCommand::IsFinished() {
+  return finished;
+}

src/main/cpp/commands/PIDTransferArmCommand.cpp

@@ -0,0 +1,44 @@
+// 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 "commands/PIDTransferArmCommand.h"
+#include <frc/smartdashboard/SmartDashboard.h>
+
+PIDPositionTransferArm::PIDPositionTransferArm(units::turn_t targetPosition, TransferArmSubsystem* transferArmSubsystem)
+    : CommandHelper{frc::PIDController{arm::kArmPositionP, 0, arm::kArmPositionD},
+                    // Close loop on heading
+                    [transferArmSubsystem]
+                    {
+                        frc::SmartDashboard::PutNumber("Arm Position ", transferArmSubsystem->GetTransferArmPosition().value());
+                        return transferArmSubsystem->GetTransferArmPosition().value();
+                    },
+                    // Set reference to targetPosition
+                    targetPosition.value(),
+                    // Pipe output to turn transfer arm
+                    [transferArmSubsystem](double output)
+                    {
+                        frc::SmartDashboard::PutNumber("Arm PID Output ", output);
+                        transferArmSubsystem->SetArmMotorVoltagePercent(output);
+                    },
+                    // Require the transfer arm
+                    {transferArmSubsystem}} {
+  // Set the controller to be continuous (because it is an angle controller)
+  m_controller.EnableContinuousInput(-1, 1);
+  // Set the controller tolerance - the delta tolerance ensures the robot is
+  // stationary at the setpoint before it is considered as having reached the
+  // reference
+  m_controller.SetTolerance(.01, .01);
+
+  AddRequirements(transferArmSubsystem);
+
+    frc::SmartDashboard::PutBoolean("PID Command Reset ", true);
+}
+
+bool PIDPositionTransferArm::IsFinished() {
+    if(m_controller.AtSetpoint())
+    {
+        frc::SmartDashboard::PutBoolean("PID Command Reset ", false);
+    }
+    return m_controller.AtSetpoint();
+}

src/main/cpp/commands/PositionTransferArmCommand.cpp

@@ -3,21 +3,26 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include "commands/PositionTransferArmCommand.h"
+#include <frc/smartdashboard/SmartDashboard.h>
 
 void PositionTransferArm::Initialize() noexcept
 {
-    transferArmSubsystem->SetTransferArmPosition(position);
+    finished = false;
+
+    // transferArmSubsystem->SetTransferArmPosition(position);
     timer.Reset();
     timer.Start();
 }
 
 void PositionTransferArm::Execute() noexcept
 {
-    transferArmSubsystem->UpdatePIDValues();
-    if (timer.HasElapsed(5_s)) { finished = true; }
+    frc::SmartDashboard::PutNumber("Arm Timer ", timer.Get().value());
+    // transferArmSubsystem->UpdatePIDValues();s
+    transferArmSubsystem->SetArmMotorVoltagePercent(.05);
+    if (timer.HasElapsed(2_s)) { finished = true; }
 }
 
 void PositionTransferArm::End(bool interrupted) noexcept
 {
-
+    transferArmSubsystem->StopTransferArm();
 }

src/main/cpp/subsystems/TransferArmSubsystem.cpp

@@ -6,18 +6,22 @@ TransferArmSubsystem::TransferArmSubsystem() noexcept
 {
     m_TransferArmMotor.SetInverted(arm::kTransferArmMotorIsInverted);
 
+    m_encoder.SetMeasurementPeriod(10);
+
     // set PID coefficients
-    m_pidController.SetP(kP);
-    m_pidController.SetI(kI);
-    m_pidController.SetD(kD);
-    m_pidController.SetIZone(kIz);
-    m_pidController.SetFF(kFF);
-    m_pidController.SetOutputRange(kMinOutput, kMaxOutput);
+    // m_pidController.SetP(kP);
+    // m_pidController.SetI(kI);
+    // m_pidController.SetD(kD);
+    // m_pidController.SetIZone(kIz);
+    // m_pidController.SetFF(kFF);
+    // m_pidController.SetOutputRange(kMinOutput, kMaxOutput);
 
     // display PID coefficients on SmartDashboard
-    frc::SmartDashboard::PutNumber("Arm P Gain ", kP);
-    frc::SmartDashboard::PutNumber("Arm D Gain ", kD);
-    frc::SmartDashboard::PutNumber("Arm F ", kFF);
+    // frc::SmartDashboard::PutNumber("Arm P Gain ", kP);
+    // frc::SmartDashboard::PutNumber("Arm D Gain ", kD);
+    // frc::SmartDashboard::PutNumber("Arm F ", kFF);
+
+    frc::SmartDashboard::PutNumber("Arm Init Position ", m_encoder.GetPosition());
 
     StopTransferArm();
 }
@@ -27,26 +31,32 @@ void TransferArmSubsystem::StopTransferArm() noexcept
     m_TransferArmMotor.StopMotor();
 }
 
-void TransferArmSubsystem::SetTransferArmPosition(const units::turn_t position) noexcept
+void TransferArmSubsystem::SetArmMotorVoltagePercent(const double percent) noexcept
 {
-    m_pidController.SetReference(position.value(), rev::CANSparkMax::ControlType::kPosition);
-    frc::SmartDashboard::PutNumber("SetPoint", position.value());
-    // frc::SmartDashboard::PutNumber("ProcessVariable", m_encoder.GetPosition());
+    m_TransferArmMotor.SetVoltage(percent * 12_V);
 }
 
-// units::turn_t TransferArmSubsystem::GetTransferArmPosition() noexcept
+// void TransferArmSubsystem::SetTransferArmPosition(const units::turn_t position) noexcept
 // {
-//     return units::turn_t(m_encoder.GetPosition());
+//     m_pidController.SetReference(position.value(), rev::CANSparkMax::ControlType::kPosition);
+//     frc::SmartDashboard::PutNumber("SetPoint", position.value());
+//     frc::SmartDashboard::PutNumber("ProcessVariable", m_encoder.GetPosition());
 // }
 
-void TransferArmSubsystem::UpdatePIDValues() noexcept
+units::turn_t TransferArmSubsystem::GetTransferArmPosition() noexcept
 {
-    double p = frc::SmartDashboard::GetNumber("Arm P Gain ", 0);
-    double d = frc::SmartDashboard::GetNumber("Arm D Gain ", 0);
-    double ff = frc::SmartDashboard::GetNumber("Arm F ", 0);
-
-    // if PID coefficients on SmartDashboard have changed, write new values to controller
-    if((p != kP)) { m_pidController.SetP(p); kP = p; }
-    if((d != kD)) { m_pidController.SetD(d); kD = d; }
-    if((ff != kFF)) { m_pidController.SetFF(ff); kFF = ff; }
+    frc::SmartDashboard::PutNumber("Arm Position ", m_encoder.GetPosition());
+    return units::turn_t(m_encoder.GetPosition());
 }
+
+// void TransferArmSubsystem::UpdatePIDValues() noexcept
+// {
+//     double p = frc::SmartDashboard::GetNumber("Arm P Gain ", 0);
+//     double d = frc::SmartDashboard::GetNumber("Arm D Gain ", 0);
+//     double ff = frc::SmartDashboard::GetNumber("Arm F ", 0);
+
+//     // if PID coefficients on SmartDashboard have changed, write new values to controller
+//     if((p != kP)) { m_pidController.SetP(p); kP = p; }
+//     if((d != kD)) { m_pidController.SetD(d); kD = d; }
+//     if((ff != kFF)) { m_pidController.SetFF(ff); kFF = ff; }
+// }

src/main/include/Constants.h

@@ -152,19 +152,22 @@ namespace intake
     constexpr units::degree_t kIntakeArmPickup = 180.0_deg;
     constexpr units::degree_t kIntakeArmLoad = 0.0_deg;
 
-    constexpr double kIntakeSpinMotorVoltagePercent = .15;
+    constexpr double kIntakeSpinMotorVoltagePercent = .75;
+    constexpr double kIntakeSpinMotorEjectVoltagePercent = -.90;
 }
 
 namespace arm
 {
     // Arm Motor Parameters
     constexpr int kTransferArmMotorCanID = 17;
-    constexpr bool kTransferArmMotorIsInverted = false;
+    constexpr bool kTransferArmMotorIsInverted = true;
 
     // Arm Controller
-    constexpr double kArmPositionP = 0.005;
-    constexpr double kArmPositionD = 0.0005;
+    constexpr double kArmPositionP = 10.0;
+    constexpr double kArmPositionD = .10;
     constexpr double kArmPositionF = 0.0;
+
+    constexpr units::turn_t kIntakeToShooterAngle = 200_deg;
 }
 
 namespace climber

src/main/include/commands/IntakeCommand.h

@@ -0,0 +1,37 @@
+// 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.
+
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include "frc/DigitalInput.h"
+#include "subsystems/IntakeSubsystem.h"
+#include "Constants.h"
+#include "subsystems/TransferArmSubsystem.h"
+
+/**
+ * This command is for picking up a note and moving to the home position with the arm
+ */
+class IntakeCommand
+    : public frc2::CommandHelper<frc2::Command, IntakeCommand> {
+ public:
+  explicit IntakeCommand(IntakeSubsystem *intakeSubsystem) 
+      : intakeSubsystem{intakeSubsystem}
+      {
+        AddRequirements(intakeSubsystem);
+      }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+  private:
+    IntakeSubsystem *intakeSubsystem{nullptr};
+    bool finished{false};
+    frc::DigitalInput limit1{8};
+    frc::DigitalInput limit2{9};
+
+};