From de6f84ed0745ca4ed1756edce4c0ca8ea1e4e5f2 Mon Sep 17 00:00:00 2001 From: MyaTaheri <91988696+MyaTaheri@users.noreply.github.com> Date: Mon, 4 Mar 2024 18:21:01 -0500 Subject: [PATCH] 3/4/24 --- .pathplanner/settings.json | 12 ++++ .../pathplanner/paths/Left2NoteBlue.path | 70 +++++++++++++++++++ .../deploy/pathplanner/paths/Middle2Note.path | 70 +++++++++++++++++++ .../pathplanner/paths/Right2NoteRed.path | 70 +++++++++++++++++++ .../deploy/pathplanner/paths/SimpleBack.path | 33 ++++++--- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 9 ++- .../commands/autonomous/ChargeCommand.java | 3 +- .../subsystems/SwerveDriveSubsystem.java | 12 +++- 10 files changed, 269 insertions(+), 19 deletions(-) create mode 100644 .pathplanner/settings.json create mode 100644 src/main/deploy/pathplanner/paths/Left2NoteBlue.path create mode 100644 src/main/deploy/pathplanner/paths/Middle2Note.path create mode 100644 src/main/deploy/pathplanner/paths/Right2NoteRed.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..2aaab01 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left2NoteBlue.path b/src/main/deploy/pathplanner/paths/Left2NoteBlue.path new file mode 100644 index 0000000..fd614bf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left2NoteBlue.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9323613212443815, + "y": 6.656317475275852 + }, + "prevControl": null, + "nextControl": { + "x": 1.9323613212443809, + "y": 6.656317475275852 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6751120813030673, + "y": 6.984842182244751 + }, + "prevControl": { + "x": 1.6751120813030673, + "y": 6.984842182244751 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "print", + "waypointRelativePos": 0.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "printLeft2Note" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 90.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle2Note.path b/src/main/deploy/pathplanner/paths/Middle2Note.path new file mode 100644 index 0000000..b180690 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle2Note.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3233687216166703, + "y": 5.562302316231041 + }, + "prevControl": null, + "nextControl": { + "x": 2.323368721616671, + "y": 5.562302316231041 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8958670517985157, + "y": 5.562302316231041 + }, + "prevControl": { + "x": 1.8958670517985157, + "y": 5.562302316231041 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "print", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "print" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right2NoteRed.path b/src/main/deploy/pathplanner/paths/Right2NoteRed.path new file mode 100644 index 0000000..66b4566 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right2NoteRed.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 15.781810114583042, + "y": 6.7769344718348545 + }, + "prevControl": null, + "nextControl": { + "x": 14.197917918710854, + "y": 6.876687903298562 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.91634883530184, + "y": 6.975960595336369 + }, + "prevControl": { + "x": 14.959553395669293, + "y": 6.829575439284806 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "print", + "waypointRelativePos": 0.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "printRight2NoteRed" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SimpleBack.path b/src/main/deploy/pathplanner/paths/SimpleBack.path index bdec935..5e297a5 100644 --- a/src/main/deploy/pathplanner/paths/SimpleBack.path +++ b/src/main/deploy/pathplanner/paths/SimpleBack.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.7609567117282383, - "y": 6.0 + "x": 1.0, + "y": 1.0 }, "prevControl": null, "nextControl": { - "x": 2.7609567117282374, - "y": 6.0 + "x": 1.9999999999999991, + "y": 1.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 2.0, + "y": 1.0 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 1.0, + "y": 1.0 }, "nextControl": null, "isLocked": false, @@ -30,12 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], "globalConstraints": { "maxVelocity": 1.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAngularVelocity": 90.0, + "maxAngularAcceleration": 90.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed58d81..35047d8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -77,7 +77,7 @@ public final static class DriverConstants { } public final static class WheelConstants { - public final static double ROTATIONS_TO_METERS = 0.33/8.33; + public final static double ROTATIONS_TO_METERS = 0.33/8.33 * 1.17; } public enum LimelightDirections { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6500af5..7325230 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -63,9 +63,9 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { System.out.println("STARTING AUTO"); - // m_robotContainer.getOdometry().zeroHeading(); - // m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0))); - // m_robotContainer.getSwerveDriveSubsystem().resetLockRot(); + m_robotContainer.getOdometry().zeroHeading(); + m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0))); + m_robotContainer.getSwerveDriveSubsystem().resetLockRot(); System.out.println("GET AUTO"); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); System.out.println("AUTO GOT!"); @@ -89,6 +89,7 @@ public void teleopInit() { // this line or comment it out. m_robotContainer.getOdometry().zeroHeading(); m_robotContainer.getSwerveDriveSubsystem().resetLockRot(); + m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0))); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 97cc530..97b72ed 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.MaxWheelModule; import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.ShooterSubsystem; + import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; @@ -34,6 +35,7 @@ import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -120,7 +122,7 @@ public class RobotContainer { private final AlignByAprilTag alignLeftOfSpeaker = new AlignByAprilTag(swerveDrive, limelight, odometry, -1.08, -0.96, 1, 0.04, 0.1, -44, -4); private final ChargeCommand chargeCommand; - + SlewRateLimiter limitX = new SlewRateLimiter(6); SlewRateLimiter limitY = new SlewRateLimiter(6); //Limits shooter motor speed @@ -134,7 +136,10 @@ public RobotContainer(RobotBase robot) { CameraServer.startAutomaticCapture(); - NamedCommands.registerCommand("toggleFieldCentric", swerveDrive.toggleFieldCentric()); + NamedCommands.registerCommand("print", new PrintCommand("print!")); + NamedCommands.registerCommand("printRight2NoteRed", new PrintCommand("righ2notered path!")); + NamedCommands.registerCommand("printLeft2Note", new PrintCommand("left2note path!")); + // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java b/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java index 6b6c90b..5217d1c 100644 --- a/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java @@ -20,7 +20,7 @@ /** An example command that uses an example subsystem. */ public class ChargeCommand extends SequentialCommandGroup { - private String TRAJECTORY_NAME = "SimpleBack"; + private String TRAJECTORY_NAME = "Middle2Note"; SwerveDriveSubsystem swerveDrive; FeedSubsystem feedSubsystem; @@ -28,6 +28,7 @@ public ChargeCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsyst this.swerveDrive = swerveDrive; this.feedSubsystem = feedSubsystem; + addRequirements(swerveDrive, feedSubsystem); addCommands( diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 9692926..5c7ee6a 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.HashMap; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathPlannerPath; @@ -19,6 +21,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriverConstants; import frc.robot.common.Odometry; @@ -86,6 +90,7 @@ public SwerveDriveSubsystem(MaxWheelModule backRight, MaxWheelModule backLeft, M var alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { + System.out.println(alliance.get()); return alliance.get() == DriverStation.Alliance.Red; } return false; @@ -108,6 +113,8 @@ public void drive(double x, double y, double rot) { } speeds = new ChassisSpeeds(x, y, rot); + //TODO - check velocity percision + // speeds = new ChassisSpeeds(-0.5, 0, 0); if (fieldCentric) { speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, odometry.getRotation2d()); @@ -199,6 +206,9 @@ public ChassisSpeeds getChassisSpeeds() } public Command followPath(PathPlannerPath path) { - return AutoBuilder.followPath(path); + return new SequentialCommandGroup( + new InstantCommand(() -> odometry.reset(path.getStartingDifferentialPose())), + AutoBuilder.followPath(path)); + } }