Skip to content

Commit

Permalink
pathplanner
Browse files Browse the repository at this point in the history
  • Loading branch information
dannynotsmart committed Jan 17, 2024
1 parent c672b0a commit 6fd7b56
Show file tree
Hide file tree
Showing 7 changed files with 292 additions and 0 deletions.
61 changes: 61 additions & 0 deletions src/main/deploy/pathplanner/autos/3 note auto.auto
Original file line number Diff line number Diff line change
@@ -0,0 +1,61 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 0.9751772230817188,
"y": 3.8807583069711598
},
"rotation": 179.49741833327897
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "note 1"
}
},
{
"type": "named",
"data": {
"name": null
}
},
{
"type": "path",
"data": {
"pathName": "note 2"
}
},
{
"type": "named",
"data": {
"name": null
}
},
{
"type": "path",
"data": {
"pathName": "note 3"
}
},
{
"type": "named",
"data": {
"name": null
}
},
{
"type": "path",
"data": {
"pathName": "end"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}
1 change: 1 addition & 0 deletions src/main/deploy/pathplanner/navgrid.json
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/end.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 2.4397903341261227,
"y": 6.772669545339091
},
"prevControl": null,
"nextControl": {
"x": 2.8689126469124426,
"y": 6.912600734289288
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.626573071327961,
"y": 7.4723254901055265
},
"prevControl": {
"x": 5.224420994294654,
"y": 7.178469993302079
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
}
49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/note 1.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.9751772230817188,
"y": 3.8807583069711598
},
"prevControl": null,
"nextControl": {
"x": 1.6002032004064017,
"y": 4.533770522086501
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.4397903341261227,
"y": 4.30055187383102
},
"prevControl": {
"x": 1.7681206271503453,
"y": 4.841619137783731
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 151.03994330604968,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
}
49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/note 2.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 2.4397903341261227,
"y": 4.30055187383102
},
"prevControl": null,
"nextControl": {
"x": 1.4696007573833327,
"y": 4.664372965109567
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.4397903341261227,
"y": 5.6438912877825755
},
"prevControl": {
"x": 1.833421848661879,
"y": 5.989054887200684
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 177.03906386583623,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
}
49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/note 3.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 2.4397903341261227,
"y": 5.6438912877825755
},
"prevControl": null,
"nextControl": {
"x": 1.0404784445970123,
"y": 6.222273535459252
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.4397903341261227,
"y": 6.772669545339091
},
"prevControl": {
"x": 2.0106680213398027,
"y": 6.632738356388893
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": -147.52880770915144,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
}
34 changes: 34 additions & 0 deletions src/main/java/org/sciborgs1155/robot/drive/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,11 @@
import static org.sciborgs1155.robot.drive.DriveConstants.*;

import com.kauailabs.navx.frc.AHRS;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.filter.SlewRateLimiter;
Expand All @@ -15,6 +20,7 @@
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.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand All @@ -31,6 +37,8 @@
import org.photonvision.EstimatedRobotPose;
import org.sciborgs1155.robot.Constants;
import org.sciborgs1155.robot.Robot;
import org.sciborgs1155.robot.drive.DriveConstants.Rotation;
import org.sciborgs1155.robot.drive.DriveConstants.Translation;

public class Drive extends SubsystemBase implements Logged, AutoCloseable {

Expand Down Expand Up @@ -109,6 +117,28 @@ public Drive(ModuleIO frontLeft, ModuleIO frontRight, ModuleIO rearLeft, ModuleI
}

SmartDashboard.putData("drive quasistatic forward", driveSysIdQuasistatic(Direction.kForward));

AutoBuilder.configureHolonomic(
this::getPose,
this::resetOdometry,
this::getChassisSpeed,
this::drive,
new HolonomicPathFollowerConfig(
new PIDConstants(Translation.P, Translation.I, Translation.D),
new PIDConstants(Rotation.P, Rotation.I, Rotation.D),
4.5,
TRACK_WIDTH.divide(2).in(Meters),
new ReplanningConfig()
),
() -> {
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this
);
}

/**
Expand Down Expand Up @@ -234,6 +264,10 @@ private SwerveModulePosition[] getModulePositions() {
return modules.stream().map(SwerveModule::position).toArray(SwerveModulePosition[]::new);
}

public ChassisSpeeds getChassisSpeed() {
return kinematics.toChassisSpeeds(getModuleStates());
}

/** Updates pose estimation based on provided {@link EstimatedRobotPose} */
public void updateEstimates(EstimatedRobotPose... poses) {
for (int i = 0; i < poses.length; i++) {
Expand Down

0 comments on commit 6fd7b56

Please sign in to comment.