From 83d2c6c6b4e02f0587b246bf98b37bd7aee2e687 Mon Sep 17 00:00:00 2001 From: RyanW Date: Sat, 12 Oct 2024 00:08:04 -0400 Subject: [PATCH] Task 9 Completion, also changed some stuff in ServoCommands --- src/main/java/frc/robot/RobotContainer.java | 13 +++++++++++-- .../java/frc/robot/commands/DriveCommands.java | 17 ++++++++++++++++- .../java/frc/robot/commands/ServoCommands.java | 10 +++++----- 3 files changed, 32 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8bcd5c2..e02fbb6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import frc.robot.subsystems.Servo; import frc.robot.subsystems.XRPDrivetrain; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -67,7 +68,15 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // The following command will run during autonomous - return null; + return new FunctionalCommand( + () -> { + RobotContainer.m_xrpDrivetrain.tankDrive(0, 0); + RobotContainer.m_xrpDrivetrain.resetEncoders(); + }, + () -> RobotContainer.m_xrpDrivetrain.tankDrive(1, 1), + interrupted -> RobotContainer.m_xrpDrivetrain.tankDrive(0, 0), + () -> (RobotContainer.m_rangefinder.findRange()) <= 2.0, + RobotContainer.m_xrpDrivetrain, RobotContainer.m_rangefinder + ); } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1ca8ae0..e45846e 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -5,11 +5,14 @@ package frc.robot.commands; +import frc.robot.Robot; import frc.robot.RobotContainer; +import frc.robot.subsystems.Rangefinder; import frc.robot.subsystems.XRPDrivetrain; import java.util.function.Supplier; +import edu.wpi.first.wpilibj.xrp.XRPRangefinder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -291,7 +294,19 @@ public static Command sequentialExampleCommand(){ //TODO: Task 9-Write a Command(function or class) that causes the XRP to drive until the distance returned by the rangefinder //is less than 2 inches. Afterwards, test it by replacing the return value of getAutonomousCommand() in RobotContainer and setting it to instead return the command you wrote. - + public static Command driveUntil(){ + //Rangefinder abc = new Rangefinder(); + return new FunctionalCommand( + () -> { + RobotContainer.m_xrpDrivetrain.tankDrive(0, 0); + RobotContainer.m_xrpDrivetrain.resetEncoders(); + }, + () -> RobotContainer.m_xrpDrivetrain.tankDrive(1, 1), + interrupted -> RobotContainer.m_xrpDrivetrain.tankDrive(0, 0), + () -> (RobotContainer.m_rangefinder.findRange()) <= 2.0, + RobotContainer.m_xrpDrivetrain, RobotContainer.m_rangefinder + ); + } //TODO: Task 10-Write a Sequential Command Group to move the XRP backwards 2 inches(you may need to make new Commands or alter existing Commands for this), set the arm preset to index 1, spin the XRP 360 degrees //and move the XRP forward 3 inches while moving the arm to 0 degrees using a by Parallel Command group. Afterwards, test it by replacing //the return value of getAutonomousCommand() in RobotContainer and setting it to instead return the command you wrote. diff --git a/src/main/java/frc/robot/commands/ServoCommands.java b/src/main/java/frc/robot/commands/ServoCommands.java index ccfbc27..8c0946d 100644 --- a/src/main/java/frc/robot/commands/ServoCommands.java +++ b/src/main/java/frc/robot/commands/ServoCommands.java @@ -18,28 +18,28 @@ public class ServoCommands { //private XRPServo test; - private static Servo hiIhopeThisWorks = new Servo(); + //private static Servo hiIhopeThisWorks = new Servo(); private static Constants constants = new Constants(); //TODO: Task 6-Write commands to move the arm up, down, and to a specific preset. The latter should be done by taking in an integer input. //The presets are in the constants file. public static Command servoAdjustAngleCommand(double adjustAmount) { return new RunCommand( - () -> hiIhopeThisWorks.adjustAngleBy(adjustAmount) - //() -> frc.robot.subsystems.Servo.m_armServo.adjustAngleBy(5) + () -> RobotContainer.m_servo.adjustAngleBy(adjustAmount) + //() -> frc.robot.subsystems.Servo.m_armServo.adjustAngleBy() or maybe RobotContainer.m_servo //note to self: if hiIhopeThisWorks doesn't work, then do ^^^ and change m_armServo to public ); } public static Command servoSetAngleCommand(double angle) { return new InstantCommand( - () -> hiIhopeThisWorks.setAngle(angle) + () -> RobotContainer.m_servo.setAngle(angle) ); } public static Command servoPresetCommand(int index) { return new InstantCommand( - () -> hiIhopeThisWorks.setAngle(constants.servoPresets[index]) + () -> RobotContainer.m_servo.setAngle(constants.servoPresets[index]) ); } }