Skip to content

Commit

Permalink
Make the first working auto path! Woohoo! I am so happy with this inf…
Browse files Browse the repository at this point in the history
…ormation!
  • Loading branch information
GalexY727 committed Jan 18, 2024
1 parent 64d3603 commit fe55158
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 15 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import java.util.Optional;

import com.pathplanner.lib.commands.PathPlannerAuto;
import com.revrobotics.CANSparkBase;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand Down Expand Up @@ -87,8 +88,7 @@ private void configureDriverBindings() {
}

public Command getAutonomousCommand() {
// TODO: Add auto commands here
return null;
return new PathPlannerAuto("BetterThanBread");
}

public void onDisabled() {}
Expand Down
52 changes: 44 additions & 8 deletions src/main/java/frc/robot/subsystems/Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,27 +4,33 @@

package frc.robot.subsystems;

import java.util.Optional;
import java.util.function.Supplier;

import com.ctre.phoenix6.hardware.Pigeon2;
import com.pathplanner.lib.auto.AutoBuilder;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.DriverUI;
import frc.robot.commands.Drive;
import frc.robot.util.Constants.AutoConstants;
import frc.robot.util.Constants.DriveConstants;
import frc.robot.util.Constants.FieldConstants;
import monologue.Logged;
Expand Down Expand Up @@ -57,14 +63,17 @@ public class Swerve extends SubsystemBase implements Logged {
DriveConstants.BACK_RIGHT_CHASSIS_ANGULAR_OFFSET);

@Log.NT
SwerveModuleState[] measuredStates;
SwerveModuleState[] swerveMeasuredStates;

@Log.NT
SwerveModuleState[] desiredStates;
SwerveModuleState[] swerveDesiredStates;

@Log.NT
Pose3d robotPose3d = new Pose3d();

@Log.NT
Pose2d robotPose2d = new Pose2d();

// The gyro sensor
private final Pigeon2 gyro = new Pigeon2(DriveConstants.GYRO_CAN_ID);

Expand Down Expand Up @@ -103,6 +112,19 @@ public class Swerve extends SubsystemBase implements Logged {
*/
public Swerve() {

AutoBuilder.configureHolonomic(
this::getPose,
this::resetOdometry,
this::getRobotRelativeSpeeds,
this::drive,
AutoConstants.HPFC,
() -> {
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
return alliance.isPresent() && alliance.get() == Alliance.Red;
},
this
);

resetEncoders();
gyro.setYaw(0);
setBrakeMode();
Expand All @@ -114,30 +136,35 @@ public Swerve() {
public void periodic() {

poseEstimator.updateWithTime(DriverUI.currentTimestamp, gyro.getRotation2d(), getModulePositions());

logPositions();

}

public void logPositions() {

if (FieldConstants.IS_SIMULATION) {
for (MAXSwerveModule mod : swerveModules) {
mod.tick();
}
}

measuredStates = new SwerveModuleState[] {
swerveMeasuredStates = new SwerveModuleState[] {
frontLeft.getState(), frontRight.getState(), rearLeft.getState(), rearRight.getState()
};

ChassisSpeeds speeds = DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(measuredStates);
ChassisSpeeds speeds = DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(swerveMeasuredStates);
resetOdometry(
getPose().exp(
new Twist2d(
0, 0,
speeds.omegaRadiansPerSecond * .02)));

}

public void logPositions() {
DriverUI.field.setRobotPose(getPose());
SmartDashboard.putNumber("Swerve/RobotRotation", gyro.getRotation2d().getRadians());

robotPose2d = getPose();

robotPose3d = new Pose3d(
new Translation3d(
Expand All @@ -148,7 +175,8 @@ public void logPositions() {
* DriveConstants.ROBOT_LENGTH_METERS / 2.0,
Rotation2d.fromDegrees(gyro.getPitch().refresh().getValue()).getSin() *
DriveConstants.ROBOT_LENGTH_METERS / 2.0)),
gyro.getRotation3d());
FieldConstants.IS_SIMULATION ? new Rotation3d(0, 0, getPose().getRotation().getRadians()) : gyro.getRotation3d());

}

/**
Expand All @@ -164,6 +192,10 @@ public SwerveDrivePoseEstimator getPoseEstimator() {
return poseEstimator;
}

public void drive(ChassisSpeeds speeds) {
drive(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond, false);
}

public void drive(double xSpeed, double ySpeed, double rotSpeed, boolean fieldRelative) {

xSpeed *= (DriveConstants.MAX_SPEED_METERS_PER_SECOND * speedMultiplier);
Expand All @@ -179,6 +211,10 @@ public void drive(double xSpeed, double ySpeed, double rotSpeed, boolean fieldRe
setModuleStates(swerveModuleStates);
}

public ChassisSpeeds getRobotRelativeSpeeds() {
return DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates());
}

/**
* Sets the wheels into an X formation to prevent movement.
*/
Expand Down Expand Up @@ -206,7 +242,7 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
rearLeft.setDesiredState(desiredStates[2]);
rearRight.setDesiredState(desiredStates[3]);

this.desiredStates = desiredStates;
this.swerveDesiredStates = desiredStates;
}

public void resetOdometry(Pose2d pose) {
Expand Down
29 changes: 24 additions & 5 deletions src/main/java/frc/robot/util/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@

import java.util.ArrayList;

import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;
import com.revrobotics.CANSparkBase;
import java.util.Optional;
import edu.wpi.first.math.controller.HolonomicDriveController;
Expand All @@ -30,7 +33,7 @@ public final class Constants {
public static final class DriveConstants {
// Driving Parameters - Note that these are not the maximum capable speeds of
// the robot, rather the allowed maximum speeds
public static double MAX_SPEED_METERS_PER_SECOND = 3;
public static double MAX_SPEED_METERS_PER_SECOND = 4.277;

public static final double MAX_ANGULAR_SPEED_RADS_PER_SECOND = 4 * Math.PI; // radians per second

Expand Down Expand Up @@ -79,10 +82,11 @@ public static final class DriveConstants {
public static final class AutoConstants {

// The below values need to be tuned for each new robot.
public static final double MAX_SPEED_METERS_PER_SECOND = 3;
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3.5;
public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 2 * Math.PI;
public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = 3 * Math.PI;
// They are curently set to the values suggested by Choreo
public static final double MAX_SPEED_METERS_PER_SECOND = 4.277;
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 7.344;
public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 10.468;
public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = 37.053;

public static final double PX_CONTROLLER = 1;
public static final double PY_CONTROLLER = 1;
Expand Down Expand Up @@ -120,6 +124,21 @@ public static final class AutoConstants {
new TrapezoidProfile.Constraints(
AutoConstants.MAX_ANGULAR_SPEED_RADIANS_PER_SECOND,
AutoConstants.MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED)));

public static final HolonomicPathFollowerConfig HPFC = new HolonomicPathFollowerConfig(
new PIDConstants(
AutoConstants.X_CORRECTION_P,
AutoConstants.X_CORRECTION_I,
AutoConstants.X_CORRECTION_D),
new PIDConstants(
AutoConstants.ROTATION_CORRECTION_P,
AutoConstants.ROTATION_CORRECTION_I,
AutoConstants.ROTATION_CORRECTION_D),
MAX_SPEED_METERS_PER_SECOND,
Math.hypot(DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH),
new ReplanningConfig()
);

}

public static final class ModuleConstants {
Expand Down

0 comments on commit fe55158

Please sign in to comment.