getting closer to fixing the setpoints

team4909 · Jan 14, 2025 · b8e6ad8 · b8e6ad8
1 parent c237328
commit b8e6ad8
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -69,18 +69,18 @@ private void configureBindings() {
             ));
 
     joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
-    joystick
-        .b()
-        .whileTrue(
-            drivetrain.applyRequest(
-                () ->
-                    point.withModuleDirection(
-                        new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
+    // joystick
+    //     .b()
+    //     .whileTrue(
+    //         drivetrain.applyRequest(
+    //             () ->
+    //                 point.withModuleDirection(
+    //                     new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
 
     joystick.x().whileTrue(s_Shooter.shootL2()).onFalse(s_Shooter.stop());
     joystick.y().whileTrue(s_Shooter.intake()).onFalse(s_Shooter.stop());
     joystick.a().whileTrue(s_Shooter.shootTrough()).onFalse(s_Shooter.stop());
-
+    joystick.b().whileTrue(s_Elevator.goToL1()).onFalse(s_Elevator.stop());
 
     joystick.povUp().whileTrue(s_Elevator.moveUp()).onFalse(s_Elevator.stop());
     joystick.povDown().whileTrue(s_Elevator.moveDown()).onFalse(s_Elevator.stop());

diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -5,11 +5,13 @@
 
 public class Elevator extends SubsystemBase {
     private final ElevatorIO m_io;
-    private final double L1Setpoint = 1;
+    private final double L1Setpoint = 100;
     private final double L2Setpoint = 5;
     private final double L3Setpoint = 0;
     private final double L4Setpoint = 0;
 
+    final double m_gearRatio = 5/1 * 5/1 * 48/72;
+
     public Elevator(ElevatorIO io) {
         m_io = io;
 
@@ -28,7 +30,11 @@ public Command moveDown() {
       }
 
       public Command goToL1(){
-        return  this.run(() -> m_io.gotosetpoint(L1Setpoint));
+        // return  this.run(() -> m_io.gotosetpoint(L1Setpoint,m_gearRatio));
+        return this.run(() -> {
+           // System.out.println("L1");
+            m_io.gotosetpoint(L1Setpoint, m_gearRatio);
+        });
       }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.elevator;
 
+import edu.wpi.first.wpilibj2.command.Command;
+
 public interface ElevatorIO {
 
     public static class ElevatorIOInputs { 
@@ -10,5 +12,5 @@ public default void setVoltage(double voltage) {}
 
     public default void setBrakeMode(boolean enableBrakeMode) {}
 
-    public abstract void gotosetpoint(double setpoint);
+    public abstract Command gotosetpoint(double setpoint, double gearRatio);
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java
@@ -4,38 +4,44 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
-public class ElevatorIOTalonFX implements ElevatorIO {
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ElevatorIOTalonFX extends SubsystemBase implements ElevatorIO{
 
 
     private final TalonFX m_left;
     private final TalonFX m_right;
-
-    private final PositionVoltage m_request;
+    final PositionVoltage m_request = new PositionVoltage(0).withSlot(0);
+    //final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0);
+    //private final PositionVoltage m_request;
 
     public ElevatorIOTalonFX() {
 
         m_left = new TalonFX(21, "CANivore2");
         m_right = new TalonFX(22, "CANivore2");
-
-        final double m_gearRatio = 5/1 * 5/1 * 72/48;
+        
+
         final TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration();
         elevatorMotorConfig.CurrentLimits.SupplyCurrentLimit = 40.0;
         elevatorMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
         // in init function, set slot 0 gains
         var slot0Configs = new Slot0Configs();
         slot0Configs.kP = 2.4; // An error of 1 rotation results in 2.4 V output
         slot0Configs.kI = 0; // no output for integrated error
-        slot0Configs.kD = 0.1; // A velocity of 1 rps results in 0.1 V output
+        slot0Configs.kD = 1; // A velocity of 1 rps results in 0.1 V output
         m_left.setControl(new Follower(22, false));
 
         m_right.setPosition(0);
         // create a position closed-loop request, voltage output, slot 0 configs
-        m_request = new PositionVoltage(0).withSlot(0);
-
+        //m_request = new PositionVoltage(0).withSlot(0);
+        //
+
         // set position to 10 rotations
 
         m_left.getConfigurator().apply(slot0Configs);
@@ -50,6 +56,7 @@ public void setVoltage(double voltage) {
 
         m_left.setControl(m_request.withOutput(-voltage));
         m_right.setControl(m_request.withOutput(voltage));
+        System.out.println("volts:" + m_right.getMotorVoltage());
     }
 
     public void setBrakeMode(boolean enableBrakeMode) {
@@ -59,9 +66,21 @@ public void setBrakeMode(boolean enableBrakeMode) {
         m_right.setNeutralMode(neutralModeValue);
     }
 
-    public void gotosetpoint(double setpoint) {
-        m_left.setControl(m_request.withPosition(setpoint));
-        m_right.setControl(m_request.withPosition(setpoint));
+    public Command gotosetpoint(double setpoint, double m_gearRatio) {
+        return this.run(() -> {
+            Double rotations = setpoint + (m_gearRatio * 2 * Math.PI * 1.76/2);
+            m_left.setControl(m_request.withPosition(rotations));
+            m_right.setControl(m_request.withPosition(rotations));
+            System.out.println("POS:" + m_right.getPosition().toString());
+            System.out.println("volts:" + m_right.getMotorVoltage());
+        });
+        //
+        // System.out.println("setpoint: " + setpoint);
+        // m_left.setControl(m_request.withPosition(10));
+        // m_right.setControl(m_request.withPosition(10));
+        // System.out.println("POS:" + m_right.getPosition().toString());
+        // System.out.println("volts:" + m_right.getMotorVoltage()
+        // .toString());
     }