From ac4fa0d8b4eedc3de43f94b51f61a2a8e0060807 Mon Sep 17 00:00:00 2001 From: Kavin Muralikrishnan Date: Sat, 26 Oct 2024 23:43:37 -0400 Subject: [PATCH] started work on system to test tasks easier --- simgui.json | 25 +++- src/main/java/frc/robot/Robot.java | 135 ++++++++++++++++++ .../frc/robot/commands/DriveCommands.java | 19 ++- .../frc/robot/commands/ServoCommands.java | 2 +- src/main/java/frc/robot/subsystems/Servo.java | 4 +- .../frc/robot/subsystems/XRPDrivetrain.java | 5 +- 6 files changed, 179 insertions(+), 11 deletions(-) diff --git a/simgui.json b/simgui.json index c12759c..ddce0cd 100644 --- a/simgui.json +++ b/simgui.json @@ -20,10 +20,33 @@ }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Task Selector": "String Chooser" + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Task Selector": { + "open": true, + "string[]##v_/SmartDashboard/Task Selector/options": { + "open": true + } + }, + "open": true + } } }, "NetworkTables Info": { + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, + "Server": { + "open": true + }, "visible": true } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 641a780..7044e50 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,21 @@ // Credit to Team 5338 for any changes to the base command XRP template. package frc.robot; +import java.lang.reflect.InvocationTargetException; +import java.lang.reflect.Method; + import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.Servo; +import frc.robot.subsystems.XRPDrivetrain; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -18,6 +30,7 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private SendableChooser m_chooser = new SendableChooser<>(); /** * This function is run when the robot is first started up and should be used for any @@ -28,6 +41,13 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + for(int i=1; i<=10; i++){ + m_chooser.addOption("Task " + i, i); + } + m_chooser.addOption("Task 3 Bonus", 11); + m_chooser.addOption("Task 7 Bonus", 12); + SmartDashboard.putData("Task Selector", m_chooser); + SmartDashboard.putString("What should happen: ", ""); } /** @@ -61,7 +81,122 @@ public void autonomousInit() { if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + switch (m_chooser.getSelected()) { + case 1: + SmartDashboard.putString("What should happen: ", "The XRP should turn about 90 degrees(don't worry if it's not accurate!)"); + DriveCommands.turnDegreesCommand(1, 90).schedule(); + break; + case 2: + //TODO for Kavin: Replace the instant commands with runOnce + SmartDashboard.putString("What should happen: ", "The XRP should drive straight for 3 seconds, turn right for 2 seconds, turn left for 2 seconds, and then stop."); + new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3), + new InstantCommand(() -> { + try { + XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, 1, 1); + System.out.println("runs"); + } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + }, + RobotContainer.m_xrpDrivetrain + ) + ), + new ParallelDeadlineGroup( + new WaitCommand(2), + new InstantCommand(() -> { + try { + XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, 1, -1); + System.out.println("runs"); + } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + }, + RobotContainer.m_xrpDrivetrain + ) + ), + new ParallelDeadlineGroup( + new WaitCommand(2), + new InstantCommand(() -> { + try { + XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, -1, 1); + System.out.println("runs"); + } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + }, + RobotContainer.m_xrpDrivetrain + ) + ), + new InstantCommand(() -> { + try { + XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, 0, 0); + System.out.println("runs"); + } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + }, + RobotContainer.m_xrpDrivetrain + )).schedule(); + break; + case 3: + SmartDashboard.putString("What should happen: ", "The XRP should drive 5 inches(don't worry if it's not accurate!). tankDriveCommand will be tested in Task 4"); + try { + DriveCommands.class.getMethod("tankDriveDistance", double.class).invoke(null, 5); + } catch (NoSuchMethodException e) { + // TODO Auto-generated catch block + try { + ((Command) Class.forName("DriveCommands$TankDriveDistance").getDeclaredConstructor(double.class).newInstance()).schedule(); + } catch (ClassNotFoundException | InstantiationException | IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException | SecurityException e1) { + // TODO Auto-generated catch block + e1.printStackTrace(); + } + } catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + break; + case 4: + SmartDashboard.putString("What should happen: ", "Change the mode from Auto to Teleop. You should now be able to control the XRP using a tank drive scheme!"); + break; + case 5: + SmartDashboard.putString("What should happen: ", "The arm should move to 30 degrees, move to 142.5 degrees, and put the number of degrees on SmartDashboard(should be 142.5)"); + try { + Servo.class.getMethod("setServoAngle", double.class).invoke(RobotContainer.m_servo, 30); + Servo.class.getMethod("setServoAngle", double.class).invoke(RobotContainer.m_servo, 142.5); + SmartDashboard.putNumber("Servo Angle", (Double) Servo.class.getMethod("getServoAngle").invoke(RobotContainer.m_servo)); + } catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException + | SecurityException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + break; + case 6: + SmartDashboard.putString("What should happen: ", "The XRP should move its servo to each of the presets."); + try{ + + } + break; + case 7: + break; + case 8: + break; + case 9: + break; + case 10: + break; + case 11: + break; + case 12: + break; + } } + /** This function is called periodically during autonomous. */ @Override diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index daed6c1..39ba298 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -197,13 +197,22 @@ has a wheel placement diameter (163 mm) - width of the wheel (8 mm) = 155 mm } - //TODO: Task 1-Rewrite TurnDegrees as a function that returns a functional command + //TODO: Task 1-Rewrite TurnDegrees as a function that returns a functional command by replacing each null value with the correct code //Code here: - + public static Command turnDegreesCommand(double speed, double degrees){ + return new FunctionalCommand( + null, /* This is what runs when the command initializes. */ + null, /* This happens periodically when the command is executed*/ + null, /* This is what runs when the command is complete */ + null, /* This is what determines if the command is complete */ + null /* This is where you write any and all subsystems used in the command. The drivetrain is RobotContainer.m_xrpDrivetrain. Perhaps you can find + a way to avoid having to type out the entire name of the drivetrain every time you reference it?*/ + ); + } //TODO: Task 3-Rewrite the following functions to use tank drive - //driveDistance or AltDriveDistance(choose 1) - //arcadeDriveCommand(rename as tankDriveCommand) - //Bonus(optional):TurnDegrees(either class or function, choose 1) + //driveDistance or AltDriveDistance(choose 1) (name as tankDriveDistance or TankDriveDistance based on if you chose to rewrite the former or the latter) + //arcadeDriveCommand(name as tankDriveCommand) + //Bonus(optional):TurnDegrees(either class or function, choose 1) (name as tankTurnDegrees or TankTurnDegrees based on if you chose to rewrite the former or the latter) /** * Commands can also a part of command groups, which are sets of commands that run in certain ways. Sequential Command Groups diff --git a/src/main/java/frc/robot/commands/ServoCommands.java b/src/main/java/frc/robot/commands/ServoCommands.java index a924e0d..e7bfb5c 100644 --- a/src/main/java/frc/robot/commands/ServoCommands.java +++ b/src/main/java/frc/robot/commands/ServoCommands.java @@ -3,6 +3,6 @@ public class ServoCommands { //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. + //Name them servoUpCommand, servoDownCommand, and servoPresetCommand. The presets are in the constants file. } diff --git a/src/main/java/frc/robot/subsystems/Servo.java b/src/main/java/frc/robot/subsystems/Servo.java index 7c7e35e..599313a 100644 --- a/src/main/java/frc/robot/subsystems/Servo.java +++ b/src/main/java/frc/robot/subsystems/Servo.java @@ -11,7 +11,7 @@ public Servo(){ m_armServo = new XRPServo(4); } - //TODO: Task 5-Write functions to control the arm. You should be able to set the arm's angle and get the angle - //it is at. + //TODO: Task 5-Write functions to control the arm. You should be able to set the arm's angle and get the angle(double) + //it is at. The functions should be called setServoAngle and getServoAngle //HINT: Type 'm_armServo.' (without quotes) in a method body to see all of the different methods the servo has. } diff --git a/src/main/java/frc/robot/subsystems/XRPDrivetrain.java b/src/main/java/frc/robot/subsystems/XRPDrivetrain.java index 1f7f69c..8b92e47 100644 --- a/src/main/java/frc/robot/subsystems/XRPDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/XRPDrivetrain.java @@ -45,8 +45,9 @@ public void arcadeDrive(double xaxisSpeed, double zaxisRotate) { m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate); } - //TODO:Task 2-Write a function that makes the robot take in a left wheel speed and a right wheel speed, and move at those speeds. - //HINT:This is called a tank drive, maybe there is a function that can easily allow for it? + //TODO:Task 2-Write a method that makes the robot take in a left wheel speed and a right wheel speed, and move at those speeds. + //This type of robot control is called tank drive, so name the function tankDrive (exactly as written) + //This should be a method that does not return anything, not a command! //HINT:To see all the functions that the drivetrain has, type "m_diffDrive." without the quotes and look at the options generated by auto complete. public void resetEncoders() {