diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf33d79..efb632b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -147,13 +147,15 @@ private void configureBindings() { joystick.leftBumper().onTrue(s_Elevator.goToL2()).onFalse(s_Elevator.goToL1()); joystick.rightBumper().onTrue(s_Elevator.goToL3()).onFalse(s_Elevator.goToL1()); joystick.leftTrigger().onTrue(s_Elevator.goToL4()).onFalse(s_Elevator.goToL1()); - joystick.povUp().whileTrue(s_Elevator.moveUp()).onFalse(s_Elevator.stop()); - joystick.povDown().whileTrue(s_Elevator.moveDown()).onFalse(s_Elevator.stop()); + joystick.povUp().whileTrue(s_Algae.moveUp()).onFalse(s_Algae.stopPivot()); + joystick.povDown().whileTrue(s_Algae.moveDown()).onFalse(s_Algae.stopPivot()); joystick.back().onTrue(s_Elevator.reZero()); // joystick.x().onTrue(s_Elevator.goToL3A()).onFalse(s_Elevator.goToL1()); joystick.a().onTrue(s_Elevator.goToL2A()).onFalse(s_Elevator.goToL1()); joystick.povLeft().onTrue(s_Algae.extend()); joystick.povRight().onTrue(s_Algae.down()); + joystick.rightStick().onTrue(s_Algae.shoot()).onFalse(s_Algae.stopShooter()); + joystick.leftStick().onTrue(s_Algae.intake()).onFalse(s_Algae.stopShooter()); diff --git a/src/main/java/frc/robot/subsystems/algae/Algae.java b/src/main/java/frc/robot/subsystems/algae/Algae.java index 3ad5f17..0ce697e 100644 --- a/src/main/java/frc/robot/subsystems/algae/Algae.java +++ b/src/main/java/frc/robot/subsystems/algae/Algae.java @@ -27,29 +27,41 @@ public Algae(AlgaeIO io) { // setDefaultCommand(stop()); } + public Command shoot() { + return this.runOnce(() -> m_io.setShootVoltage(1)).withName("Shoot"); + } + + public Command stopShooter() { + return this.runOnce(() -> m_io.setShootVoltage(0)).withName("Stop"); + } + + public Command intake() { + return this.runOnce(() -> m_io.setShootVoltage(-1)).withName("Intake"); + } + public Command moveUp() { - return this.runOnce(() -> m_io.setVoltage(1)).withName("UP"); + return this.runOnce(() -> m_io.setPivotVoltage(1)).withName("Move Up"); } - public Command stop() { - return this.runOnce(() -> m_io.setVoltage(0)).withName("Stop"); + public Command stopPivot() { + return this.runOnce(() -> m_io.setPivotVoltage(0)).withName("Stop"); } public Command moveDown() { - return this.runOnce(() -> m_io.setVoltage(-1)).withName("Move Down"); + return this.runOnce(() -> m_io.setPivotVoltage(-1)).withName("Move Down"); } public Command down(){ // return this.run(() -> m_io.gotosetpoint(L1Setpoint,m_gearRatio)); return this.runOnce(() -> { m_io.gotosetpoint(DownPosition, m_gearRatio); - }).withName("L1"); + }).withName("Down"); } public Command extend(){ // return this.run(() -> m_io.gotosetpoint(L1Setpoint,m_gearRatio)); return this.runOnce(() -> { m_io.gotosetpoint(ExtendedPosition, m_gearRatio); - }).withName("L2"); + }).withName("Extend"); } public Command reZero(){ return this.runOnce(() -> { diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java index f700884..88f9df1 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java @@ -12,7 +12,9 @@ public static class AlgaeIOInputs { public double voltage = 0d; } - public default void setVoltage(double voltage) {} + public default void setShootVoltage(double voltage) {} + + public default void setPivotVoltage(double voltage) {} public default void setBrakeMode(boolean enableBrakeMode) {} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIOTalonFX.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIOTalonFX.java index e6a50c4..09aac6e 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIOTalonFX.java @@ -23,7 +23,8 @@ public class AlgaeIOTalonFX extends SubsystemBase implements AlgaeIO{ - private final TalonFX m_algaemotor; + private final TalonFX m_algaeshootmotor; + private final TalonFX m_algaepivotmotor; private double m_rotations; final PositionVoltage m_request; //final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); @@ -33,7 +34,8 @@ public AlgaeIOTalonFX() { - m_algaemotor = new TalonFX(23, "CANivore2"); + m_algaeshootmotor = new TalonFX(23, "CANivore2"); + m_algaepivotmotor = new TalonFX(24, "CANivore2"); m_request = new PositionVoltage(0).withSlot(0); @@ -50,43 +52,54 @@ public AlgaeIOTalonFX() { slot0Configs.kD = 0; // A velocity of 1 rps results in 0.1 V output slot0Configs.kG = 0.5; - m_algaemotor.setPosition(0); - m_algaemotor.getConfigurator().apply(algaeMotorConfig); - m_algaemotor.getConfigurator().apply(outputConfigs); - m_algaemotor.getConfigurator().apply(slot0Configs); + m_algaepivotmotor.setPosition(0); + m_algaeshootmotor.getConfigurator().apply(algaeMotorConfig); + m_algaepivotmotor.getConfigurator().apply(algaeMotorConfig); + m_algaeshootmotor.getConfigurator().apply(outputConfigs); + m_algaepivotmotor.getConfigurator().apply(outputConfigs); + m_algaeshootmotor.getConfigurator().apply(slot0Configs); + m_algaepivotmotor.getConfigurator().apply(slot0Configs); } - public void setVoltage(double voltage) { + public void setShootVoltage(double voltage) { // return this.runOnce(()->{ final VoltageOut request = new VoltageOut(0); - m_algaemotor.setControl(request.withOutput(voltage)); + m_algaeshootmotor.setControl(request.withOutput(voltage)); // }); - System.out.println("volts:" + m_algaemotor.getMotorVoltage()); + System.out.println("volts:" + m_algaeshootmotor.getMotorVoltage()); + } + + public void setPivotVoltage(double voltage) { + // return this.runOnce(()->{ + final VoltageOut request = new VoltageOut(0); + m_algaepivotmotor.setControl(request.withOutput(voltage)); + // }); + System.out.println("volts:" + m_algaepivotmotor.getMotorVoltage()); } public void setBrakeMode(boolean enableBrakeMode) { final NeutralModeValue neutralModeValue = enableBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast; - m_algaemotor.setNeutralMode(neutralModeValue); + m_algaeshootmotor.setNeutralMode(neutralModeValue); } @Override public void gotosetpoint(double setpoint, double gearRatio) { double rotations = setpoint * gearRatio; m_rotations = rotations; System.out.println("rotations:" + rotations); - m_algaemotor.setControl(m_request.withPosition(rotations)); + m_algaepivotmotor.setControl(m_request.withPosition(rotations)); } public double getVelocity(){ - return m_algaemotor.getVelocity().getValueAsDouble(); + return m_algaeshootmotor.getVelocity().getValueAsDouble(); } public double getVoltage(){ - return m_algaemotor.getMotorVoltage().getValueAsDouble(); + return m_algaeshootmotor.getMotorVoltage().getValueAsDouble(); } @Override public double getPosition() { - return m_algaemotor.getPosition().getValueAsDouble(); + return m_algaeshootmotor.getPosition().getValueAsDouble(); } public double getSetpoint(){ @@ -94,7 +107,7 @@ public double getSetpoint(){ } public void setPosition(double position){ - m_algaemotor.setPosition(position); + m_algaeshootmotor.setPosition(position); } }