not tested

team4909 · Feb 5, 2025 · f4749b2 · f4749b2
1 parent 1d9e8d5
commit f4749b2
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 24 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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,51 +52,62 @@ 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(){
         return m_rotations;
     }
 
     public void setPosition(double position){
-        m_algaemotor.setPosition(position);
+        m_algaeshootmotor.setPosition(position);
     }
 
 }