forked from Jagwires7443/Swerve
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #6 from Team2559/IntakeCommand
Intake command
- Loading branch information
Showing
11 changed files
with
322 additions
and
46 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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(); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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}; | ||
|
|
||
| }; |
Oops, something went wrong.