From 351753cafcf58a6a55d4339a5e16ff9d504f05e4 Mon Sep 17 00:00:00 2001 From: Griffin Date: Tue, 19 Mar 2024 18:26:44 -0400 Subject: [PATCH 1/7] added moving the lift down at start of auton before moving it up --- src/main/java/frc/robot/Robot.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b81a0f0..975ff1ad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,9 +10,11 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.autons.Auton; import frc.robot.autons.AutonSelector; +import frc.robot.commands.LiftDownCommand; import frc.robot.commands.LiftUpCommand; import frc.robot.constants.SwerveModuleConstants; import frc.robot.subsystems.Dashboard; @@ -70,7 +72,9 @@ public void disabledExit() { @Override public void autonomousInit() { - new LiftUpCommand().schedule(); + new SequentialCommandGroup( + new LiftDownCommand(), + new LiftUpCommand()).schedule(); driveTrain.zeroHeading(); autonomousCommand = autonSelector.chooseAuton(); // if (DriverStation.isFMSAttached()) { From e1a2ce86652562c9bdec6076de7b2ffb19373ada Mon Sep 17 00:00:00 2001 From: Griffin Date: Tue, 19 Mar 2024 18:45:04 -0400 Subject: [PATCH 2/7] fixed method name and added if >and = --- src/main/java/frc/robot/commands/LiftUpCommand.java | 2 +- src/main/java/frc/robot/subsystems/Lift.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java index 04b71c21..7c3c43ce 100644 --- a/src/main/java/frc/robot/commands/LiftUpCommand.java +++ b/src/main/java/frc/robot/commands/LiftUpCommand.java @@ -24,7 +24,7 @@ public void execute() { @Override public boolean isFinished() { - return !lift.isLiftUp(); + return lift.isLiftUp(); } @Override diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index 45570aad..7ae301dd 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -142,8 +142,8 @@ public void runRightUp() { } public boolean isLiftUp() { - return rightLiftEncoder.getPosition() < LiftConstants.maxExtension - && leftLiftEncoder.getPosition() < LiftConstants.maxExtension; + return rightLiftEncoder.getPosition() >= LiftConstants.maxExtension + && leftLiftEncoder.getPosition() >= LiftConstants.maxExtension; } public boolean isRightLiftUp() { From 4e3ef9b6f31a6c32dcb3534a2f3c0888a22b9fe1 Mon Sep 17 00:00:00 2001 From: Griffin Date: Tue, 19 Mar 2024 20:44:50 -0400 Subject: [PATCH 3/7] working on tuning --- .pathplanner/settings.json | 24 ++++++++--------- .../paths/From amp to midline acquire.path | 6 ++--- .../paths/From amp to midline.path | 6 ++--- .../paths/From amp to top note.path | 6 ++--- .../pathplanner/paths/From note 1 to amp.path | 6 ++--- .../From position 1 to amp exagerated.path | 6 ++--- .../paths/From position 1 to amp.path | 6 ++--- .../pathplanner/paths/Taxi Position 1.path | 6 ++--- .../pathplanner/paths/Taxi Position 2.path | 4 +-- .../pathplanner/paths/Taxi Position 3.path | 12 ++++----- src/main/java/frc/robot/autons/Auton.java | 5 ++-- .../frc/robot/constants/AutoConstants.java | 7 +++-- .../frc/robot/constants/DriveConstants.java | 6 ----- .../java/frc/robot/subsystems/Dashboard.java | 8 ++++++ .../java/frc/robot/subsystems/DriveTrain.java | 26 ------------------- 15 files changed, 55 insertions(+), 79 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f3750877..7c3675d9 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -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": 5.0, + "defaultMaxAccel": 3.5, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 6.0 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path index c921852a..34b0ed08 100644 --- a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path +++ b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path @@ -54,8 +54,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -70,5 +70,5 @@ "rotation": -90.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/From amp to midline.path b/src/main/deploy/pathplanner/paths/From amp to midline.path index 2ec2824e..dee8c92b 100644 --- a/src/main/deploy/pathplanner/paths/From amp to midline.path +++ b/src/main/deploy/pathplanner/paths/From amp to midline.path @@ -54,8 +54,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -70,5 +70,5 @@ "rotation": -90.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/From amp to top note.path b/src/main/deploy/pathplanner/paths/From amp to top note.path index d6c6ea83..8c1018db 100644 --- a/src/main/deploy/pathplanner/paths/From amp to top note.path +++ b/src/main/deploy/pathplanner/paths/From amp to top note.path @@ -64,8 +64,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -80,5 +80,5 @@ "rotation": -90.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/From note 1 to amp.path b/src/main/deploy/pathplanner/paths/From note 1 to amp.path index 8b2c7e24..11330887 100644 --- a/src/main/deploy/pathplanner/paths/From note 1 to amp.path +++ b/src/main/deploy/pathplanner/paths/From note 1 to amp.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -48,5 +48,5 @@ "rotation": -45.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path b/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path index 3a0b6aa9..23c7f782 100644 --- a/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path +++ b/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": null, "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp.path b/src/main/deploy/pathplanner/paths/From position 1 to amp.path index 2f8a6d3b..99051af9 100644 --- a/src/main/deploy/pathplanner/paths/From position 1 to amp.path +++ b/src/main/deploy/pathplanner/paths/From position 1 to amp.path @@ -54,8 +54,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -67,5 +67,5 @@ "reversed": false, "folder": null, "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 1.path b/src/main/deploy/pathplanner/paths/Taxi Position 1.path index de4d5e76..2ed360dc 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 1.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": null, "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 2.path b/src/main/deploy/pathplanner/paths/Taxi Position 2.path index 7b31d439..84b87d5c 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 2.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 3.path b/src/main/deploy/pathplanner/paths/Taxi Position 3.path index 149cdb80..c6a47e23 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 3.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5763977418691195, - "y": 1.6839582397113522 + "x": 3.32, + "y": 1.68 }, "prevControl": { - "x": 2.6363977418691196, - "y": 1.6839582397113522 + "x": 3.38, + "y": 1.68 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/frc/robot/autons/Auton.java b/src/main/java/frc/robot/autons/Auton.java index 6a96d3fc..2bba5964 100644 --- a/src/main/java/frc/robot/autons/Auton.java +++ b/src/main/java/frc/robot/autons/Auton.java @@ -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; @@ -62,7 +61,9 @@ 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.kPXController, AutoConstants.kIXController, 0.0), // Translation + // PID + // constants new PIDConstants(AutoConstants.kPThetaController, 0.0, 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 diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 0af56584..4c71541a 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -3,13 +3,12 @@ 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 kPXController = 0.7; + public static final double kIXController = 0.05; public static final double kPThetaController = 2.5; // Constraint for the motion profiled robot angle controller diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index c836b527..67bcdfac 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -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 diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index 7cd46bf1..fb4d9f0d 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -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; @@ -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 @@ -100,6 +103,11 @@ private Dashboard() { .withPosition(2, 1) .withSize(4, 3) .withWidget(BuiltInWidgets.kField); + BuiltInAccelerometer acc = new BuiltInAccelerometer(); + graphTab.addDouble("acc", acc::getZ) + .withPosition(0, 0) + .withSize(2, 2) + .withWidget(BuiltInWidgets.kGraph); PathPlannerLogging.setLogTargetPoseCallback((pose) -> { field.getObject("target pose").setPose(pose); diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 8dccb730..8a985a31 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -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 From c8ed9647fb8e1700a596d4aae49734b83f2389a9 Mon Sep 17 00:00:00 2001 From: Griffin Date: Wed, 20 Mar 2024 17:34:00 -0400 Subject: [PATCH 4/7] finished with straight line pid tuning for auto --- .pathplanner/settings.json | 4 ++-- .../pathplanner/paths/From amp to midline acquire.path | 4 ++-- src/main/deploy/pathplanner/paths/From amp to midline.path | 4 ++-- src/main/deploy/pathplanner/paths/From amp to top note.path | 4 ++-- src/main/deploy/pathplanner/paths/From note 1 to amp.path | 4 ++-- .../paths/From position 1 to amp exagerated.path | 4 ++-- .../deploy/pathplanner/paths/From position 1 to amp.path | 4 ++-- src/main/deploy/pathplanner/paths/Taxi Position 1.path | 4 ++-- src/main/deploy/pathplanner/paths/Taxi Position 2.path | 4 ++-- src/main/deploy/pathplanner/paths/Taxi Position 3.path | 4 ++-- src/main/java/frc/robot/Robot.java | 6 +++--- src/main/java/frc/robot/constants/AutoConstants.java | 4 ++-- 12 files changed, 25 insertions(+), 25 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 7c3675d9..abe4b3d1 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,8 +6,8 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 5.0, - "defaultMaxAccel": 3.5, + "defaultMaxVel": 7.5, + "defaultMaxAccel": 1.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 6.0 diff --git a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path index 34b0ed08..0381c590 100644 --- a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path +++ b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path @@ -54,8 +54,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/From amp to midline.path b/src/main/deploy/pathplanner/paths/From amp to midline.path index dee8c92b..72a05db8 100644 --- a/src/main/deploy/pathplanner/paths/From amp to midline.path +++ b/src/main/deploy/pathplanner/paths/From amp to midline.path @@ -54,8 +54,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/From amp to top note.path b/src/main/deploy/pathplanner/paths/From amp to top note.path index 8c1018db..a14c91e5 100644 --- a/src/main/deploy/pathplanner/paths/From amp to top note.path +++ b/src/main/deploy/pathplanner/paths/From amp to top note.path @@ -64,8 +64,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/From note 1 to amp.path b/src/main/deploy/pathplanner/paths/From note 1 to amp.path index 11330887..c6244449 100644 --- a/src/main/deploy/pathplanner/paths/From note 1 to amp.path +++ b/src/main/deploy/pathplanner/paths/From note 1 to amp.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path b/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path index 23c7f782..7ff71f14 100644 --- a/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path +++ b/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp.path b/src/main/deploy/pathplanner/paths/From position 1 to amp.path index 99051af9..8be51e6d 100644 --- a/src/main/deploy/pathplanner/paths/From position 1 to amp.path +++ b/src/main/deploy/pathplanner/paths/From position 1 to amp.path @@ -54,8 +54,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 1.path b/src/main/deploy/pathplanner/paths/Taxi Position 1.path index 2ed360dc..666bc413 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 1.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 2.path b/src/main/deploy/pathplanner/paths/Taxi Position 2.path index 84b87d5c..21494260 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 2.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 3.path b/src/main/deploy/pathplanner/paths/Taxi Position 3.path index c6a47e23..83903976 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 3.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 975ff1ad..2e974bad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,9 +72,9 @@ public void disabledExit() { @Override public void autonomousInit() { - new SequentialCommandGroup( - new LiftDownCommand(), - new LiftUpCommand()).schedule(); + // new SequentialCommandGroup( + // new LiftDownCommand(), + // new LiftUpCommand()).schedule(); driveTrain.zeroHeading(); autonomousCommand = autonSelector.chooseAuton(); // if (DriverStation.isFMSAttached()) { diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 4c71541a..91e9df88 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -7,8 +7,8 @@ public final class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 0.7; - public static final double kIXController = 0.05; + public static final double kPXController = 1.0; + public static final double kIXController = 0.35; public static final double kPThetaController = 2.5; // Constraint for the motion profiled robot angle controller From 0093c7f1a589a77185245215eb26ef1e9849023c Mon Sep 17 00:00:00 2001 From: Griffin Date: Wed, 20 Mar 2024 20:01:37 -0400 Subject: [PATCH 5/7] fixed theta PIDs!! --- .../paths/From amp to midline acquire.path | 2 +- .../paths/From amp to midline.path | 2 +- .../paths/From amp to top note.path | 2 +- .../pathplanner/paths/From note 1 to amp.path | 2 +- .../From position 1 to amp exagerated.path | 2 +- .../paths/From position 1 to amp.path | 2 +- .../pathplanner/paths/Taxi Position 1.path | 2 +- .../pathplanner/paths/Taxi Position 2.path | 2 +- .../paths/Taxi Position 3 reversed.path | 52 +++++++++++++++++++ .../pathplanner/paths/Taxi Position 3.path | 10 ++-- src/main/java/frc/robot/autons/Auton.java | 4 +- src/main/java/frc/robot/autons/Paths.java | 1 + src/main/java/frc/robot/autons/TaxiPos3.java | 7 +++ .../java/frc/robot/autons/Trajectories.java | 5 ++ .../frc/robot/constants/AutoConstants.java | 3 +- 15 files changed, 83 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path diff --git a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path index 0381c590..1eac50ee 100644 --- a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path +++ b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path @@ -54,7 +54,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/From amp to midline.path b/src/main/deploy/pathplanner/paths/From amp to midline.path index 72a05db8..875d816f 100644 --- a/src/main/deploy/pathplanner/paths/From amp to midline.path +++ b/src/main/deploy/pathplanner/paths/From amp to midline.path @@ -54,7 +54,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/From amp to top note.path b/src/main/deploy/pathplanner/paths/From amp to top note.path index a14c91e5..e5817e4b 100644 --- a/src/main/deploy/pathplanner/paths/From amp to top note.path +++ b/src/main/deploy/pathplanner/paths/From amp to top note.path @@ -64,7 +64,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/From note 1 to amp.path b/src/main/deploy/pathplanner/paths/From note 1 to amp.path index c6244449..e7ead16c 100644 --- a/src/main/deploy/pathplanner/paths/From note 1 to amp.path +++ b/src/main/deploy/pathplanner/paths/From note 1 to amp.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path b/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path index 7ff71f14..60453529 100644 --- a/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path +++ b/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp.path b/src/main/deploy/pathplanner/paths/From position 1 to amp.path index 8be51e6d..cc018a29 100644 --- a/src/main/deploy/pathplanner/paths/From position 1 to amp.path +++ b/src/main/deploy/pathplanner/paths/From position 1 to amp.path @@ -54,7 +54,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 1.path b/src/main/deploy/pathplanner/paths/Taxi Position 1.path index 666bc413..03986871 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 1.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 2.path b/src/main/deploy/pathplanner/paths/Taxi Position 2.path index 21494260..e953699c 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 2.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path b/src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path new file mode 100644 index 00000000..b2b7c3c6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.32, + "y": 1.68 + }, + "prevControl": null, + "nextControl": { + "x": 3.26, + "y": 1.6800000000000002 + }, + "isLocked": false, + "linkedName": "endTaxi3" + }, + { + "anchor": { + "x": 1.3239288017037572, + "y": 1.6839582397113522 + }, + "prevControl": { + "x": 1.3839288017037572, + "y": 1.6839582397113522 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "startTaxi3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 7.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 3.path b/src/main/deploy/pathplanner/paths/Taxi Position 3.path index 83903976..6bc96443 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 3.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 3.path @@ -9,10 +9,10 @@ "prevControl": null, "nextControl": { "x": 1.263928801703757, - "y": 1.6839582397113524 + "y": 1.6839582397113526 }, "isLocked": false, - "linkedName": null + "linkedName": "startTaxi3" }, { "anchor": { @@ -25,21 +25,21 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "endTaxi3" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 7.5, "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 180.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/autons/Auton.java b/src/main/java/frc/robot/autons/Auton.java index 2bba5964..6c2ae1f5 100644 --- a/src/main/java/frc/robot/autons/Auton.java +++ b/src/main/java/frc/robot/autons/Auton.java @@ -64,7 +64,9 @@ public Command followPathCommand(PathPlannerPath path) { new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, 0.0), // Translation // PID // constants - new PIDConstants(AutoConstants.kPThetaController, 0.0, 0.0), // Rotation 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. diff --git a/src/main/java/frc/robot/autons/Paths.java b/src/main/java/frc/robot/autons/Paths.java index ce7e25a9..3e9a97b5 100644 --- a/src/main/java/frc/robot/autons/Paths.java +++ b/src/main/java/frc/robot/autons/Paths.java @@ -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"); diff --git a/src/main/java/frc/robot/autons/TaxiPos3.java b/src/main/java/frc/robot/autons/TaxiPos3.java index e1a78b9e..829d622f 100644 --- a/src/main/java/frc/robot/autons/TaxiPos3.java +++ b/src/main/java/frc/robot/autons/TaxiPos3.java @@ -2,7 +2,9 @@ import java.util.Optional; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.Dashboard; public class TaxiPos3 extends Auton { @@ -10,9 +12,14 @@ public TaxiPos3(Optional alliance) { super(alliance); Dashboard.getInstance().setTrajectory(Trajectories.getFromPos3TaxiTrajectory()); + Dashboard.getInstance().setTrajectory(Trajectories.getFromTaxi3ToPos1Trajectory()); this.setInitialPose(Trajectories.getFromPos3TaxiTrajectory()); addCommands( + followPathCommand(Paths.taxiPath3), + new WaitCommand(0.5), + followPathCommand(Paths.pathFromTaxi3ToPos1), + new WaitCommand(0.5), followPathCommand(Paths.taxiPath3)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/autons/Trajectories.java b/src/main/java/frc/robot/autons/Trajectories.java index 64b09477..49032ebb 100644 --- a/src/main/java/frc/robot/autons/Trajectories.java +++ b/src/main/java/frc/robot/autons/Trajectories.java @@ -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())); diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 91e9df88..2ed2e43a 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -9,7 +9,8 @@ public final class AutoConstants { public static final double kPXController = 1.0; public static final double kIXController = 0.35; - public static final double kPThetaController = 2.5; + 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( From b42c1fed82715106c6b6c1306c3f297d5ee6c5c7 Mon Sep 17 00:00:00 2001 From: Griffin Date: Wed, 20 Mar 2024 20:04:15 -0400 Subject: [PATCH 6/7] got rid of change on robot --- src/main/java/frc/robot/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2e974bad..f3d72887 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,9 +72,9 @@ public void disabledExit() { @Override public void autonomousInit() { - // new SequentialCommandGroup( - // new LiftDownCommand(), - // new LiftUpCommand()).schedule(); + new SequentialCommandGroup( + new LiftDownCommand(), + new LiftUpCommand()).schedule(); driveTrain.zeroHeading(); autonomousCommand = autonSelector.chooseAuton(); // if (DriverStation.isFMSAttached()) { From 0536645aa91907c814822f4b0c717e69fcafd530 Mon Sep 17 00:00:00 2001 From: Griffin Date: Wed, 20 Mar 2024 20:04:33 -0400 Subject: [PATCH 7/7] got rid of paths used for practice --- .../paths/Taxi Position 3 reversed.path | 52 ------------------- .../pathplanner/paths/Taxi Position 3.path | 6 +-- src/main/java/frc/robot/autons/TaxiPos3.java | 7 --- 3 files changed, 3 insertions(+), 62 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path b/src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path deleted file mode 100644 index b2b7c3c6..00000000 --- a/src/main/deploy/pathplanner/paths/Taxi Position 3 reversed.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 3.32, - "y": 1.68 - }, - "prevControl": null, - "nextControl": { - "x": 3.26, - "y": 1.6800000000000002 - }, - "isLocked": false, - "linkedName": "endTaxi3" - }, - { - "anchor": { - "x": 1.3239288017037572, - "y": 1.6839582397113522 - }, - "prevControl": { - "x": 1.3839288017037572, - "y": 1.6839582397113522 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "startTaxi3" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 7.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 3.path b/src/main/deploy/pathplanner/paths/Taxi Position 3.path index 6bc96443..48a964f8 100644 --- a/src/main/deploy/pathplanner/paths/Taxi Position 3.path +++ b/src/main/deploy/pathplanner/paths/Taxi Position 3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.32, + "x": 2.32, "y": 1.68 }, "prevControl": { - "x": 3.38, + "x": 2.38, "y": 1.68 }, "nextControl": null, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/autons/TaxiPos3.java b/src/main/java/frc/robot/autons/TaxiPos3.java index 829d622f..e1a78b9e 100644 --- a/src/main/java/frc/robot/autons/TaxiPos3.java +++ b/src/main/java/frc/robot/autons/TaxiPos3.java @@ -2,9 +2,7 @@ import java.util.Optional; -import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.Dashboard; public class TaxiPos3 extends Auton { @@ -12,14 +10,9 @@ public TaxiPos3(Optional alliance) { super(alliance); Dashboard.getInstance().setTrajectory(Trajectories.getFromPos3TaxiTrajectory()); - Dashboard.getInstance().setTrajectory(Trajectories.getFromTaxi3ToPos1Trajectory()); this.setInitialPose(Trajectories.getFromPos3TaxiTrajectory()); addCommands( - followPathCommand(Paths.taxiPath3), - new WaitCommand(0.5), - followPathCommand(Paths.pathFromTaxi3ToPos1), - new WaitCommand(0.5), followPathCommand(Paths.taxiPath3)); } } \ No newline at end of file