diff --git a/src/main/java/frc/robot/IntakeConstants.java b/src/main/java/frc/robot/IntakeConstants.java new file mode 100644 index 0000000..8fc71b7 --- /dev/null +++ b/src/main/java/frc/robot/IntakeConstants.java @@ -0,0 +1,6 @@ +package frc.robot; + +public class IntakeConstants { + public static final double IntakeDroppedAngle = 9.0; + public static final double IntakeRaisedAngle = 9.0; +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8cfa519..cab2f9f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,14 +10,32 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class RobotContainer { public final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + public final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); + private boolean IntakeDropped = false; + private boolean lastAButton = false; + private XboxController controller = new XboxController(0); public RobotContainer() { configureBindings(); + m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),m_IntakeSubsystem)); + } + + public void periodic(){ + if(controller.getAButton()){ + lastAButton = true; + if (!lastAButton) + IntakeDropped = !IntakeDropped; + } + else { + lastAButton = false; + } } private void configureBindings() { diff --git a/src/main/java/frc/robot/subsystems/intake.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java similarity index 86% rename from src/main/java/frc/robot/subsystems/intake.java rename to src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c0bd4b4..e91f9f2 100644 --- a/src/main/java/frc/robot/subsystems/intake.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Intake extends SubsystemBase { +public class IntakeSubsystem extends SubsystemBase { //TODO: add constants CANSparkFlex intakeMotor = new CANSparkFlex(0, MotorType.kBrushless); CANSparkFlex armMotor = new CANSparkFlex(0, MotorType.kBrushless); @@ -39,15 +39,22 @@ public class Intake extends SubsystemBase { /** Creates a new intake. */ - public Intake() { + public IntakeSubsystem() { } - - void load(double speed){ + /** + * + * @param speed motor power to apply to intake + *@param angle in radians + * + */ + public void load(double speed, double angle){ intakeMotor.set(speed); - //TODO: Implement load/unload through speed (which is a trigger) + tiltToAngle(angle); + } /** + * *@param angle in radians *@everyone join vc we are playing gartic phone */ @@ -56,6 +63,7 @@ public void tiltToAngle(double angle) { double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle); armMotor.set(motorPower); } + @Override