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/Constants.java b/src/main/java/Team4450/Robot25/Constants.java index 7bcb977..abbddba 100644 --- a/src/main/java/Team4450/Robot25/Constants.java +++ b/src/main/java/Team4450/Robot25/Constants.java @@ -287,4 +287,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/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/src/main/java/Team4450/Robot25/RobotContainer.java b/src/main/java/Team4450/Robot25/RobotContainer.java index ad279c9..9826c90 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,33 @@ 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? 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 (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? // 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 @@ -348,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. * @@ -361,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); @@ -382,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)) @@ -474,8 +492,8 @@ private void configureButtonBindings() new Trigger(() -> driverController.getPOV() == 0) .onTrue(new RetractClimber(climber)); - new Trigger(() -> driverController.getXButton()) - .onTrue(new IntakeAlgaeGround(elevatedManipulator)); + new Trigger(() -> driverController.getXButton()) + .onTrue(new IntakeAlgaeGround(elevatedManipulator)); new Trigger(() -> driverController.getYButton()) .onTrue(new Preset(elevatedManipulator, PresetPosition.RESET)); @@ -500,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: @@ -593,10 +611,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 @@ -630,6 +647,7 @@ public Command getAutonomousCommand() { return autoCommand; } + // ! Do not use getters just make it public. public static String getAutonomousCommandName() { return autonomousCommandName; @@ -715,10 +733,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. diff --git a/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java b/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java index cb9d800..092b599 100644 --- a/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java +++ b/src/main/java/Team4450/Robot25/commands/DriveToAlgaeTag.java @@ -60,6 +60,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 691c006..39c448c 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; @@ -35,7 +35,7 @@ public DriveToCoralTag (DriveBase robotDrive, PhotonVision photonVision, boolean this.robotDrive = robotDrive; this.photonVision = photonVision; this.alsoDrive = alsoDrive; - + // if (alsoDrive) addRequirements(robotDrive); SendableRegistry.addLW(translationController, "DriveToCoralTag Translation PID"); @@ -59,7 +59,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 @@ -99,18 +99,16 @@ public void execute() { if (alsoDrive) { robotDrive.driveRobotRelative(rotation, movement, 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 +117,7 @@ public void end(boolean interrupted) { robotDrive.disableTrackingSlowMode(); robotDrive.clearPPRotationOverride(); - SmartDashboard.putString("DriveToCoralTag", "Tag Tracking Ended"); + SmartDashboard.putString("DriveToAlgaeTag", "Tag Tracking Ended"); } } \ No newline at end of file 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.