Skip to content
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

Jump to bottom

135 fix pids for auton #136

Merged
merged 5 commits into from
Mar 21, 2024
Merged

135 fix pids for auton #136

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
4e3ef9b
working on tuning
Griffin9881 Mar 20, 2024
c8ed964
finished with straight line pid tuning for auto
Griffin9881 Mar 20, 2024
0093c7f
fixed theta PIDs!!
Griffin9881 Mar 21, 2024
b42c1fe
got rid of change on robot
Griffin9881 Mar 21, 2024
0536645
got rid of paths used for practice
Griffin9881 Mar 21, 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
24 changes: 12 additions & 12 deletions .pathplanner/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
{
"robotWidth": 0.8202676,
"robotLength": 0.8202676,
"holonomicMode": true,
"pathFolders": [
"New Folder"
],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"maxModuleSpeed": 4.5
"robotWidth": 0.82,
"robotLength": 0.82,
"holonomicMode": true,
"pathFolders": [
"New Folder"
],
"autoFolders": [],
"defaultMaxVel": 7.5,
"defaultMaxAccel": 1.5,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"maxModuleSpeed": 6.0
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/From amp to midline acquire.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -70,5 +70,5 @@
"rotation": -90.0,
"velocity": 0
},
"useDefaultConstraints": false
"useDefaultConstraints": true
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/From amp to midline.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -70,5 +70,5 @@
"rotation": -90.0,
"velocity": 0
},
"useDefaultConstraints": false
"useDefaultConstraints": true
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/From amp to top note.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -80,5 +80,5 @@
"rotation": -90.0,
"velocity": 0
},
"useDefaultConstraints": false
"useDefaultConstraints": true
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/From note 1 to amp.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -48,5 +48,5 @@
"rotation": -45.0,
"velocity": 0
},
"useDefaultConstraints": false
"useDefaultConstraints": true
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -45,5 +45,5 @@
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
"useDefaultConstraints": true
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/From position 1 to amp.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -67,5 +67,5 @@
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
"useDefaultConstraints": true
}
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/Taxi Position 1.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -45,5 +45,5 @@
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
"useDefaultConstraints": true
}
4 changes: 2 additions & 2 deletions src/main/deploy/pathplanner/paths/Taxi Position 2.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand Down
20 changes: 10 additions & 10 deletions src/main/deploy/pathplanner/paths/Taxi Position 3.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,37 +9,37 @@
"prevControl": null,
"nextControl": {
"x": 1.263928801703757,
"y": 1.6839582397113524
"y": 1.6839582397113526
},
"isLocked": false,
"linkedName": null
"linkedName": "startTaxi3"
},
{
"anchor": {
"x": 2.5763977418691195,
"y": 1.6839582397113522
"x": 2.32,
"y": 1.68
},
"prevControl": {
"x": 2.6363977418691196,
"y": 1.6839582397113522
"x": 2.38,
"y": 1.68
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "endTaxi3"
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 7.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotation": 0.0,
"rotateFast": false
},
"reversed": false,
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,8 @@ public void disabledExit() {
@Override
public void autonomousInit() {
new SequentialCommandGroup(
new LiftDownCommand(),
new LiftUpCommand()).schedule();
new LiftDownCommand(),
new LiftUpCommand()).schedule();
driveTrain.zeroHeading();
autonomousCommand = autonSelector.chooseAuton();
// if (DriverStation.isFMSAttached()) {
Expand Down
9 changes: 6 additions & 3 deletions src/main/java/frc/robot/autons/Auton.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.autons.AutonSelector.AutonChoices;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.DriveConstants;
import frc.robot.constants.FieldConstants;
Expand Down Expand Up @@ -62,8 +61,12 @@ public Command followPathCommand(PathPlannerPath path) {
// ChassisSpeeds
new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your
// Constants class
new PIDConstants(AutoConstants.kPXController, 0.0, 0.0), // Translation PID constants
new PIDConstants(AutoConstants.kPThetaController, 0.0, 0.0), // Rotation PID constants
new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, 0.0), // Translation
// PID
// constants
new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, 0.0), // Rotation
// PID
// constants
DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
DriveConstants.kBaseRadius, // Drive base radius in meters. Distance from robot center to
// furthest module.
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/autons/Paths.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
public class Paths {
public static PathPlannerPath pathFromPosition1ToAmp = PathPlannerPath.fromPathFile("From position 1 to amp");

public static PathPlannerPath pathFromTaxi3ToPos1 = PathPlannerPath.fromPathFile("Taxi Position 3 reversed");
public static PathPlannerPath pathFromAmpToNote1 = PathPlannerPath.fromPathFile("From amp to top note");

public static PathPlannerPath pathFromNote1ToAmp = PathPlannerPath.fromPathFile("From note 1 to amp");
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/autons/Trajectories.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,11 @@ public static PathPlannerTrajectory getFromAmpToNote1Trajectory() {
Rotation2d.fromDegrees(DriveTrain.getInstance().getHeading()));
}

public static PathPlannerTrajectory getFromTaxi3ToPos1Trajectory() {
return new PathPlannerTrajectory(Paths.pathFromTaxi3ToPos1, DriveTrain.getInstance().getChassisSpeeds(),
Rotation2d.fromDegrees(DriveTrain.getInstance().getHeading()));
}

public static PathPlannerTrajectory getFromPos1TaxiTrajectory() {
return new PathPlannerTrajectory(Paths.pathFromNote1ToAmp, DriveTrain.getInstance().getChassisSpeeds(),
Rotation2d.fromDegrees(DriveTrain.getInstance().getHeading()));
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/constants/AutoConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,14 @@
import edu.wpi.first.math.trajectory.TrapezoidProfile;

public final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = DriveConstants.kMaxSpeedMetersPerSecond;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 7;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;

public static final double kPXController = 0.3;
public static final double kPYController = 2.0;
public static final double kPThetaController = 2.5;
public static final double kPXController = 1.0;
public static final double kIXController = 0.35;
public static final double kPThetaController = 1.0;
public static final double kIThetaController = 0.05;

// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
Expand Down
6 changes: 0 additions & 6 deletions src/main/java/frc/robot/constants/DriveConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,6 @@ public final class DriveConstants {
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
public static final double kFineAdjustmentPercent = 0.2;

// Rotation PID Constants
public static final double kRotationLockP = 0.01;
public static final double kRotationLockI = 0.000;
public static final double kRotationLockD = 0.000;
public static final double kRotationLockTolerance = 1.0;

// Chassis configuration
public static final double kTrackWidth = Units.inchesToMeters(22.5);
// Distance between centers of right and left wheels on robot
Expand Down
8 changes: 8 additions & 0 deletions src/main/java/frc/robot/subsystems/Dashboard.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,11 @@
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.shuffleboard.WidgetType;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
Expand All @@ -32,6 +34,7 @@ public class Dashboard extends SubsystemBase {
// Tabs
private final ShuffleboardTab driversTab = Shuffleboard.getTab("Drivers");
private final ShuffleboardTab controlsTab = Shuffleboard.getTab("Controls");
private final ShuffleboardTab graphTab = Shuffleboard.getTab("Graph");
private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault();

// Variables
Expand Down Expand Up @@ -100,6 +103,11 @@ private Dashboard() {
.withPosition(2, 1)
.withSize(4, 3)
.withWidget(BuiltInWidgets.kField);
BuiltInAccelerometer acc = new BuiltInAccelerometer();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should revert this file

graphTab.addDouble("acc", acc::getZ)
.withPosition(0, 0)
.withSize(2, 2)
.withWidget(BuiltInWidgets.kGraph);

PathPlannerLogging.setLogTargetPoseCallback((pose) -> {
field.getObject("target pose").setPose(pose);
Expand Down
26 changes: 0 additions & 26 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 @@ -77,32 +77,6 @@ public static DriveTrain getInstance() {
private DriveTrain() {
super();
gyro.reset();

// Auto Setup
AutoBuilder.configureHolonomic(
this::getPose,
this::resetOdometry,
this::getChassisSpeeds,
this::applyChassisSpeeds,
new HolonomicPathFollowerConfig(
new PIDConstants(AutoConstants.kPXController, 0.0, 0.0), // Translation PID constants
new PIDConstants(AutoConstants.kPThetaController, 0.0, 0.0), // Rotation PID constants
DriveConstants.kMaxSpeedMetersPerSecond,
DriveConstants.kBaseRadius,
new ReplanningConfig()),
() -> {
// Boolean supplier that controls when the path will be mirrored for the red
// alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE

var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this);
}

@Override
Expand Down
Loading