diff --git a/src/ConveyorSubsystem.cpp b/src/ConveyorSubsystem.cpp index 9d66dca..b9e4bfc 100644 --- a/src/ConveyorSubsystem.cpp +++ b/src/ConveyorSubsystem.cpp @@ -22,27 +22,45 @@ void ConveyorSubsystem::robotInit(){ operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::RIGHT_BUTTON); operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER); operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON); + operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::Y_BUTTON); } -void ConveyorSubsystem::teleopInit() {} +void ConveyorSubsystem::teleopInit() { + m_autoConveyor = false; +} void ConveyorSubsystem::teleop(){ - - if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER)) { - setLowerMotor(lowerConveyorSpeed.Get()); - } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON)) { - setLowerMotor(-lowerConveyorSpeed.Get()); - } else { - setLowerMotor(0.0); - } - - if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER)) { - setUpperMotor(upperConveyorSpeed.Get()); - } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON)) { - setUpperMotor(-upperConveyorSpeed.Get()); - } else { - setUpperMotor(0.0); + bool ballDetected = m_input.Get(); + frc::SmartDashboard::PutBoolean("Ball Detected", ballDetected); + + if (operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::Y_BUTTON)) { + if (ballDetected){ + setLowerMotor(-lowerConveyorSpeed.Get()); + setUpperMotor(-upperConveyorSpeed.Get()); + } else { + setLowerMotor(0.0); + setUpperMotor(0.0); + } + m_autoConveyor = true; + } else if (!operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::Y_BUTTON)) { + if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER)) { + setLowerMotor(lowerConveyorSpeed.Get()); + } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON)) { + setLowerMotor(-lowerConveyorSpeed.Get()); + } else { + setLowerMotor(0.0); + } + + if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER)) { + setUpperMotor(upperConveyorSpeed.Get()); + } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON)) { + setUpperMotor(-upperConveyorSpeed.Get()); + } else { + setUpperMotor(0.0); + } + } + frc::SmartDashboard::PutBoolean("Conveyor Auto Run", m_autoConveyor); } void ConveyorSubsystem::setLowerMotor(double lowerConveyorSpeed){ diff --git a/src/ConveyorSubsystem.h b/src/ConveyorSubsystem.h index 08c8827..772601f 100644 --- a/src/ConveyorSubsystem.h +++ b/src/ConveyorSubsystem.h @@ -2,6 +2,8 @@ #include #include +#include +#include #include "Config.h" class ConveyorSubsystem : public CORESubsystem { @@ -16,5 +18,7 @@ class ConveyorSubsystem : public CORESubsystem { COREConstant lowerConveyorSpeed, upperConveyorSpeed; private: TalonSRX m_lowerConveyorMotor, m_frontConveyorMotor, m_backConveyorMotor; + frc::DigitalInput m_input{0}; + bool m_autoConveyor; }; diff --git a/src/IntakeSubsystem.cpp b/src/IntakeSubsystem.cpp index 3461f36..4099a39 100644 --- a/src/IntakeSubsystem.cpp +++ b/src/IntakeSubsystem.cpp @@ -1,9 +1,9 @@ #include "IntakeSubsystem.h" IntakeSubsystem::IntakeSubsystem() : + intakeSpeed("Intake Speed", 0.45), m_intakeMotor(INTAKE_PORT), - m_intakeSolenoid(LEFT_DRIVE_SHIFTER_PCM, INTAKE_DOWN_PORT, INTAKE_UP_PORT), - intakeSpeed("Intake Speed", 0.45){ + m_intakeSolenoid(LEFT_DRIVE_SHIFTER_PCM, INTAKE_DOWN_PORT, INTAKE_UP_PORT){ } void IntakeSubsystem::robotInit(){