Skip to content

Commenting drivetrain and robotcontainer fix #51

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
sophiep29 wants to merge 3 commits into from
Closed

Commenting drivetrain and robotcontainer fix #51

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
13a0255
Commenting drivetrain and robotcontainer fix
sophiep29 May 23, 2025
508bc83
Commenting drivetrain and robotcontainer fix
sophiep29 May 23, 2025
8047e20
Commenting drivetrain and robotcontainer fix
sophiep29 Jun 2, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
42 changes: 38 additions & 4 deletions src/main/java/frc/robot/subsystems/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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[] {
Expand All @@ -129,6 +141,7 @@ public SwerveModuleState[] getDesiredStates() {
};
}

// Logs actual swerve module states
@Logged(name = "actualStates")
public SwerveModuleState[] getActualStates() {
return new SwerveModuleState[] {
Expand All @@ -139,6 +152,7 @@ public SwerveModuleState[] getActualStates() {
};
}

// logs actual chassis speeds
@Logged(name = "actualChassisSpeeds")
public ChassisSpeeds getChassisSpeeds() {
return DrivetrainConfig.DRIVE_KINEMATICS.toChassisSpeeds(
Expand All @@ -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(),
Expand All @@ -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<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 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
Expand Down Expand Up @@ -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<Rotation2d> rotation) {
return Commands.runEnd(
Expand All @@ -249,6 +282,7 @@ public Command followLine(
this);
}

// Initializes and logs targetAngle and lastDistance
@Logged public Rotation2d targetAngle;
@Logged public double lastDistance;

Expand Down
Loading