Merge pull request #52 from Ocebots/commenting

Author: KaraParikh1

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -60,6 +60,7 @@ public class Drivetrain extends SubsystemBase {
           CANMappings.REAR_RIGHT_TURNING,
           DrivetrainConfig.BACK_RIGHT_CHASSIS_ANGULAR_OFFSET);
 
+  // creates PID controllers
   public final PIDController xController =
       new PIDController(
           OrbitConfig.ORBIT_DISTANCE_P, OrbitConfig.ORBIT_DISTANCE_I, OrbitConfig.ORBIT_DISTANCE_D);
@@ -70,31 +71,38 @@ public class Drivetrain extends SubsystemBase {
       new PIDController(
           OrbitConfig.ORBIT_ROTATION_P, OrbitConfig.ORBIT_ROTATION_I, OrbitConfig.ORBIT_ROTATION_D);
 
+  // Creates PhotonPoseEstimator that estimates the robot position from one camera
   private PhotonPoseEstimator vision =
       new PhotonPoseEstimator(
           VisionConfig.LAYOUT, VisionConfig.STRATEGY, VisionConfig.CAMERA_POSITION);
 
+  // Creates PhotonPoseEstimator that estimates the robot position from another camera
   private PhotonPoseEstimator driverVision =
       new PhotonPoseEstimator(
           VisionConfig.LAYOUT, VisionConfig.STRATEGY, VisionConfig.DRIVER_CAMERA_POSITION);
 
+  // creates Ultrasonic rangeFinder that measures distance (currently not in use)
   Ultrasonic rangeFinder = new Ultrasonic(1, 2);
 
   // The gyro sensor
   @NotLogged private final AHRS gyro = new AHRS(AHRS.NavXComType.kMXP_SPI);
 
+  // creates new field 2d to be shared with smart dashboard
   @NotLogged private final Field2d field = new Field2d();
 
   // Slew rate filter variables for controlling lateral acceleration
   @NotLogged
   private SlewRateLimiter magLimiter = new SlewRateLimiter(DrivetrainConfig.MAX_ACCELERATION);
 
+  // slew rate filter variables for controlling rotation acceleration
   @NotLogged
   private SlewRateLimiter rotLimiter =
       new SlewRateLimiter(DrivetrainConfig.MAX_ROTATIONAL_ACCELERATION);
 
+  // creates new robot chassis speeds
   @Logged private ChassisSpeeds desiredChassisSpeeds = new ChassisSpeeds();
 
+  // creates new swerve drive pose estimator
   @NotLogged
   public SwerveDrivePoseEstimator poseEstimator =
       new SwerveDrivePoseEstimator(
@@ -109,8 +117,11 @@ public class Drivetrain extends SubsystemBase {
           new Pose2d(0, 0, getHeading()));
 
   public Drivetrain() {
+    // sends field data to smart dashboard
     SmartDashboard.putData(field);
+    // makes sure that rotController automatically maps shortest route to setpoint
     rotController.enableContinuousInput(-Math.PI, Math.PI);
+    // sets xController, yController, and rotController position and velocity tolerance
     xController.setTolerance(
         DrivetrainConfig.DISTANCE_POSITION_TOLERANCE, DrivetrainConfig.DISTANCE_VELOCITY_TOLERANCE);
     yController.setTolerance(
@@ -119,6 +130,7 @@ public Drivetrain() {
         DrivetrainConfig.ROTATION_POSITION_TOLERANCE, DrivetrainConfig.ROTATION_VELOCITY_TOLERANCE);
   }
 
+  // Logs desired swerve module states
   @Logged(name = "desiredStates")
   public SwerveModuleState[] getDesiredStates() {
     return new SwerveModuleState[] {
@@ -129,6 +141,7 @@ public SwerveModuleState[] getDesiredStates() {
     };
   }
 
+  // Logs actual swerve module states
   @Logged(name = "actualStates")
   public SwerveModuleState[] getActualStates() {
     return new SwerveModuleState[] {
@@ -139,6 +152,7 @@ public SwerveModuleState[] getActualStates() {
     };
   }
 
+  // logs actual chassis speeds
   @Logged(name = "actualChassisSpeeds")
   public ChassisSpeeds getChassisSpeeds() {
     return DrivetrainConfig.DRIVE_KINEMATICS.toChassisSpeeds(
@@ -150,25 +164,31 @@ public ChassisSpeeds getChassisSpeeds() {
         });
   }
 
+  // sets desired chassis speeds given a speed and gives swerveModuleStates new module states
   public void setChassisSpeeds(ChassisSpeeds speeds) {
     desiredChassisSpeeds = speeds;
     var swerveModuleStates = DrivetrainConfig.DRIVE_KINEMATICS.toSwerveModuleStates(speeds);
 
+    // renormalizes wheel speeds if any individual speed is above the specified maximum given the
+    // current swerve module states and the absolute maximum speed the swerve modules can reach
     SwerveDriveKinematics.desaturateWheelSpeeds(
         swerveModuleStates, DrivetrainConfig.MAX_SPEED_METERS_PER_SECOND);
 
+    // sets the desired states of the swerve modules
     this.frontLeft.setDesiredState(swerveModuleStates[0]);
     this.frontRight.setDesiredState(swerveModuleStates[1]);
     this.rearLeft.setDesiredState(swerveModuleStates[2]);
     this.rearRight.setDesiredState(swerveModuleStates[3]);
   }
 
+  // the range measured from the rangefinder in millimeters
   public double getDistance() {
     return rangeFinder.getRangeMM();
   }
 
   @Override
   public void periodic() {
+    // has ultrasonic sensor send ping and get distance measurement periodically
     rangeFinder.ping();
     // Update the odometry in the periodic block
 
@@ -182,11 +202,22 @@ public void periodic() {
               this.rearRight.getPosition()
             }));
 
+    // Updates the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE_STRATEGY
+    // (not in use)
     vision.setReferencePose(this.poseEstimator.getEstimatedPosition());
     driverVision.setReferencePose(this.poseEstimator.getEstimatedPosition());
 
+    // Puts the pose data from one camera into a list
     List<PhotonPipelineResult> results = Vision.camera.getAllUnreadResults();
 
+    // If there is pose data from the cameras, get the latest estimated pose and update the 'vision'
+    // photon pose estimator
+    // If there is no multi tag result and the distance from the camera to the target is greater
+    // than
+    // 4 meters, return
+    // Otherwise, add the latest vision pose estimate to a filter with the odometry pose estimate
+    // and set
+    // the guessed pose from that to the current pose
     if (!results.isEmpty()) {
       PhotonPipelineResult result = results.get(results.size() - 1);
       vision
@@ -249,6 +280,7 @@ public Command followLine(
         this);
   }
 
+  // Initializes and logs targetAngle and lastDistance
   @Logged public Rotation2d targetAngle;
   @Logged public double lastDistance;