Merge pull request #73 from chopshop-166/wheel-testing-h

Drive wheel testing function
chopshop-166 · Oct 27, 2023 · 3066c6d · 3066c6d
2 parents 3fb64db + 1fc1dd2
commit 3066c6d
Show file tree

Hide file tree

Showing 5 changed files with 33 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/ArmPresets.java b/src/main/java/frc/robot/ArmPresets.java
@@ -21,8 +21,8 @@ public enum ArmPresets {
             76.3),
 
     // Human Player Station = HPS
-    HPS_PICKUP(0, 77,
-            72),
+    HPS_PICKUP(0, 76.3,
+            71.3),
 
     ARM_STOWED(1, 2, 1),
 

diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java
@@ -128,6 +128,14 @@ public CommandBase prepareToScoreCone() {
                 armRotate.moveTo(ArmPresets.HIGH_SCORE));
     }
 
+    public CommandBase testDriveDriver() {
+        return sequence(
+                drive.moveForDirectional(0, 1, 5),
+                drive.moveForDirectional(1, 0, 5),
+                drive.moveForDirectional(0, -1, 5),
+                drive.moveForDirectional(-1, 0, 5));
+    }
+
     public CommandBase stowArmCloseIntake() {
         return sequence(
                 armExtend.moveTo(ArmPresets.ARM_STOWED),

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -154,11 +154,12 @@ public CommandBase rumbleAndIntakeSpinningOff() {
     public CommandBase pickUpGamePiece = sequence(
             new ConditionalCommand(
                     sequence(
-                            armRotate.moveTo(ArmPresets.CONE_PICKUP), armExtend.moveTo(ArmPresets.CONE_PICKUP)),
+                            armRotate.moveTo(ArmPresets.CONE_PICKUP), armExtend.moveTo(ArmPresets.CONE_PICKUP),
+                            intake.toggle()),
                     sequence(
                             armRotate.moveTo(ArmPresets.CUBE_PICKUP), armExtend.moveTo(
                                     ArmPresets.CUBE_PICKUP),
-                            intake.coneRelease()),
+                            intake.coneRelease(), intake.toggle()),
                     () -> {
                         return gamePieceSub.get() == "Cone";
                     }));
@@ -225,8 +226,19 @@ public void configureButtonBindings() {
         driveController.x().whileTrue(drive.rotateToAngle(Rotation2d.fromDegrees(180), () -> 0, () -> 0));
         driveController.a().whileTrue(drive.rotateToAngle(Rotation2d.fromDegrees(0), () -> 0, () -> 0));
 
-        (new Trigger(() -> driveController.getRightTriggerAxis() > 0.5))
-                .onTrue(balanceArm.pushDown()).onFalse(balanceArm.pushUp());
+        driveController.povDown().onTrue(drive.moveForDirectional(0, 1, 5));
+        driveController.povRight().onTrue(drive.moveForDirectional(1, 0, 5));
+        driveController.povUp().onTrue(drive.moveForDirectional(0, -1, 5));
+        driveController.povLeft().onTrue(drive.moveForDirectional(-1, 0, 5));
+        /*
+         * public CommandBase testDriveDriver() {
+         * return sequence(
+         * moveForDirectional(0, 1, 5),
+         * moveForDirectional(1, 0, 5),
+         * moveForDirectional(0, -1, 5),
+         * moveForDirectional(-1, 0, 5));
+         * }
+         */
         // Arm
 
         // COPILOT CONTROLLER

diff --git a/src/main/java/frc/robot/maps/FrostBiteMap.java b/src/main/java/frc/robot/maps/FrostBiteMap.java
@@ -177,7 +177,7 @@ public ArmRotateMap getArmRotateMap() {
         absEncoder.setDutyCycleRange(1.0 / 1025.0, 1024.0 / 1025.0);
         absEncoder.setDistancePerRotation(-360);
         // Adjust this to move the encoder zero point to the retracted position
-        absEncoder.setPositionOffset(91.89780758483522 - 7);
+        absEncoder.setPositionOffset(54.8);
 
         CSFusedEncoder fusedEncoder = new CSFusedEncoder(encoder, absEncoder);
 

diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -13,7 +13,6 @@
 import com.chopshop166.chopshoplib.commands.FunctionalWaitCommand;
 import com.chopshop166.chopshoplib.commands.SmartSubsystemBase;
 
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -395,6 +394,12 @@ public CommandBase balance() {
 
     }
 
+    public CommandBase moveForDirectional(double xSpeed, double ySpeed, double seconds) {
+        return race(
+                driveRaw(() -> xSpeed, () -> ySpeed, () -> 0),
+                new FunctionalWaitCommand(() -> seconds)).andThen(safeStateCmd());
+    }
+
     public CommandBase resetGyroCommand() {
         return cmd().onInitialize(() -> {
             resetGyro();