Skip to content
This repository has been archived by the owner on Dec 2, 2024. It is now read-only.

Adding PathPlanner #10

Closed
wants to merge 3 commits into from
Closed

Adding PathPlanner #10

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
af1d86e
Adding PathPlanner
Turbojax07 Oct 26, 2024
348c797
Importing Logger to RobotContainer
Turbojax07 Oct 27, 2024
fd67f1f
Merge branch 'main' into main
Turbojax07 Oct 27, 2024
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
29 changes: 27 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,18 @@

package frc.robot;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathConstraints;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
Expand Down Expand Up @@ -52,6 +59,7 @@
import frc.robot.subsystems.shooter.ShooterIOReplay;
import frc.robot.subsystems.shooter.ShooterIOSim;
import frc.robot.subsystems.shooter.ShooterIOSparkMax;
import org.littletonrobotics.junction.Logger;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand Down Expand Up @@ -323,8 +331,25 @@ private static double teleopAxisAdjustment(double x) {
}

public Command getAutonomousCommand() {
// TODO Auto-generated method stub
return null;
Pose2d endPose =
m_drive
.getPose()
.plus(
new Transform2d(
m_drive.getPose().getX() + Units.feetToMeters(2),
m_drive.getPose().getY(),
m_drive.getRotation()));

Logger.recordOutput("Initial Pose", m_drive.getPose());
Logger.recordOutput("Desired Pose", endPose);

return AutoBuilder.pathfindToPose(
endPose,
new PathConstraints(
DriveConstants.maxLinearVelocity,
DriveConstants.maxLinearAccel,
DriveConstants.maxAngularVelocity,
DriveConstants.maxAngularAccel));
}

// public Command getAutonomousCommand() {
Expand Down
82 changes: 53 additions & 29 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@
package frc.robot.subsystems.drive;

import com.google.common.collect.Streams;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.ReplanningConfig;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
Expand All @@ -27,6 +30,9 @@
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.DriveConstants;
Expand All @@ -38,6 +44,8 @@
import org.littletonrobotics.junction.Logger;

public class Drive extends SubsystemBase {
private final Field2d field = new Field2d();

static final Lock odometryLock = new ReentrantLock();
private final GyroIO gyroIO;
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
Expand All @@ -53,7 +61,8 @@ public class Drive extends SubsystemBase {
new SwerveModulePosition()
};
private SwerveDrivePoseEstimator poseEstimator =
new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
new SwerveDrivePoseEstimator(
kinematics, rawGyroRotation, lastModulePositions, new Pose2d(1.4, 6, new Rotation2d()));

public Drive(
GyroIO gyroIO,
Expand All @@ -72,9 +81,27 @@ public Drive(
// SparkMaxOdometryThread.getInstance().start();
// PhoenixOdometryThread.getInstance().start();

AutoBuilder.configureHolonomic(
this::getPose,
this::setPose,
this::getVelocity,
this::setVelocity,
new HolonomicPathFollowerConfig(
DriveConstants.maxLinearVelocity,
DriveConstants.trackWidth / 2.0,
new ReplanningConfig()),
() ->
(DriverStation.getAlliance().isPresent())
? ((DriverStation.getAlliance().get().equals(Alliance.Blue)) ? false : true)
: false,
this);
}

public void periodic() {
field.setRobotPose(getPose());

SmartDashboard.putData(field);

odometryLock.lock(); // Prevents odometry updates while reading data
gyroIO.processInputs(gyroInputs);
for (var module : modules) {
Expand Down Expand Up @@ -159,34 +186,31 @@ public static Translation2d[] getModuleTranslations() {
}

public Command runVoltageTeleopFieldRelative(Supplier<ChassisSpeeds> speeds) {
return this.run(
() -> {
var allianceSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), gyroInputs.yawPosition);
// Calculate module setpoints
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(allianceSpeeds, 0.02);
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(
setpointStates, DriveConstants.maxLinearVelocity);

Logger.recordOutput("Drive/Target Speeds", discreteSpeeds);
Logger.recordOutput("Drive/Speed Error", discreteSpeeds.minus(getVelocity()));
Logger.recordOutput(
"Drive/Target Chassis Speeds Field Relative",
ChassisSpeeds.fromRobotRelativeSpeeds(discreteSpeeds, getRotation()));

// Send setpoints to modules
SwerveModuleState[] optimizedSetpointStates =
Streams.zip(
Arrays.stream(modules),
Arrays.stream(setpointStates),
(m, s) -> m.runSetpoint(s))
.toArray(SwerveModuleState[]::new);

// Log setpoint states
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
});
return this.run(() -> setVelocity(speeds.get()));
}

public void setVelocity(ChassisSpeeds speeds) {
var allianceSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, gyroInputs.yawPosition);
// Calculate module setpoints
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(allianceSpeeds, 0.02);
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearVelocity);

Logger.recordOutput("Drive/Target Speeds", discreteSpeeds);
Logger.recordOutput("Drive/Speed Error", discreteSpeeds.minus(getVelocity()));
Logger.recordOutput(
"Drive/Target Chassis Speeds Field Relative",
ChassisSpeeds.fromRobotRelativeSpeeds(discreteSpeeds, getRotation()));

// Send setpoints to modules
SwerveModuleState[] optimizedSetpointStates =
Streams.zip(
Arrays.stream(modules), Arrays.stream(setpointStates), (m, s) -> m.runSetpoint(s))
.toArray(SwerveModuleState[]::new);

// Log setpoint states
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
}

@AutoLogOutput(key = "Odometry/Velocity")
Expand Down
Loading