manually found the position for trap climbing

TitaniumTitans · Mar 18, 2024 · 0696bcd · 0696bcd
1 parent 4f00d21
commit 0696bcd
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -190,11 +190,11 @@ private void configureButtonBindings() {
 
     passSpinUpTrigger.whileTrue(
         m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS)
-            .alongWith(m_shooter.runShooterVelocity(false, 3500, 3500)));
+            .alongWith(m_shooter.runShooterVelocity(false, () -> 3500, () -> 3500)));
 
     passTrigger.whileTrue(
         m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS)
-            .alongWith(m_shooter.runShooterVelocity(true, 3500, 3500)));
+            .alongWith(m_shooter.runShooterVelocity(true, () -> 3500, () -> 3500)));
 
     m_driverController.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP));
     m_driverController.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE));
@@ -230,15 +230,19 @@ private void configureButtonBindings() {
     m_operatorController.y().onTrue(m_armSubsystem.incrementWristManual(-m_armIncrement.get()));
 
     m_operatorController.leftTrigger().whileTrue(
-        m_shooter.runShooterVelocity(false, m_leftPower.get(), m_rightPower.get()));
+        m_shooter.runShooterVelocity(false, m_leftPower::get, m_rightPower::get));
     m_operatorController.rightTrigger().whileTrue(
-        m_shooter.runShooterVelocity(true, m_leftPower.get(), m_rightPower.get()));
+        m_shooter.runShooterVelocity(true, m_leftPower::get, m_rightPower::get));
 
-    m_operatorController.leftBumper().whileTrue(m_climber.setClimberPosition(10));
-    m_operatorController.rightBumper().whileTrue(m_climber.setClimberPosition(720));
+    m_operatorController.leftBumper().whileTrue(m_climber.setClimberPosition(10.0));
+    m_operatorController.rightBumper().whileTrue(m_climber.setClimberPosition(360.0 * 10.0));
 
-    m_operatorController.pov(0).whileTrue(m_climber.setLeftClimberPowerFactory(0.5));
-    m_operatorController.pov(90).whileTrue(m_climber.setLeftClimberPowerFactory(-0.5));
+    // arm 45.0
+    // wrist 123.5]\[
+
+
+    m_operatorController.pov(0).whileTrue(m_climber.setClimberPowerFactory(0.5));
+    m_operatorController.pov(90).whileTrue(m_climber.setClimberPowerFactory(-0.5));
     m_operatorController.pov(180).whileTrue(m_climber.setRightClimberPowerFactory(0.5));
     m_operatorController.pov(270).whileTrue(m_climber.setRightClimberPowerFactory(-0.5));
 

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
@@ -5,6 +5,8 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
+import java.util.function.DoubleSupplier;
+
 public class ShooterSubsystem extends SubsystemBase {
 
   private final ShooterIO m_io;
@@ -45,13 +47,13 @@ public void setIntakePower(double power) {
   }
 
   public Command runShooterVelocity(boolean runKicker) {
-    return runShooterVelocity(runKicker, 3750, 4500);
+    return runShooterVelocity(runKicker, () -> 3750, () -> 4500);
   }
 
-  public Command runShooterVelocity(boolean runKicker, double leftRPM, double rightRPM) {
+  public Command runShooterVelocity(boolean runKicker, DoubleSupplier leftRPM, DoubleSupplier rightRPM) {
     return runEnd(() -> {
-          m_leftSpeedSetpoint = leftRPM;
-          m_rightSpeedSetpoint = rightRPM;
+          m_leftSpeedSetpoint = leftRPM.getAsDouble();
+          m_rightSpeedSetpoint = rightRPM.getAsDouble();
 
           m_io.setLeftVelocityRpm(m_leftSpeedSetpoint);
           m_io.setRightVelocityRpm(m_rightSpeedSetpoint);