update conversion factor, works intakeless

grt192 · Jan 20, 2025 · 204b0a7 · 204b0a7
1 parent d88bdfe
commit 204b0a7
Show file tree

Hide file tree

Showing 6 changed files with 55 additions and 32 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -58,5 +58,5 @@
     "edu.wpi.first.math.**.struct.*",
   ],
   "wpilib.autoStartRioLog": true,
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable"
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -67,15 +67,15 @@ public static class LoggingConstants{
   }
 
   public static class IntakeConstants {
-    public static final int PIVOT_ID = 0;
+    public static final int PIVOT_ID = 1;
     public static final int ROLLER_ID = 0;
     public static final int INTAKE_SENSOR_ID = 0;
     public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI);
     public static final double ZERO_POSITION = 0;
     public static final double SOURCE_POSITION = 0; //TODO: change
     public static final double OUTTAKE_POSITION = 0; //TODO: change
-    public static final double VERTICAL_POSITION = 0; //TODO: change
-    public static final double PIVOT_TOLERANCE = 0; //TODO: change
+    public static final double VERTICAL_POSITION = Units.degreesToRadians(90); //TODO: change
+    public static final double PIVOT_TOLERANCE = .05; //TODO: change
 
   }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -40,19 +40,20 @@ public class RobotContainer {
 
   private BaseDriveController driveController;
 
-  private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
+  // private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
   private final PivotSubsystem pivotSubsystem = new PivotSubsystem();
-  private final RollerSubsystem rollerSubsystem = new RollerSubsystem();
+  // private final RollerSubsystem rollerSubsystem = new RollerSubsystem();
 
   private final FieldManagementSubsystem fieldManagementSubsystem = new FieldManagementSubsystem();
   private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = new PhoenixLoggingSubsystem(fieldManagementSubsystem);
 
   private CommandPS5Controller mechController;
-  private Trigger aButton = new Trigger(mechController.button(0));
+  private Trigger aButton;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     mechController = new CommandPS5Controller(1);
+    aButton = new Trigger(mechController.cross());
 
     constructDriveController(); 
     startLog();
@@ -74,17 +75,17 @@ private void configureBindings() {
       * the robot is controlled along its own axes, otherwise controls apply to the field axes by default. If the
       * swerve aim button is held down, the robot will rotate automatically to always face a target, and only
       * translation will be manually controllable. */
-    swerveSubsystem.setDefaultCommand(
-      new RunCommand(() -> {
-        swerveSubsystem.setDrivePowers(
-          driveController.getForwardPower(),
-          driveController.getLeftPower(),
-          driveController.getRotatePower()
-        );
-        }, 
-        swerveSubsystem
-      )
-    );
+    // swerveSubsystem.setDefaultCommand(
+    //   new RunCommand(() -> {
+    //     swerveSubsystem.setDrivePowers(
+    //       driveController.getForwardPower(),
+    //       driveController.getLeftPower(),
+    //       driveController.getRotatePower()
+    //     );
+    //     }, 
+    //     swerveSubsystem
+    //   )
+    // );
 
     aButton.onTrue(
       new ConditionalCommand(
@@ -94,17 +95,21 @@ private void configureBindings() {
         )
     );
 
-    rollerSubsystem.setDefaultCommand(new InstantCommand( () -> {
-      rollerSubsystem.setRollerPower(mechController.getL2Axis() - mechController.getR2Axis());
-    }, rollerSubsystem));
-
-    /* Pressing the button resets the field axes to the current robot axes. */
-    driveController.bindDriverHeadingReset(
-      () ->{
-        swerveSubsystem.resetDriverHeading();
-      },
-      swerveSubsystem
-    );
+    // aButton.onTrue(
+    //     new SetPivotVerticalCommand(pivotSubsystem)
+    // );
+
+    // rollerSubsystem.setDefaultCommand(new InstantCommand( () -> {
+    //   rollerSubsystem.setRollerPower(mechController.getL2Axis() - mechController.getR2Axis());
+    // }, rollerSubsystem));
+
+    // /* Pressing the button resets the field axes to the current robot axes. */
+    // driveController.bindDriverHeadingReset(
+    //   () ->{
+    //     swerveSubsystem.resetDriverHeading();
+    //   },
+    //   swerveSubsystem
+    // );
   }
 
   /**

diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java
@@ -15,10 +15,16 @@ public SetPivotVerticalCommand(PivotSubsystem pivotSubsystem) {
     @Override
     public void execute() {
         this.pivotSubsystem.setState(PivotState.VERTICAL);
+        System.out.println("setting vertical");
     }
 
     @Override
     public boolean isFinished() {
         return pivotSubsystem.atState(PivotState.VERTICAL);
     }
+
+    @Override
+    public void end(boolean interrupted) {
+        pivotSubsystem.setSpeed(0);
+    }
 }
diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java
@@ -15,10 +15,16 @@ public SetPivotZeroCommand(PivotSubsystem pivotSubsystem) {
     @Override
     public void execute() {
         this.pivotSubsystem.setState(PivotState.ZERO);
+        System.out.println("setting horizaontal");
     }
 
     @Override
     public boolean isFinished() {
         return pivotSubsystem.atState(PivotState.ZERO);
     }
+
+    @Override
+    public void end(boolean interrupted) {
+        pivotSubsystem.setSpeed(0);
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java
@@ -41,9 +41,10 @@ public PivotSubsystem(){
 
         pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless);
         pivotEncoder = pivotMotor.getEncoder();
+        pivotEncoder.setPosition(0);
 
         encoderConfig = new EncoderConfig();
-        encoderConfig.positionConversionFactor(PIVOT_CONVERSION_FACTOR);
+        encoderConfig.positionConversionFactor(1./ PIVOT_CONVERSION_FACTOR);
 
         softLimitConfig = new SoftLimitConfig();
         softLimitConfig.forwardSoftLimitEnabled(true)
@@ -56,8 +57,8 @@ public PivotSubsystem(){
 
         sparkMaxConfig = new SparkMaxConfig();
         sparkMaxConfig.apply(closedLoopConfig)
-                      .apply(encoderConfig)
-                      .apply(softLimitConfig);
+                      .apply(encoderConfig);
+                    //   .apply(softLimitConfig);
 
         pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 
@@ -77,9 +78,14 @@ public PivotState getTargetState() {
 
     public void setState(PivotState targetState) {
         pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition);
+        // System.out.println("state set to : " + targetState.getTargetAngle());
         this.targetState = targetState;
     }
 
+    public void setSpeed(double speed) {
+        pivotMotor.set(speed);
+    }
+
     public boolean atState(PivotState state) {
         return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE;
     }