From 9e4ca452110fa6b9162b768433bee3b468a52f3a Mon Sep 17 00:00:00 2001 From: Robot Date: Tue, 1 Apr 2025 16:36:11 -0700 Subject: [PATCH 1/5] Changes to be committed: --- src/main/java/Team4450/Robot25/Constants.java | 4 ++-- src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java | 2 +- src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/Team4450/Robot25/Constants.java b/src/main/java/Team4450/Robot25/Constants.java index 3f0eecb..5a892dc 100644 --- a/src/main/java/Team4450/Robot25/Constants.java +++ b/src/main/java/Team4450/Robot25/Constants.java @@ -62,7 +62,7 @@ public final class Constants // Multiply by 2π for radians traveled/angular displacement and by the spool radius in meters to get linear displacement. // The spool radius is 0.875 inches, which is 0.022225 meters (source: looked it up). // The factor is negative, likely because the gears swap rotation direction, but this is not a significant issue. - public static final double ELEVATOR_WINCH_FACTOR = (-1.0 / (38.0 / 8.0)) * (2 * Math.PI) * 0.022225; //Changed to 2025 Value! + public static final double ELEVATOR_WINCH_FACTOR = (-1.0 / (38.0 / 9.0)) * (2 * Math.PI) * 0.022225; //Changed to 2025 Value! // Pneumatic valve controller port assignments. public static final int COMPRESSOR = 1; @@ -285,4 +285,4 @@ public static final class NeoMotorConstants { //-------------------- No student code above this line ------------------------------------------------------ } -; \ No newline at end of file +; diff --git a/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java b/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java index d89dfdd..b781480 100644 --- a/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java +++ b/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java @@ -63,7 +63,7 @@ public void initialize (){ @Override public void execute() { - if (nullTagCounter > 5) { + if (nullTagCounter > 20) { robotDrive.drive(0, 0, 0, false); } // logic for chosing "closest" target in PV subsystem diff --git a/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java b/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java index 53ebf44..8982268 100644 --- a/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java +++ b/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java @@ -63,7 +63,7 @@ public void initialize (){ @Override public void execute() { - if (nullTagCounter > 5) { + if (nullTagCounter > 20) { robotDrive.drive(0, 0, 0, false); } // logic for chosing "closest" target in PV subsystem From 416235d03202457d230960d5db7e6c90e873b1bb Mon Sep 17 00:00:00 2001 From: Robot Date: Tue, 1 Apr 2025 18:04:52 -0700 Subject: [PATCH 2/5] Questions 1 Changes to be committed: --- .../java/Team4450/Robot25/RobotContainer.java | 49 +++++++++++++------ 1 file changed, 34 insertions(+), 15 deletions(-) diff --git a/src/main/java/Team4450/Robot25/RobotContainer.java b/src/main/java/Team4450/Robot25/RobotContainer.java index 3bd0350..29764c8 100644 --- a/src/main/java/Team4450/Robot25/RobotContainer.java +++ b/src/main/java/Team4450/Robot25/RobotContainer.java @@ -21,7 +21,6 @@ import Team4450.Robot25.commands.RetractClimber; import Team4450.Robot25.commands.OuttakeAlgae; - import Team4450.Robot25.subsystems.AlgaeManipulator; import Team4450.Robot25.subsystems.AlgaeGroundIntake; import Team4450.Robot25.subsystems.Candle; @@ -81,6 +80,7 @@ public class RobotContainer public static PhotonVision pvCoralTagCameraLeft; public static PhotonVision pvCoralTagCameraRight; public static PhotonVision pvAlgaeTagCamera; + // ! What is this? private Candle candle = null; public static Elevator elevator; public static ElevatedManipulator elevatedManipulator; @@ -106,18 +106,23 @@ public class RobotContainer // constructor called, but you do get initialize called again and then on to execute & etc. // So this means you have to be careful about command initialization activities as a persistent // command in effect has two lifetimes (or scopes): Class global and each new time the command - // is scheduled. Note the FIRST doc on the scheduler process is not accurate as of 2020. + // is scheduled. + + // Note the FIRST doc on the scheduler process is not accurate as of 2020. // GamePads. 2 Game Pads use RobotLib XboxController wrapper class for some extra features. // Note that button responsiveness may be slowed as the schedulers command list gets longer // or commands get longer as buttons are processed once per scheduler run. private XboxController driverController = new XboxController(DRIVER_PAD); + // ! Why the difference? public static XboxController utilityController = new XboxController(UTILITY_PAD); + // ! What does this mean? // private PowerDistribution pdp = new PowerDistribution(REV_PDB, PowerDistribution.ModuleType.kCTRE); private PowerDistribution pdp = new PowerDistribution(REV_PDB, PowerDistribution.ModuleType.kRev); + // ! What does this mean? // Compressor class controls the CTRE/REV Pneumatics control Module. private Compressor pcm = new Compressor(PneumaticsModuleType.REVPH); @@ -140,17 +145,21 @@ public class RobotContainer */ public RobotContainer() throws Exception { + // ! Why so many empty consoleLog functions? Util.consoleLog(); + // ! What does this mean? SendableRegistry.addLW(pdp, "PDH"); // Only sent to NT in Test mode. // Get information about the match environment from the Field Control System. + // ! Where does this go? getMatchInformation(); // Read properties file from RoboRio "disk". If we fail to open the file, // log the exception but continue and default to competition robot. + // ! What is in this? try { robotProperties = Util.readProperties(); } catch (Exception e) { Util.logException(e);} @@ -168,15 +177,20 @@ public RobotContainer() throws Exception boolean compressorEnabled = true; // Default if no property. + // ! Checking robot properties but not checking the specific property? if (robotProperties != null) compressorEnabled = Boolean.parseBoolean(robotProperties.getProperty("CompressorEnabledByDefault")); SmartDashboard.putBoolean("CompressorEnabled", compressorEnabled); + // ! What is this? // Reset PDB & PCM sticky faults. + // ! What is this? resetFaults(); + // ! Why also make them wait until it is ready? + // Create NavX object here since must done before CameraFeed is created (don't remember why). // Navx calibrates at power on and must complete before robot moves. Takes ~1 second for 2nd // generation Navx ~15 seconds for classic Navx. We assume there will be enough time between @@ -185,20 +199,20 @@ public RobotContainer() throws Exception // Warning: The navx instance is shared with the swerve drive code. Resetting or otherwise // manipulating the navx (as opposed to just reading data) may crash the swerve drive code. + // ! Can we seperate the instances? navx = NavX.getInstance(); // Add navx as a Sendable. Updates the dashboard heading indicator automatically. + // ! Why named Gyro2? SmartDashboard.putData("Gyro2", navx); // Invert driving joy sticks Y axis so + values mean forward. // Invert driving joy sticks X axis so + values mean right. - driverController.invertY(true); driverController.invertX(true); // Create subsystems prior to button mapping. - shuffleBoard = new ShuffleBoard(); driveBase = new DriveBase(); pvCoralTagCameraLeft = new PhotonVision(CORAL_CAMERA_TAG_LEFT, PipelineType.POSE_ESTIMATION, CORAL_CAMERA_TAG_LEFT_TRANSFORM); @@ -209,43 +223,49 @@ public RobotContainer() throws Exception elevator = new Elevator(driveBase); climber = new Climber(); algaeGroundIntake = new AlgaeGroundIntake(); - // coralGroundIntake = new CoralGroundIntake(); elevatedManipulator = new ElevatedManipulator(coralManipulator, - // coralGroundIntake, algaeManipulator, algaeGroundIntake, elevator); + // ! What does this mean? // if (RobotBase.isReal()) // { // candle = new Candle(CTRE_CANDLE, 8+26); // candle.setDefaultCommand(new UpdateCandle(candle)); // } + // ! What does this mean? // Create any persistent commands. + // ! What does this mean? // Set any subsystem Default commands. - // This sets up the photonVision subsystem to constantly update the robotDrive odometry + // This sets up the photonVision subsystem to constantly update the robotDrive odometry // ! At periodic? // with AprilTags (if it sees them). (As well as vision simulator) // pvAlgaeTagCamera.setDefaultCommand(new UpdateVisionPose(driveBase, pvAlgaeTagCamera)); // pvCoralTagCameraLeft.setDefaultCommand(new UpdateVisionPose(driveBase, pvCoralTagCameraLeft)); // pvCoralTagCameraRight.setDefaultCommand(new UpdateVisionPose(driveBase, pvCoralTagCameraRight)); + // ! Why does this need to be ran on the periodic? + // ! Why is this not next to the actual code that it is commenting? // Set the default drive command. This command will be scheduled automatically to run // every teleop period and so use the gamepad joy sticks to drive the robot. + // ! Why do this, this seems to only be an issue because drive is being ran every periodic which it does not? + // ! Why is this not next to the actual code that it is commenting? // We pass the GetY() functions on the Joysticks as a DoubleSuppier. The point of this // is removing the direct connection between the Drive and XboxController classes. We // are in effect passing functions into the Drive command so it can read the values // later when the Drive command is executing under the Scheduler. Drive command code does // not have to know anything about the JoySticks (or any other source) but can still read - // them. We can pass the DoubleSupplier two ways. First is with () -> lambda expression + // them. We can pass the DoubleSupplier two ways. First is with () -> lambda expression // ! I hate Lamba expressions? // which wraps the getLeftY() function in a DoubleSupplier instance. Second is using the // controller class convenience method getRightYDS() which returns getRightY() as a // DoubleSupplier. We show both ways here as an example. + // ! Define this next to running the drive command not here? // The joystick controls for driving: // Left stick Y axis -> forward and backwards movement (throttle) // Left stick X axis -> left and right movement (strafe) @@ -271,10 +291,6 @@ public RobotContainer() throws Exception driverController.getRightXDS(), driverController)); - // elevatedManipulator.setDefaultCommand(new RunCommand( - // ()->{elevatedManipulator.moveRelative(-MathUtil.applyDeadband(utilityController.getLeftY() * 0.1, DRIVE_DEADBAND)); - // }, elevatedManipulator)); - elevator.setDefaultCommand(new RunCommand( ()->{elevator.move(-MathUtil.applyDeadband(utilityController.getLeftY() * 0.5, DRIVE_DEADBAND)); }, elevator)); @@ -298,6 +314,8 @@ public RobotContainer() throws Exception cameraFeed.start(); } + // ! Why is this wrong, does it not update when a joystick is plugged in? + // ! Also why is this here and not like in the Drivestation code on the laptop instead of on the robot? // Start a thread that will wait 30 seconds then disable the missing // joystick warning. This is long enough for when the warning is valid // but will stop flooding the console log when we are legitimately @@ -307,30 +325,31 @@ public RobotContainer() throws Exception try { Timer.delay(30); + // ! Why does this take an argument? DriverStation.silenceJoystickConnectionWarning(true); } catch (Exception e) { } }).start(); // Log info about NavX. - + // ! How often does this happen? navx.dumpValuesToNetworkTables(); if (navx.isConnected()) Util.consoleLog("NavX connected version=%s", navx.getAHRS().getFirmwareVersion()); else { + // ! Update on shuffleboard. Exception e = new Exception("NavX is NOT connected!"); Util.logException(e); } // Configure autonomous routines and send to dashboard. - setAutoChoices(); // Configure the button bindings. - configureButtonBindings(); + // ! What are these so called trajectory files? // Load any trajectory files in a separate thread on first scheduler run. // We do this because trajectory loads can take up to 10 seconds to load so we want this // being done while we are getting started up. Hopefully will complete before we are ready to From 495a76601757451cb69b9767689b413bd7c2340e Mon Sep 17 00:00:00 2001 From: Robot Date: Wed, 2 Apr 2025 18:02:08 -0700 Subject: [PATCH 3/5] Some questions Changes to be committed: --- .../autos/A-Pod Left Lane 3L4.auto | 127 ++++++++++++++++++ ...auto => Practice Field Left Lane 3L4.auto} | 0 .../paths/A-Pod E-Right to LeftPickup.path | 54 ++++++++ .../paths/A-Pod HighStart to E-Right.path | 59 ++++++++ src/main/deploy/pathplanner/settings.json | 1 + .../java/Team4450/Robot25/RobotContainer.java | 28 ++-- .../Robot25/commands/DriveToAlgaeTag.java | 9 +- .../Robot25/commands/DriveToCoralTag.java | 39 +++--- 8 files changed, 280 insertions(+), 37 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/A-Pod Left Lane 3L4.auto rename src/main/deploy/pathplanner/autos/{Left Lane 3L4.auto => Practice Field Left Lane 3L4.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/A-Pod E-Right to LeftPickup.path create mode 100644 src/main/deploy/pathplanner/paths/A-Pod HighStart to E-Right.path diff --git a/src/main/deploy/pathplanner/autos/A-Pod Left Lane 3L4.auto b/src/main/deploy/pathplanner/autos/A-Pod Left Lane 3L4.auto new file mode 100644 index 0000000..7da835f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-Pod Left Lane 3L4.auto @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A-Pod HighStart to E-Right" + } + }, + { + "type": "named", + "data": { + "name": "Raise to L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Outtake Coral" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A-Pod E-Right to LeftPickup" + } + }, + { + "type": "named", + "data": { + "name": "Intake Coral" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftPickup to F-Right" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Raise to L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Outtake Coral" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "F-Right to LeftPickup" + } + }, + { + "type": "named", + "data": { + "name": "Intake Coral" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftPickup to F-Left" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Lane 3L4.auto b/src/main/deploy/pathplanner/autos/Practice Field Left Lane 3L4.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Left Lane 3L4.auto rename to src/main/deploy/pathplanner/autos/Practice Field Left Lane 3L4.auto diff --git a/src/main/deploy/pathplanner/paths/A-Pod E-Right to LeftPickup.path b/src/main/deploy/pathplanner/paths/A-Pod E-Right to LeftPickup.path new file mode 100644 index 0000000..c8decd3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A-Pod E-Right to LeftPickup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.715981259277075, + "y": 5.115002339678413 + }, + "prevControl": null, + "nextControl": { + "x": 5.129412322188593, + "y": 6.516260834474633 + }, + "isLocked": false, + "linkedName": "A-Pod E-Right" + }, + { + "anchor": { + "x": 1.0575889518992114, + "y": 7.099006835581519 + }, + "prevControl": { + "x": 1.4537143191199033, + "y": 5.851672432384792 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LeftPickup" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -145.0 + }, + "reversed": false, + "folder": "A-Pod", + "idealStartingState": { + "velocity": 1.0, + "rotation": -31.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Pod HighStart to E-Right.path b/src/main/deploy/pathplanner/paths/A-Pod HighStart to E-Right.path new file mode 100644 index 0000000..88e4c92 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A-Pod HighStart to E-Right.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.217001229745369, + "y": 7.465524811921297 + }, + "prevControl": null, + "nextControl": { + "x": 5.676006221064815, + "y": 5.8633861400462965 + }, + "isLocked": false, + "linkedName": "HighStart" + }, + { + "anchor": { + "x": 4.715981259277075, + "y": 5.115002339678413 + }, + "prevControl": { + "x": 5.282882667727779, + "y": 5.979526987565737 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "A-Pod E-Right" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.21123560855263157, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -31.0 + }, + "reversed": false, + "folder": "A-Pod", + "idealStartingState": { + "velocity": 1.0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 9fcb0e9..d68342d 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -6,6 +6,7 @@ "Misc. (good)", "Net to Pickup (good)", "Net to Reef (good)", + "A-Pod", "Pickups To Reefs (good)", "Processor to reef (good)", "Reef to Processor (good)", diff --git a/src/main/java/Team4450/Robot25/RobotContainer.java b/src/main/java/Team4450/Robot25/RobotContainer.java index 29764c8..c21b014 100644 --- a/src/main/java/Team4450/Robot25/RobotContainer.java +++ b/src/main/java/Team4450/Robot25/RobotContainer.java @@ -422,7 +422,7 @@ private void configureButtonBindings() .onTrue(new InstantCommand(driveBase::zeroGyro)); // toggle field-oriented driving mode. - new Trigger(() -> driverController.getBackButton()) + new Trigger(() -> driverController.getAButton()) .onTrue(new InstantCommand(driveBase::toggleFieldRelative)); //Holding Right D-Pad button sets X pattern to stop movement. @@ -490,12 +490,8 @@ private void configureButtonBindings() new ExtendClimber(climber), new InstantCommand(() -> algaeManipulator.extendOut()))); - new Trigger(() -> driverController.getBButton() && climber.pistonStatus() == true) + new Trigger(() -> driverController.getPOV() == 0) .onTrue(new RetractClimber(climber)); - - - new Trigger(() -> driverController.getAButton()) - .onTrue(new RetractClimber(climber)); new Trigger(() -> driverController.getXButton()) .onTrue(new IntakeAlgaeGround(elevatedManipulator)); @@ -506,19 +502,19 @@ private void configureButtonBindings() new Trigger(() -> driverController.getRightBumperButton()) .whileTrue(new DriveToAlgaeTag(driveBase, pvAlgaeTagCamera, true, true)); - new Trigger(() -> driverController.getLeftTrigger()) - .whileTrue(new AlignToTag(driveBase, pvCoralTagCameraRight, true, true) - .andThen(new DriveToCoralTag(driveBase, pvCoralTagCameraRight, true, true))); - - new Trigger(() -> driverController.getRightTrigger()) - .whileTrue(new AlignToTag(driveBase, pvCoralTagCameraLeft, true, true) - .andThen(new DriveToCoralTag(driveBase, pvCoralTagCameraLeft, true, true))); + // new Trigger(() -> driverController.getLeftTrigger()) + // .whileTrue(new AlignToTag(driveBase, pvCoralTagCameraRight, true, true) + // .andThen(new DriveToCoralTag(driveBase, pvCoralTagCameraRight, true, true))); // new Trigger(() -> driverController.getRightTrigger()) - // .whileTrue(new DriveToCoralTag(driveBase, pvCoralTagCameraLeft, true, true)); + // .whileTrue(new AlignToTag(driveBase, pvCoralTagCameraLeft, true, true) + // .andThen(new DriveToCoralTag(driveBase, pvCoralTagCameraLeft, true, true))); - // new Trigger(() -> driverController.getLeftTrigger()) - // .whileTrue(new DriveToCoralTag(driveBase, pvCoralTagCameraRight, true, true)); + new Trigger(() -> driverController.getRightTrigger()) + .whileTrue(new DriveToCoralTag(driveBase, pvCoralTagCameraLeft, true, true)); + + new Trigger(() -> driverController.getLeftTrigger()) + .whileTrue(new DriveToCoralTag(driveBase, pvCoralTagCameraRight, true, true)); // new Trigger(() -> driverController.getYButton()) // .onTrue(new InstantCommand(() -> algaeGroundIntake.stop())); diff --git a/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java b/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java index b781480..07470de 100644 --- a/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java +++ b/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java @@ -59,6 +59,7 @@ public void initialize (){ translationController.setTolerance(0.5); SmartDashboard.putString("DriveToAlgaeTag", "Tag Tracking Initialized"); + // SmartDashboard.putData("Algae Translation", translationController); } @Override @@ -94,6 +95,12 @@ public void execute() { double rotation = rotationController.calculate(targetYaw); // attempt to minimize double movement = translationController.calculate(targetPitch); // attempt to minimize + if(targetYaw < 11.2){ + rotation = 0; + } + else if(targetYaw > 12.2){ + rotation = 0; + } Util.consoleLog("in[yaw=%f, pitch=%f] out[rot=%f, mov=%f]", target.getYaw(), target.getPitch(), rotation, movement); @@ -109,7 +116,7 @@ public void execute() { public void end(boolean interrupted) { Util.consoleLog("interrupted=%b", interrupted); - if (alsoDrive) robotDrive.drive(0, 0, 0, false); + // if (alsoDrive) robotDrive.drive(0, 0, 0, false); if (initialFieldRel) robotDrive.toggleFieldRelative(); // restore beginning state diff --git a/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java b/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java index 8982268..6f39ae8 100644 --- a/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java +++ b/src/main/java/Team4450/Robot25/commands/DriveToCoralTag.java @@ -7,10 +7,10 @@ import edu.wpi.first.wpilibj2.command.Command; import Team4450.Robot25.subsystems.PhotonVision; -import java.util.Optional; - -import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.PhotonPipelineResult; + +import java.util.Optional; import Team4450.Robot25.subsystems.DriveBase; @@ -22,8 +22,8 @@ */ public class DriveToCoralTag extends Command { - PIDController rotationController = new PIDController(0.03, 0.0001, 0); // for rotating drivebase - PIDController translationController = new PIDController(0.04, 0.005, 0); // for moving drivebase in X,Y plane + PIDController rotationController = new PIDController(0.03, 0, 0); // for rotating drivebase + PIDController translationController = new PIDController(0.08, 0.005, 0); // for moving drivebase in X,Y plane DriveBase robotDrive; PhotonVision photonVision; private boolean alsoDrive; @@ -37,8 +37,8 @@ public DriveToCoralTag (DriveBase robotDrive, PhotonVision photonVision, boolean this.photonVision = photonVision; this.alsoDrive = alsoDrive; - SendableRegistry.addLW(translationController, "DriveToCoralTag Translation PID"); - SendableRegistry.addLW(rotationController, "DriveToCoralTag Rotation PID"); + SendableRegistry.addLW(translationController, "DriveToAlgaeTag Translation PID"); + SendableRegistry.addLW(rotationController, "DriveToAlgaeTag Rotation PID"); } public void initialize (){ @@ -58,7 +58,7 @@ public void initialize (){ translationController.setSetpoint(-15); // target should be at -15 pitch translationController.setTolerance(0.5); - SmartDashboard.putString("DriveToCoralTag", "Tag Tracking Initialized"); + SmartDashboard.putString("DriveToAlgaeTag", "Tag Tracking Initialized"); } @Override @@ -69,15 +69,15 @@ public void execute() { // logic for chosing "closest" target in PV subsystem Optional pipeline = photonVision.getLatestResult(); //PhotonTrackedTarget target = photonVision.getLatestResult(); - if (pipeline.isEmpty() || pipeline == null) { - nullTagCounter += 1; - return; - } + // if (pipeline.isEmpty() || pipeline == null) { + // nullTagCounter += 1; + // return; + // } - if(pipeline.get().getTargets().size() == 0){ - nullTagCounter += 1; - return; - } + // if(pipeline.get().getTargets().size() == 0){ + // nullTagCounter += 1; + // return; + // } PhotonTrackedTarget target = pipeline.get().getTargets().get(0); @@ -87,6 +87,7 @@ public void execute() { nullTagCounter += 1; return; } + double targetYaw = target.getYaw(); double targetPitch = target.getPitch(); @@ -99,18 +100,16 @@ public void execute() { if (alsoDrive) { robotDrive.driveRobotRelative(rotation, movement, 0); nullTagCounter = 0; - } else { robotDrive.setTrackingRotation(rotation); } } - @Override public void end(boolean interrupted) { Util.consoleLog("interrupted=%b", interrupted); - if (alsoDrive) robotDrive.drive(0, 0, 0, false); + // if (alsoDrive) robotDrive.drive(0, 0, 0, false); if (initialFieldRel) robotDrive.toggleFieldRelative(); // restore beginning state @@ -119,7 +118,7 @@ public void end(boolean interrupted) { robotDrive.disableTrackingSlowMode(); robotDrive.clearPPRotationOverride(); - SmartDashboard.putString("DriveToCoralTag", "Tag Tracking Ended"); + SmartDashboard.putString("DriveToAlgaeTag", "Tag Tracking Ended"); } } From 83ba82330a4b975e0f3e83eabbd2692dc30be5d2 Mon Sep 17 00:00:00 2001 From: Robot Date: Thu, 3 Apr 2025 06:25:34 -0700 Subject: [PATCH 4/5] More questions Changes to be committed: --- .../java/Team4450/Robot25/RobotContainer.java | 39 ++++++++++--------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/src/main/java/Team4450/Robot25/RobotContainer.java b/src/main/java/Team4450/Robot25/RobotContainer.java index c21b014..0f043e3 100644 --- a/src/main/java/Team4450/Robot25/RobotContainer.java +++ b/src/main/java/Team4450/Robot25/RobotContainer.java @@ -331,22 +331,24 @@ public RobotContainer() throws Exception }).start(); // Log info about NavX. - // ! How often does this happen? + // ! How often does this happen? Only once? It seems to send a lot of data to the log. navx.dumpValuesToNetworkTables(); if (navx.isConnected()) Util.consoleLog("NavX connected version=%s", navx.getAHRS().getFirmwareVersion()); else { - // ! Update on shuffleboard. + // ! Update on shuffleboard (Feature). Exception e = new Exception("NavX is NOT connected!"); Util.logException(e); } // Configure autonomous routines and send to dashboard. + // ! If this is only ran once (which it is) and defined in the same file do not make it a function and just write it out here. setAutoChoices(); // Configure the button bindings. + // ! If this is only ran once (which it is) and defined in the same file do not make it a function and just write it out here. configureButtonBindings(); // ! What are these so called trajectory files? @@ -367,9 +369,11 @@ public RobotContainer() throws Exception //PathPlannerTrajectory ppTestTrajectory = loadPPTrajectoryFile("richard"); + // ! Why is this logged here? Util.consoleLog(functionMarker); } + // ! We don't use 3 sticks and the launchpad so the comment is wrong. /** * Use this method to define your button->command mappings. * @@ -380,18 +384,24 @@ private void configureButtonBindings() { Util.consoleLog(); - // ------- Driver pad buttons ------------- + // ------- Driver controller buttons ------------- // For simple functions, instead of creating commands, we can call convenience functions on // the target subsystem from an InstantCommand. It can be tricky deciding what functions // should be an aspect of the subsystem and what functions should be in Commands... + // + // ! The answer the question above is none or all, going forward I do not want to mix, even if this is annoying. // POV buttons do same as alternate driving mode but without any lateral // movement and increments of 45deg. // new Trigger(()-> driverController.getPOV() != -1) // .onTrue(new PointToYaw(()->PointToYaw.yawFromPOV(driverController.getPOV()), driveBase, false)) + // + // + // ! Maybe some kind of order for registering commands. + // ! Maybe instead of by controller do it by locality of behavior. - // vibrate between 30 and 25 sec left in match. + // Vibrate both controllers starting at 30 and until 25 sec left in match. new Trigger(() -> Timer.getMatchTime() < 30 && Timer.getMatchTime() > 25).whileTrue(new StartEndCommand( () -> { driverController.setRumble(RumbleType.kBothRumble, 0.5); @@ -401,17 +411,6 @@ private void configureButtonBindings() utilityController.setRumble(RumbleType.kBothRumble, 0); })); - // holding top right bumper enables the alternate rotation mode in - // which the driver points stick to desired heading. - - //new Trigger(() -> driverController.getRightBumperButton()) - // .whileTrue(new PointToYaw( - // ()->PointToYaw.yawFromAxes( - // -MathUtil.applyDeadband(driverController.getRightX(), Constants.DRIVE_DEADBAND), - // -MathUtil.applyDeadband(driverController.getRightY(), Constants.DRIVE_DEADBAND) - // ), driveBase, false - //)); - // toggle slow-mode new Trigger(() -> driverController.getLeftBumperButton()) .onTrue(new InstantCommand(driveBase::enableSlowMode)) @@ -519,7 +518,7 @@ private void configureButtonBindings() // new Trigger(() -> driverController.getYButton()) // .onTrue(new InstantCommand(() -> algaeGroundIntake.stop())); - // -------- Utility pad buttons ---------- + // -------- Utility controller buttons ---------- //Use Preset Command for the following: @@ -613,10 +612,9 @@ private void configureButtonBindings() .onTrue(new InstantCommand(() -> elevatedManipulator.algaeManipulator.pivotUp())) .onFalse(new InstantCommand(() -> elevatedManipulator.algaeManipulator.pivotDown())); - - - } + + // ! Again should not be a function because it is only called once. /** * Use this to pass the autonomous command to the main {@link Robot} class. * Determines which auto command from the selection made by the operator on the @@ -650,6 +648,7 @@ public Command getAutonomousCommand() { return autoCommand; } + // ! Do not use getters just make it public. public static String getAutonomousCommandName() { return autonomousCommandName; @@ -735,10 +734,12 @@ public void resetFaults() if (monitorPDPThread != null) monitorPDPThread.reset(); } + // ! Put this in the correct spot. public void fixPathPlannerGyro() { driveBase.fixPathPlannerGyro(); } + // ! This should not be there if it is used. /** * Loads a PathPlanner path file into a path planner trajectory. * @param fileName Name of file. Will automatically look in deploy directory and add the .path ext. From 22b718b1f2c600ef4a28fe62a145ce85f9baac33 Mon Sep 17 00:00:00 2001 From: Robot Date: Thu, 3 Apr 2025 18:20:01 -0700 Subject: [PATCH 5/5] Changes to be committed: --- .factorypath | 4 ++++ generalChanges.txt | 5 +++++ src/main/java/Team4450/Robot25/Robot.java | 23 ++++++++++++++++------- style.txt | 7 +++++++ 4 files changed, 32 insertions(+), 7 deletions(-) create mode 100644 .factorypath create mode 100644 generalChanges.txt create mode 100644 style.txt diff --git a/.factorypath b/.factorypath new file mode 100644 index 0000000..9447675 --- /dev/null +++ b/.factorypath @@ -0,0 +1,4 @@ + + + + diff --git a/generalChanges.txt b/generalChanges.txt new file mode 100644 index 0000000..dc314c2 --- /dev/null +++ b/generalChanges.txt @@ -0,0 +1,5 @@ +Change the watchdog timer to be 0.02 instead of the larger one (The one that is flooding the roborio). +How to minimize the logging that is flooding the robo rio? +Add a way to delete the deploy directory on each upload (https://www.chiefdelphi.com/t/clearing-the-deploy-directory-on-a-roborio-running-commands-on-the-roborio/458273/5). +What is the overhead for sending shuffleboard periodics? +Make controllers not switch on the drive station. diff --git a/src/main/java/Team4450/Robot25/Robot.java b/src/main/java/Team4450/Robot25/Robot.java index d8a4f33..73fa1c5 100644 --- a/src/main/java/Team4450/Robot25/Robot.java +++ b/src/main/java/Team4450/Robot25/Robot.java @@ -35,18 +35,24 @@ public class Robot extends TimedRobot public void robotInit() { try { + // ! Do I not understand Java or what is this for? robot = this; // Stored in Constants. + // ! What is the LCD, just to definition does not work on it and I don't see the import. LCD.clearAll(); LCD.printLine(LCD_1, "Mode: RobotInit"); // Set up our custom logger. + // ! What is the difference. Util.CustomLogger.setup(); + // ! If this is the difference it should be before the line of code not after. + // ! If we are overrunning the periodic should we not just make it less frequent. + // The wpilib classes that underlie this class generate a lot of warning // messages that flood the Riolog and make it almost unusable. The warnings - // are about our code in the robotPeriodic() function taking longer than .02 + // are about our code in the robotPeriodic() function taking longer than 0.02 // sec to execute. It's very hard to stay under this limit. So...copied classes // from the wpilib name space to inside this project and modified them to allow // us to control these warnings and log some of them to our log file. The warnings @@ -62,15 +68,16 @@ public void robotInit() // IterativeRobotBase and Watchdog have been modified. TimedRobot is needed to // pull in these modified classes. Look for "4450" in the code for the mods. // - // Note that the periodic function is called very .02 sec. If our code runs too + // Note that the periodic function is called very 0.02 sec. If our code runs too // long that can lead to various control problems. But, it has proven hard to - // do anything useful and not exceed the .02 sec watchdogs, though we have made + // do anything useful and not exceed the 0.02 sec watchdogs, though we have made // improvements to various functions to reduce execution time or used threading. // We have trimmed the volume of overrun messages but they still occur. enableWatchDogWarning(false); enableWatchDogFlush(false); this.setWatchDogTimeout(.04); + // ! What does this mean, does it mean that commands are ran at the top of every second? CommandScheduler.getInstance().setPeriod(1.0); // Set Java to catch any uncaught exceptions and record them in our log file. @@ -93,13 +100,12 @@ public void uncaughtException(Thread t, Throwable e) SendableVersion.INSTANCE.init(PROGRAM_NAME); + // Why will this information not be correct during simulation. // Note: under simulation, this information will not be correct. Util.consoleLog("%s compiled by %s at %s (branch=%s, commit=%s)", SendableVersion.INSTANCE.getProgramVersion(), SendableVersion.INSTANCE.getUser(), SendableVersion.INSTANCE.getTime(), SendableVersion.INSTANCE.getBranch(), SendableVersion.INSTANCE.getCommit()); - //Util.consoleLog("manifest path=%s", SendableVersion.INSTANCE.getPath()); - // Send program version to the dashboard. SmartDashboard.putString("Program", PROGRAM_NAME); @@ -108,6 +114,8 @@ public void uncaughtException(Thread t, Throwable e) Util.consoleLog("Robot WPILib=%s Java=%s", WPILibVersion.Version, System.getProperty("java.version")); Util.consoleLog("RobotLib=%s", LibraryVersion.version); + // ! Is there a way to fix this. + // Note: Any Sendables added to SmartDashboard or Shuffleboard are sent to the DS on every // loop of a TimedRobot. In this case it means that the SendableVersion data would be sent // to the DS every 20ms even though it does not change. Sendables must be added to the SDB @@ -131,7 +139,8 @@ public void uncaughtException(Thread t, Throwable e) Util.logException(e); endCompetition(); } - + // ! What is this? + // ! Is this just a number of '-' characters to split up the log. Util.consoleLog(functionMarker); } @@ -147,7 +156,7 @@ public void uncaughtException(Thread t, Throwable e) @Override public void robotPeriodic() { - // This function is called approx every .02 second. + // This function is called approx every 0.02 second. // Runs the Scheduler. It is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. Scheduler must be called from the robot's periodic diff --git a/style.txt b/style.txt new file mode 100644 index 0000000..a8136b2 --- /dev/null +++ b/style.txt @@ -0,0 +1,7 @@ +Functions that are ran once will not be functions. +Getters and Setters will not be used make it public instead. +Inline comments have // A space and then the sentance, also as you can see they start with a capital letter and end with proper punctuation. +Sentances start with a capital letter and end with a period or exclimation point or question mark. +Large sections of commented code will be deleted because we will have git history. +Minimize runs during the periodical. +Why are empty console logs used.