diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73a7347..9722d8a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,7 +37,7 @@ public class RobotContainer { true, drivetrain); private int lastButtonPressed; - // slows down robot durring pickup + // slows down robot during pickup private Command pickup = coral .pickUpCoral() diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 5ef3b62..4de9837 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/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); - private PhotonPoseEstimator driverVision = + // Creates PhotonPoseEstimator that estimates the robot position from the other camera + private PhotonPoseEstimator elevatorBodyVision = 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 + // 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 chassisspeeds @Logged private ChassisSpeeds desiredChassisSpeeds = new ChassisSpeeds(); + // creates new swervedriveposeestimator @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,28 +164,35 @@ 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 + // Update the odometry in the periodic block field.setRobotPose( this.poseEstimator.update( getHeading(), @@ -182,11 +203,21 @@ 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()); + elevatorBodyVision.setReferencePose(this.poseEstimator.getEstimatedPosition()); + // Puts the pose data from one camera into a list List 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 multitag 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 @@ -230,6 +261,8 @@ public Pose2d getPose() { return this.poseEstimator.getEstimatedPosition(); } + // sets Chassis Speeds according to the goal distance and rotation according to where the sonar + // is facing public Command followLine( DoubleSupplier distance, DoubleSupplier speed, Supplier rotation) { return Commands.runEnd( @@ -249,6 +282,7 @@ public Command followLine( this); } + // Initializes and logs targetAngle and lastDistance @Logged public Rotation2d targetAngle; @Logged public double lastDistance;