diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index fcbbbce..38d0dba 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -16,11 +16,14 @@ #include #include #include +#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 #include @@ -263,6 +266,7 @@ 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}) @@ -270,8 +274,11 @@ void RobotContainer::ConfigureBindings() noexcept 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 diff --git a/src/main/cpp/commands/IntakeCommand.cpp b/src/main/cpp/commands/IntakeCommand.cpp new file mode 100644 index 0000000..0041cf6 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +// 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; +} diff --git a/src/main/cpp/commands/IntakeEjectCommand.cpp b/src/main/cpp/commands/IntakeEjectCommand.cpp new file mode 100644 index 0000000..fd52729 --- /dev/null +++ b/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; +} diff --git a/src/main/cpp/commands/PIDTransferArmCommand.cpp b/src/main/cpp/commands/PIDTransferArmCommand.cpp new file mode 100644 index 0000000..9bd367e --- /dev/null +++ b/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 + +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(); +} diff --git a/src/main/cpp/commands/PositionTransferArmCommand.cpp b/src/main/cpp/commands/PositionTransferArmCommand.cpp index 558acae..c6d13fd 100644 --- a/src/main/cpp/commands/PositionTransferArmCommand.cpp +++ b/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 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(); } diff --git a/src/main/cpp/subsystems/TransferArmSubsystem.cpp b/src/main/cpp/subsystems/TransferArmSubsystem.cpp index b3137b1..5cb3cd8 100644 --- a/src/main/cpp/subsystems/TransferArmSubsystem.cpp +++ b/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; } +// } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 115e10b..33eb008 100644 --- a/src/main/include/Constants.h +++ b/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 diff --git a/src/main/include/commands/IntakeCommand.h b/src/main/include/commands/IntakeCommand.h new file mode 100644 index 0000000..6e2af3f --- /dev/null +++ b/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 +#include +#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 { + 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}; + +}; diff --git a/src/main/include/commands/IntakeEjectCommand.h b/src/main/include/commands/IntakeEjectCommand.h new file mode 100644 index 0000000..79d5c43 --- /dev/null +++ b/src/main/include/commands/IntakeEjectCommand.h @@ -0,0 +1,38 @@ +// 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 +#include +#include "subsystems/IntakeSubsystem.h" +#include "Constants.h" +#include + + + +/** + * This command is for ejecting the note from the intake. + * This can be called by the shootcommand to feed into the shooter + * Or used to eject a note onto the field + */ +class IntakeEjectCommand + : public frc2::CommandHelper { + public: + explicit IntakeEjectCommand(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::Timer timer{}; +}; diff --git a/src/main/include/commands/PIDTransferArmCommand.h b/src/main/include/commands/PIDTransferArmCommand.h new file mode 100644 index 0000000..753def0 --- /dev/null +++ b/src/main/include/commands/PIDTransferArmCommand.h @@ -0,0 +1,30 @@ +// 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 +#include +#include +#include +#include +#include +#include "subsystems/TransferArmSubsystem.h" +#include "Constants.h" + +/** + * A command that will turn the transfer arm to the specified angle. + */ +class PIDPositionTransferArm : public frc2::CommandHelper { + public: + /** + * Turns the transfer arm to robot to the specified angle. + * + * @param targetAngleDegrees The angle to turn to + * @param transferArmSubsystem The transferArmSubsystem subsystem to use + */ + PIDPositionTransferArm(units::turn_t targetPosition, TransferArmSubsystem* transferArmSubsystem); + + bool IsFinished() override; +}; diff --git a/src/main/include/subsystems/TransferArmSubsystem.h b/src/main/include/subsystems/TransferArmSubsystem.h index 61fd090..b74f814 100644 --- a/src/main/include/subsystems/TransferArmSubsystem.h +++ b/src/main/include/subsystems/TransferArmSubsystem.h @@ -14,13 +14,14 @@ class TransferArmSubsystem : public frc2::SubsystemBase TransferArmSubsystem &operator=(const TransferArmSubsystem &) = delete; void StopTransferArm() noexcept; - void SetTransferArmPosition(const units::turn_t position) noexcept; - // units::turn_t GetTransferArmPosition() noexcept; - void UpdatePIDValues() noexcept; + void SetArmMotorVoltagePercent(const double percent) noexcept; + // void SetTransferArmPosition(const units::turn_t position) noexcept; + units::turn_t GetTransferArmPosition() noexcept; + // void UpdatePIDValues() noexcept; private: rev::CANSparkMax m_TransferArmMotor{arm::kTransferArmMotorCanID, rev::CANSparkMax::MotorType::kBrushless}; - // rev::CANEncoder m_encoder = m_TransferArmMotor.GetAlternateEncoder(rev::CANEncoder::AlternateEncoderType::kQuadrature, 8192); - rev::SparkPIDController m_pidController = m_TransferArmMotor.GetPIDController(); - double kP = arm::kArmPositionP, kI = 0, kD = arm::kArmPositionD, kIz = 0, kFF = arm::kArmPositionF, kMaxOutput = 1, kMinOutput = -1; + rev::SparkMaxAlternateEncoder m_encoder = m_TransferArmMotor.GetAlternateEncoder(8192); + // rev::SparkPIDController m_pidController = m_TransferArmMotor.GetPIDController(); + // double kP = arm::kArmPositionP, kI = 0, kD = arm::kArmPositionD, kIz = 0, kFF = arm::kArmPositionF, kMaxOutput = 1, kMinOutput = -1; };