From 976b77ff98b54717374fdc675caea62ba7bb2af1 Mon Sep 17 00:00:00 2001 From: Timothy Bevis Date: Tue, 3 Oct 2023 11:27:05 -0400 Subject: [PATCH 1/6] Update absolute encoder and add functionality to sequences. --- src/main/java/frc/robot/ArmPresets.java | 4 ++-- src/main/java/frc/robot/Robot.java | 5 +++-- src/main/java/frc/robot/maps/FrostBiteMap.java | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/ArmPresets.java b/src/main/java/frc/robot/ArmPresets.java index b8e903c1..83cbcb6a 100644 --- 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/Robot.java b/src/main/java/frc/robot/Robot.java index 4fb6f8f0..9a2c627d 100644 --- 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"; })); diff --git a/src/main/java/frc/robot/maps/FrostBiteMap.java b/src/main/java/frc/robot/maps/FrostBiteMap.java index 22a12dc3..8f046edb 100644 --- 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(8); CSFusedEncoder fusedEncoder = new CSFusedEncoder(encoder, absEncoder); From 27fdf68e32bb8e0abbeaef36a0c8412b9cecd200 Mon Sep 17 00:00:00 2001 From: Timothy Bevis Date: Thu, 5 Oct 2023 19:38:33 -0400 Subject: [PATCH 2/6] Update Abs. Encoder Value --- src/main/java/frc/robot/maps/FrostBiteMap.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/maps/FrostBiteMap.java b/src/main/java/frc/robot/maps/FrostBiteMap.java index 8f046edb..6cca09b4 100644 --- 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(8); + absEncoder.setPositionOffset(66.8); CSFusedEncoder fusedEncoder = new CSFusedEncoder(encoder, absEncoder); From a80431e2e52761b0412c18a510f3218d826f3bfe Mon Sep 17 00:00:00 2001 From: "Liam.murray" Date: Thu, 19 Oct 2023 20:41:47 -0400 Subject: [PATCH 3/6] Updated absolute encoder value --- src/main/java/frc/robot/maps/FrostBiteMap.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/maps/FrostBiteMap.java b/src/main/java/frc/robot/maps/FrostBiteMap.java index 6cca09b4..ae6d82e9 100644 --- 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(66.8); + absEncoder.setPositionOffset(54.8); CSFusedEncoder fusedEncoder = new CSFusedEncoder(encoder, absEncoder); From aac50ddf527ab2973f5a7975629f6764b28febfc Mon Sep 17 00:00:00 2001 From: Helen Saunders Date: Tue, 24 Oct 2023 19:31:57 -0400 Subject: [PATCH 4/6] First (good) Attempt at wheel test --- src/main/java/frc/robot/Auto.java | 14 ++++++++++++++ src/main/java/frc/robot/Robot.java | 3 +++ src/main/java/frc/robot/subsystems/Drive.java | 1 - 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index 4380b033..2cabc259 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -53,6 +53,12 @@ private CommandBase moveFor(double speed, double seconds) { new FunctionalWaitCommand(() -> seconds)).andThen(drive.safeStateCmd()); } + private CommandBase moveForDirectional(double xSpeed, double ySpeed, double seconds) { + return race( + drive.driveRaw(() -> xSpeed, () -> ySpeed, () -> 0), + new FunctionalWaitCommand(() -> seconds)).andThen(drive.safeStateCmd()); + } + public CommandBase leaveCommunity() { return (scoreConeWhile( drive.driveRelative(new Translation2d(4, 0), 180, 6))); @@ -128,6 +134,14 @@ public CommandBase prepareToScoreCone() { armRotate.moveTo(ArmPresets.HIGH_SCORE)); } + public CommandBase testDriveDriver() { + return sequence( + moveForDirectional(0, 1, 5), + moveForDirectional(1, 0, 5), + moveForDirectional(0, -1, 5), + 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 index 4fb6f8f0..b615e84c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -93,6 +93,9 @@ public class Robot extends CommandRobot { // return auto.combinedAuto(conePos, cubePos, cubeScorePos); // }); + @Autonomous(name = "TEST_DriveWheelRun") + public CommandBase driveWheelTest = auto.testDriveDriver(); + public CommandBase driveScoreMidNode = sequence( armRotate.moveTo(ArmPresets.MEDIUM_SCORE), drive.driveToNearest(), diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index dd53dd5a..fe1102e8 100644 --- 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; From f5a925eb5799e335b138ca4a32f09132f67c3462 Mon Sep 17 00:00:00 2001 From: Helen Saunders Date: Thu, 26 Oct 2023 18:30:56 -0400 Subject: [PATCH 5/6] Map to button, move commmands to right spot --- src/main/java/frc/robot/Auto.java | 14 ++++---------- src/main/java/frc/robot/Robot.java | 18 +++++++++++++----- src/main/java/frc/robot/subsystems/Drive.java | 6 ++++++ 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index 2cabc259..8717f7db 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -53,12 +53,6 @@ private CommandBase moveFor(double speed, double seconds) { new FunctionalWaitCommand(() -> seconds)).andThen(drive.safeStateCmd()); } - private CommandBase moveForDirectional(double xSpeed, double ySpeed, double seconds) { - return race( - drive.driveRaw(() -> xSpeed, () -> ySpeed, () -> 0), - new FunctionalWaitCommand(() -> seconds)).andThen(drive.safeStateCmd()); - } - public CommandBase leaveCommunity() { return (scoreConeWhile( drive.driveRelative(new Translation2d(4, 0), 180, 6))); @@ -136,10 +130,10 @@ public CommandBase prepareToScoreCone() { public CommandBase testDriveDriver() { return sequence( - moveForDirectional(0, 1, 5), - moveForDirectional(1, 0, 5), - moveForDirectional(0, -1, 5), - moveForDirectional(-1, 0, 5)); + drive.moveForDirectional(0, 1, 5), + drive.moveForDirectional(1, 0, 5), + drive.moveForDirectional(0, -1, 5), + drive.moveForDirectional(-1, 0, 5)); } public CommandBase stowArmCloseIntake() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b615e84c..018ea8bc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -93,9 +93,6 @@ public class Robot extends CommandRobot { // return auto.combinedAuto(conePos, cubePos, cubeScorePos); // }); - @Autonomous(name = "TEST_DriveWheelRun") - public CommandBase driveWheelTest = auto.testDriveDriver(); - public CommandBase driveScoreMidNode = sequence( armRotate.moveTo(ArmPresets.MEDIUM_SCORE), drive.driveToNearest(), @@ -228,8 +225,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.povUp().onTrue(drive.moveForDirectional(0, 1, 5)); + driveController.povLeft().onTrue(drive.moveForDirectional(1, 0, 5)); + driveController.povRight().onTrue(drive.moveForDirectional(0, -1, 5)); + driveController.povDown().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/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index fe1102e8..1e944183 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -394,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(); From 67946cac320ea89b2e1f009a0f198e0bd49b4891 Mon Sep 17 00:00:00 2001 From: Helen Saunders Date: Thu, 26 Oct 2023 18:53:39 -0400 Subject: [PATCH 6/6] Now the buttons are accurate: up = forward, down = backward, left = left, right = right --- src/main/java/frc/robot/Robot.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 018ea8bc..22fe2eed 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -225,10 +225,10 @@ public void configureButtonBindings() { driveController.x().whileTrue(drive.rotateToAngle(Rotation2d.fromDegrees(180), () -> 0, () -> 0)); driveController.a().whileTrue(drive.rotateToAngle(Rotation2d.fromDegrees(0), () -> 0, () -> 0)); - driveController.povUp().onTrue(drive.moveForDirectional(0, 1, 5)); - driveController.povLeft().onTrue(drive.moveForDirectional(1, 0, 5)); - driveController.povRight().onTrue(drive.moveForDirectional(0, -1, 5)); - driveController.povDown().onTrue(drive.moveForDirectional(-1, 0, 5)); + 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(