diff --git a/.DS_Store b/.DS_Store index 0222d34..7803058 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/bin/main/frc/robot/Constants$DrivetrainConstants.class b/bin/main/frc/robot/Constants$DrivetrainConstants.class index 856e4ce..167dd6e 100644 Binary files a/bin/main/frc/robot/Constants$DrivetrainConstants.class and b/bin/main/frc/robot/Constants$DrivetrainConstants.class differ diff --git a/bin/main/frc/robot/RobotContainer$1.class b/bin/main/frc/robot/RobotContainer$1.class index d6330d6..b7415ff 100644 Binary files a/bin/main/frc/robot/RobotContainer$1.class and b/bin/main/frc/robot/RobotContainer$1.class differ diff --git a/bin/main/frc/robot/RobotContainer$2.class b/bin/main/frc/robot/RobotContainer$2.class index 7c37524..41ede61 100644 Binary files a/bin/main/frc/robot/RobotContainer$2.class and b/bin/main/frc/robot/RobotContainer$2.class differ diff --git a/bin/main/frc/robot/RobotContainer$3.class b/bin/main/frc/robot/RobotContainer$3.class index 7561caa..af5f15e 100644 Binary files a/bin/main/frc/robot/RobotContainer$3.class and b/bin/main/frc/robot/RobotContainer$3.class differ diff --git a/bin/main/frc/robot/RobotContainer$4.class b/bin/main/frc/robot/RobotContainer$4.class index 485ba14..d94b262 100644 Binary files a/bin/main/frc/robot/RobotContainer$4.class and b/bin/main/frc/robot/RobotContainer$4.class differ diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index ab54277..c1b2ac3 100644 Binary files a/bin/main/frc/robot/RobotContainer.class and b/bin/main/frc/robot/RobotContainer.class differ diff --git a/bin/main/frc/robot/commands/Autos.class b/bin/main/frc/robot/commands/Autos.class index 9f2100b..16d4e14 100644 Binary files a/bin/main/frc/robot/commands/Autos.class and b/bin/main/frc/robot/commands/Autos.class differ diff --git a/bin/main/frc/robot/commands/compositions/CancelIntakeNote.class b/bin/main/frc/robot/commands/compositions/CancelIntakeNote.class index c580c40..cf45c2f 100644 Binary files a/bin/main/frc/robot/commands/compositions/CancelIntakeNote.class and b/bin/main/frc/robot/commands/compositions/CancelIntakeNote.class differ diff --git a/bin/main/frc/robot/commands/compositions/IntakeNote.class b/bin/main/frc/robot/commands/compositions/IntakeNote.class index 449bb79..2a633d4 100644 Binary files a/bin/main/frc/robot/commands/compositions/IntakeNote.class and b/bin/main/frc/robot/commands/compositions/IntakeNote.class differ diff --git a/bin/main/frc/robot/commands/drivetrain/AbsoluteFieldDrive.class b/bin/main/frc/robot/commands/drivetrain/AbsoluteFieldDrive.class index 11e7920..a67c87a 100644 Binary files a/bin/main/frc/robot/commands/drivetrain/AbsoluteFieldDrive.class and b/bin/main/frc/robot/commands/drivetrain/AbsoluteFieldDrive.class differ diff --git a/bin/main/frc/robot/commands/drivetrain/DriveToNote.class b/bin/main/frc/robot/commands/drivetrain/DriveToNote.class index be94ce1..9a3576c 100644 Binary files a/bin/main/frc/robot/commands/drivetrain/DriveToNote.class and b/bin/main/frc/robot/commands/drivetrain/DriveToNote.class differ diff --git a/bin/main/frc/robot/lib/util/RebelUtil.class b/bin/main/frc/robot/lib/util/RebelUtil.class index 6e187a9..839ad87 100644 Binary files a/bin/main/frc/robot/lib/util/RebelUtil.class and b/bin/main/frc/robot/lib/util/RebelUtil.class differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/DriveFFController.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/DriveFFController.class index c897b46..709a66d 100644 Binary files a/bin/main/frc/robot/subsystems/drivetrain/swerve/DriveFFController.class and b/bin/main/frc/robot/subsystems/drivetrain/swerve/DriveFFController.class differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.class index 46ec2fd..91045e1 100644 Binary files a/bin/main/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.class and b/bin/main/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.class differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class index 8542e5b..70e314d 100644 Binary files a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class and b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class index 4d57137..d6b0a54 100644 Binary files a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class and b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/vision/NoteDetector.class b/bin/main/frc/robot/subsystems/drivetrain/vision/NoteDetector.class index e1ef0fa..e515b4b 100644 Binary files a/bin/main/frc/robot/subsystems/drivetrain/vision/NoteDetector.class and b/bin/main/frc/robot/subsystems/drivetrain/vision/NoteDetector.class differ diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt index bb82515..2a2cc19 100644 --- a/src/main/deploy/example.txt +++ b/src/main/deploy/example.txt @@ -1,3 +1,3 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path index 74c48a8..8640e6d 100644 --- a/src/main/deploy/pathplanner/New Path.path +++ b/src/main/deploy/pathplanner/New Path.path @@ -1,74 +1,74 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.0, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.0, - "y": 3.0 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.0, - "y": 5.0 - }, - "prevControl": { - "x": 3.0, - "y": 4.0 - }, - "nextControl": { - "x": 3.0, - "y": 4.0 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.0, - "y": 3.0 - }, - "prevControl": { - "x": 4.0, - "y": 3.0 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1PAmp.auto b/src/main/deploy/pathplanner/autos/1PAmp.auto index 568636e..31bb442 100644 --- a/src/main/deploy/pathplanner/autos/1PAmp.auto +++ b/src/main/deploy/pathplanner/autos/1PAmp.auto @@ -1,43 +1,43 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7050561319382253, - "y": 6.72 - }, - "rotation": -120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.5 - } - }, - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7050561319382253, + "y": 6.72 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1PMid.auto b/src/main/deploy/pathplanner/autos/1PMid.auto index ea38d60..bd9eb19 100644 --- a/src/main/deploy/pathplanner/autos/1PMid.auto +++ b/src/main/deploy/pathplanner/autos/1PMid.auto @@ -1,37 +1,37 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.37, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.37, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2.5PAmpA1.auto b/src/main/deploy/pathplanner/autos/2.5PAmpA1.auto index 1ac7939..f24ee08 100644 --- a/src/main/deploy/pathplanner/autos/2.5PAmpA1.auto +++ b/src/main/deploy/pathplanner/autos/2.5PAmpA1.auto @@ -1,119 +1,119 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7, - "y": 6.7 - }, - "rotation": -120.13594030325343 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromAMP" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToAMP" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar1FromAMP" - } - }, - { - "type": "named", - "data": { - "name": "InIntake" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar1ToHome" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7, + "y": 6.7 + }, + "rotation": -120.13594030325343 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromAMP" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToAMP" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar1FromAMP" + } + }, + { + "type": "named", + "data": { + "name": "InIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar1ToHome" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2PAmp.auto b/src/main/deploy/pathplanner/autos/2PAmp.auto index 4b41ea0..f567cde 100644 --- a/src/main/deploy/pathplanner/autos/2PAmp.auto +++ b/src/main/deploy/pathplanner/autos/2PAmp.auto @@ -1,75 +1,75 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7050561319382253, - "y": 6.72 - }, - "rotation": -120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromAMP" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToAMP" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7050561319382253, + "y": 6.72 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromAMP" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToAMP" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2PMid.auto b/src/main/deploy/pathplanner/autos/2PMid.auto index 5e56e43..e756f66 100644 --- a/src/main/deploy/pathplanner/autos/2PMid.auto +++ b/src/main/deploy/pathplanner/autos/2PMid.auto @@ -1,81 +1,81 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.35, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2PMidOut.auto b/src/main/deploy/pathplanner/autos/2PMidOut.auto index cb79106..4316b6a 100644 --- a/src/main/deploy/pathplanner/autos/2PMidOut.auto +++ b/src/main/deploy/pathplanner/autos/2PMidOut.auto @@ -1,126 +1,126 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.35, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MidToFar3" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 6.0 - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "CancelIntakeNote" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MidToFar3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 6.0 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "CancelIntakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2PSource.auto b/src/main/deploy/pathplanner/autos/2PSource.auto index 5e05951..e6f4989 100644 --- a/src/main/deploy/pathplanner/autos/2PSource.auto +++ b/src/main/deploy/pathplanner/autos/2PSource.auto @@ -1,75 +1,75 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.716766968484007, - "y": 4.368142031576504 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToSource" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.716766968484007, + "y": 4.368142031576504 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToSource" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2PSourceTaxi.auto b/src/main/deploy/pathplanner/autos/2PSourceTaxi.auto index d74603c..b54d691 100644 --- a/src/main/deploy/pathplanner/autos/2PSourceTaxi.auto +++ b/src/main/deploy/pathplanner/autos/2PSourceTaxi.auto @@ -1,113 +1,113 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.37 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToSource" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToSource" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 4.37 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToSource" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToSource" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2SourceS4.auto b/src/main/deploy/pathplanner/autos/2SourceS4.auto index 029961d..5226abe 100644 --- a/src/main/deploy/pathplanner/autos/2SourceS4.auto +++ b/src/main/deploy/pathplanner/autos/2SourceS4.auto @@ -1,81 +1,81 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.37 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceToFar4" - } - }, - { - "type": "path", - "data": { - "pathName": "Far4ToSource" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 4.37 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToFar4" + } + }, + { + "type": "path", + "data": { + "pathName": "Far4ToSource" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PAmpA1.auto b/src/main/deploy/pathplanner/autos/3PAmpA1.auto index 912160f..0dab7e3 100644 --- a/src/main/deploy/pathplanner/autos/3PAmpA1.auto +++ b/src/main/deploy/pathplanner/autos/3PAmpA1.auto @@ -1,125 +1,125 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7, - "y": 6.7 - }, - "rotation": -120.13594030325343 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromAMP" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToAMP" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar1FromAMP" - } - }, - { - "type": "named", - "data": { - "name": "InIntake" - } - }, - { - "type": "path", - "data": { - "pathName": "ToAMPFromFar1" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7, + "y": 6.7 + }, + "rotation": -120.13594030325343 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromAMP" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToAMP" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar1FromAMP" + } + }, + { + "type": "named", + "data": { + "name": "InIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "ToAMPFromFar1" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PAmpA2.auto b/src/main/deploy/pathplanner/autos/3PAmpA2.auto index e1f2924..8f34f4b 100644 --- a/src/main/deploy/pathplanner/autos/3PAmpA2.auto +++ b/src/main/deploy/pathplanner/autos/3PAmpA2.auto @@ -1,125 +1,125 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7, - "y": 6.7 - }, - "rotation": -120.13594030325343 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromAMP" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToAMP" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar2FromAMP" - } - }, - { - "type": "named", - "data": { - "name": "InIntake" - } - }, - { - "type": "path", - "data": { - "pathName": null - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7, + "y": 6.7 + }, + "rotation": -120.13594030325343 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromAMP" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToAMP" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar2FromAMP" + } + }, + { + "type": "named", + "data": { + "name": "InIntake" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PAmpAM.auto b/src/main/deploy/pathplanner/autos/3PAmpAM.auto index ee998ea..f4b276f 100644 --- a/src/main/deploy/pathplanner/autos/3PAmpAM.auto +++ b/src/main/deploy/pathplanner/autos/3PAmpAM.auto @@ -1,113 +1,113 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.71, - "y": 6.72 - }, - "rotation": -120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromAMP" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToAMP" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": null - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToAMP" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.71, + "y": 6.72 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromAMP" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToAMP" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToAMP" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PMidAmpVariable.auto b/src/main/deploy/pathplanner/autos/3PMidAmpVariable.auto index 4027f34..72397c3 100644 --- a/src/main/deploy/pathplanner/autos/3PMidAmpVariable.auto +++ b/src/main/deploy/pathplanner/autos/3PMidAmpVariable.auto @@ -1,161 +1,161 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.35, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - }, - { - "type": "named", - "data": { - "name": "VariableShoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "Copy of FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "RollIntakeIn" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MidPointToAmp" - } - }, - { - "type": "path", - "data": { - "pathName": "AmpToMidPoint" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.33 - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + }, + { + "type": "named", + "data": { + "name": "VariableShoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "Copy of FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "RollIntakeIn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MidPointToAmp" + } + }, + { + "type": "path", + "data": { + "pathName": "AmpToMidPoint" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.33 + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PMidM3.auto b/src/main/deploy/pathplanner/autos/3PMidM3.auto index ad410bf..4159fa4 100644 --- a/src/main/deploy/pathplanner/autos/3PMidM3.auto +++ b/src/main/deploy/pathplanner/autos/3PMidM3.auto @@ -1,132 +1,132 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.37, - "y": 5.524499726219249 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.68 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar3FromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar3ToMid" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "CancelIntakeNote" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.37, + "y": 5.524499726219249 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.68 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar3FromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar3ToMid" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "CancelIntakeNote" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PMidMAScoreTurn.auto b/src/main/deploy/pathplanner/autos/3PMidMAScoreTurn.auto index 8ed0ef2..3503dd7 100644 --- a/src/main/deploy/pathplanner/autos/3PMidMAScoreTurn.auto +++ b/src/main/deploy/pathplanner/autos/3PMidMAScoreTurn.auto @@ -1,113 +1,113 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.37, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromMidTurn" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToMidTurn" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.37, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromMidTurn" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToMidTurn" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PMidMS.auto b/src/main/deploy/pathplanner/autos/3PMidMS.auto index 9c6de55..f4888eb 100644 --- a/src/main/deploy/pathplanner/autos/3PMidMS.auto +++ b/src/main/deploy/pathplanner/autos/3PMidMS.auto @@ -1,119 +1,119 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.34, - "y": 5.51843359236209 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToMid" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.34, + "y": 5.51843359236209 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToMid" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PSourceFar4Far5.auto b/src/main/deploy/pathplanner/autos/3PSourceFar4Far5.auto index 9917e44..163e7da 100644 --- a/src/main/deploy/pathplanner/autos/3PSourceFar4Far5.auto +++ b/src/main/deploy/pathplanner/autos/3PSourceFar4Far5.auto @@ -1,145 +1,145 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.37 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.4 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar5FromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar5ToSource" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.7 - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceToFar4" - } - }, - { - "type": "path", - "data": { - "pathName": "Far4ToSource" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.7 - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 4.37 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar5FromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar5ToSource" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.7 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToFar4" + } + }, + { + "type": "path", + "data": { + "pathName": "Far4ToSource" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.7 + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PSourceFar4S.auto b/src/main/deploy/pathplanner/autos/3PSourceFar4S.auto index ddf2488..be3eab0 100644 --- a/src/main/deploy/pathplanner/autos/3PSourceFar4S.auto +++ b/src/main/deploy/pathplanner/autos/3PSourceFar4S.auto @@ -1,75 +1,75 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.368142031576504 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.9 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceToFar4" - } - }, - { - "type": "path", - "data": { - "pathName": "Far4ToSource" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 4.368142031576504 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.9 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToFar4" + } + }, + { + "type": "path", + "data": { + "pathName": "Far4ToSource" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PSourceS4.auto b/src/main/deploy/pathplanner/autos/3PSourceS4.auto index 62c8c3f..1a0abf0 100644 --- a/src/main/deploy/pathplanner/autos/3PSourceS4.auto +++ b/src/main/deploy/pathplanner/autos/3PSourceS4.auto @@ -1,119 +1,119 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.37 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.7 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToSource" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceToFar4" - } - }, - { - "type": "path", - "data": { - "pathName": "Far4ToSource" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 4.37 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToSource" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToFar4" + } + }, + { + "type": "path", + "data": { + "pathName": "Far4ToSource" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3PSourceS5.auto b/src/main/deploy/pathplanner/autos/3PSourceS5.auto index 462d1b2..f900b30 100644 --- a/src/main/deploy/pathplanner/autos/3PSourceS5.auto +++ b/src/main/deploy/pathplanner/autos/3PSourceS5.auto @@ -1,113 +1,113 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.37 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar5FromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar5ToSource" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "CancelIntakeNote" - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToSource" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 4.37 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar5FromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar5ToSource" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "CancelIntakeNote" + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToSource" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4PAMPAM1.auto b/src/main/deploy/pathplanner/autos/4PAMPAM1.auto index ba2fa55..3d0f2c6 100644 --- a/src/main/deploy/pathplanner/autos/4PAMPAM1.auto +++ b/src/main/deploy/pathplanner/autos/4PAMPAM1.auto @@ -1,91 +1,91 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.75, - "y": 6.5832987179074784 - }, - "rotation": -120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.68 - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": null - } - }, - { - "type": "path", - "data": { - "pathName": null - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": null - } - }, - { - "type": "path", - "data": { - "pathName": null - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.75, + "y": 6.5832987179074784 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.68 + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4PAmpA12.auto b/src/main/deploy/pathplanner/autos/4PAmpA12.auto index 566d8c9..45acded 100644 --- a/src/main/deploy/pathplanner/autos/4PAmpA12.auto +++ b/src/main/deploy/pathplanner/autos/4PAmpA12.auto @@ -1,157 +1,157 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7050561319382253, - "y": 6.722020177278586 - }, - "rotation": -120.13594030325343 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.9 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromAMP" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToAMP" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar1FromAMP" - } - }, - { - "type": "path", - "data": { - "pathName": null - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": null - } - }, - { - "type": "path", - "data": { - "pathName": "Copy of FromFar2ToAMP" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7050561319382253, + "y": 6.722020177278586 + }, + "rotation": -120.13594030325343 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.9 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromAMP" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToAMP" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar1FromAMP" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "path", + "data": { + "pathName": "Copy of FromFar2ToAMP" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4PMidMASScoreTrans.auto b/src/main/deploy/pathplanner/autos/4PMidMASScoreTrans.auto index 0e0eae2..bdcb0ff 100644 --- a/src/main/deploy/pathplanner/autos/4PMidMASScoreTrans.auto +++ b/src/main/deploy/pathplanner/autos/4PMidMASScoreTrans.auto @@ -1,163 +1,163 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.36, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.9 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.35 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromMidTrans" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToMidTrans" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromMidTrans" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToMidTrans" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.9 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.35 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromMidTrans" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToMidTrans" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromMidTrans" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToMidTrans" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4PMidMASScoreTurn.auto b/src/main/deploy/pathplanner/autos/4PMidMASScoreTurn.auto index 25e3105..8743c0e 100644 --- a/src/main/deploy/pathplanner/autos/4PMidMASScoreTurn.auto +++ b/src/main/deploy/pathplanner/autos/4PMidMASScoreTurn.auto @@ -1,163 +1,163 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.35, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.6 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.0 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromMidTurn" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToMidTurn" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.0 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromMidTurn" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToMidTurn" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4PMidMS3.auto b/src/main/deploy/pathplanner/autos/4PMidMS3.auto index 6cc63f9..fd2d4b7 100644 --- a/src/main/deploy/pathplanner/autos/4PMidMS3.auto +++ b/src/main/deploy/pathplanner/autos/4PMidMS3.auto @@ -1,91 +1,91 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.37, - "y": 5.524499726219249 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "path", - "data": { - "pathName": "ToFar3FromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar3ToMid" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToMid" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.37, + "y": 5.524499726219249 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "path", + "data": { + "pathName": "ToFar3FromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar3ToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4PMidMSFar1.auto b/src/main/deploy/pathplanner/autos/4PMidMSFar1.auto index 03a77b9..2710f4b 100644 --- a/src/main/deploy/pathplanner/autos/4PMidMSFar1.auto +++ b/src/main/deploy/pathplanner/autos/4PMidMSFar1.auto @@ -1,170 +1,170 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.35, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromMidTurn" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.8 - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar1FromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar1ToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromMidTurn" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar1FromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar1ToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4PSourceSM5.auto b/src/main/deploy/pathplanner/autos/4PSourceSM5.auto index 975863b..645188b 100644 --- a/src/main/deploy/pathplanner/autos/4PSourceSM5.auto +++ b/src/main/deploy/pathplanner/autos/4PSourceSM5.auto @@ -1,91 +1,91 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.716766968484007, - "y": 4.368142031576504 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.68 - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": null - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar5ToSource" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToMid" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.716766968484007, + "y": 4.368142031576504 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.68 + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar5ToSource" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AdaptableTest.auto b/src/main/deploy/pathplanner/autos/AdaptableTest.auto index e14f154..6189f4e 100644 --- a/src/main/deploy/pathplanner/autos/AdaptableTest.auto +++ b/src/main/deploy/pathplanner/autos/AdaptableTest.auto @@ -1,108 +1,108 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7, - "y": 6.7 - }, - "rotation": -120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAmpNoteFromAmpShort" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AmpNotePresent" - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AmpNoteNotPresent" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "CancelIntakeNote" - } - }, - { - "type": "named", - "data": { - "name": "DriveToMidNoteFromAmp" - } - } - ] - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "MidNotePresent" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "ToAmpFromAmpNoteShort" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7, + "y": 6.7 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAmpNoteFromAmpShort" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AmpNotePresent" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AmpNoteNotPresent" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "CancelIntakeNote" + } + }, + { + "type": "named", + "data": { + "name": "DriveToMidNoteFromAmp" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "MidNotePresent" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "ToAmpFromAmpNoteShort" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpMidlineDisrupt.auto b/src/main/deploy/pathplanner/autos/AmpMidlineDisrupt.auto index 3f453b7..16b9e36 100644 --- a/src/main/deploy/pathplanner/autos/AmpMidlineDisrupt.auto +++ b/src/main/deploy/pathplanner/autos/AmpMidlineDisrupt.auto @@ -1,43 +1,43 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.36, - "y": 7.696215392430785 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "RollIntakeEject" - } - }, - { - "type": "path", - "data": { - "pathName": "ToFar1RideWall" - } - }, - { - "type": "named", - "data": { - "name": "StopIntake" - } - }, - { - "type": "path", - "data": { - "pathName": "theFuckMeMovement" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 7.696215392430785 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RollIntakeEject" + } + }, + { + "type": "path", + "data": { + "pathName": "ToFar1RideWall" + } + }, + { + "type": "named", + "data": { + "name": "StopIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "theFuckMeMovement" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTrans.auto b/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTrans.auto index 777ffef..dcebc59 100644 --- a/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTrans.auto +++ b/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTrans.auto @@ -1,97 +1,97 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.36, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromMidTrans" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToMidTrans" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromMidTrans" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToMidTrans" - } - } - ] - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromMidTrans" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToMidTrans" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromMidTrans" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToMidTrans" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTurn.auto b/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTurn.auto index 25dc323..9883d0d 100644 --- a/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTurn.auto +++ b/src/main/deploy/pathplanner/autos/Copy of 4PMidMASScoreTurn.auto @@ -1,97 +1,97 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.34, - "y": 5.51843359236209 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToSourceNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "FromSourceNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToAMPNoteFromMidTurn" - } - }, - { - "type": "path", - "data": { - "pathName": "FromAMPNoteToMidTurn" - } - } - ] - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.34, + "y": 5.51843359236209 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToSourceNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "FromSourceNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToAMPNoteFromMidTurn" + } + }, + { + "type": "path", + "data": { + "pathName": "FromAMPNoteToMidTurn" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MegaChink.auto b/src/main/deploy/pathplanner/autos/MegaChink.auto index ea0a364..fb2cfcb 100644 --- a/src/main/deploy/pathplanner/autos/MegaChink.auto +++ b/src/main/deploy/pathplanner/autos/MegaChink.auto @@ -1,114 +1,114 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.37 - }, - "rotation": 120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar5FromSource" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar5ToShot" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.1 - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "LobNoteAuto" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceShotToFar4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Far4ToFar3" - } - }, - { - "type": "path", - "data": { - "pathName": "FromFar3ToSource" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 4.37 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar5FromSource" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar5ToShot" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.1 + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "LobNoteAuto" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceShotToFar4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Far4ToFar3" + } + }, + { + "type": "path", + "data": { + "pathName": "FromFar3ToSource" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/OUCH4PMidMASScoreTurn.auto b/src/main/deploy/pathplanner/autos/OUCH4PMidMASScoreTurn.auto index 3499c73..7702e66 100644 --- a/src/main/deploy/pathplanner/autos/OUCH4PMidMASScoreTurn.auto +++ b/src/main/deploy/pathplanner/autos/OUCH4PMidMASScoreTurn.auto @@ -1,93 +1,93 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.35, - "y": 5.52 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShooterWindUp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShootNoteAuto" - } - }, - { - "type": "named", - "data": { - "name": "VariableShoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeNote" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToMidNoteFromMid" - } - }, - { - "type": "path", - "data": { - "pathName": "Copy of FromMidNoteToMid" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "RollIntakeIn" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "ShooterStop" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.52 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterWindUp" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShootNoteAuto" + } + }, + { + "type": "named", + "data": { + "name": "VariableShoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMidNoteFromMid" + } + }, + { + "type": "path", + "data": { + "pathName": "Copy of FromMidNoteToMid" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "RollIntakeIn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "ShooterStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceMidlineDisrupt.auto b/src/main/deploy/pathplanner/autos/SourceMidlineDisrupt.auto index e7a0a7a..e14beb3 100644 --- a/src/main/deploy/pathplanner/autos/SourceMidlineDisrupt.auto +++ b/src/main/deploy/pathplanner/autos/SourceMidlineDisrupt.auto @@ -1,31 +1,31 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4311279977766793, - "y": 1.5809629336805058 - }, - "rotation": 0.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ToFar5FromSourceUnaligned" - } - }, - { - "type": "path", - "data": { - "pathName": "theFuckYouMovement" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4311279977766793, + "y": 1.5809629336805058 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToFar5FromSourceUnaligned" + } + }, + { + "type": "path", + "data": { + "pathName": "theFuckYouMovement" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TT.auto b/src/main/deploy/pathplanner/autos/TT.auto index 1c86383..b97ea93 100644 --- a/src/main/deploy/pathplanner/autos/TT.auto +++ b/src/main/deploy/pathplanner/autos/TT.auto @@ -1,25 +1,25 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TurnTesting" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TurnTesting" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AmpToMidPoint.path b/src/main/deploy/pathplanner/paths/AmpToMidPoint.path index 099d420..ec12dca 100644 --- a/src/main/deploy/pathplanner/paths/AmpToMidPoint.path +++ b/src/main/deploy/pathplanner/paths/AmpToMidPoint.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.81, - "y": 6.49 - }, - "prevControl": null, - "nextControl": { - "x": 2.883271729453586, - "y": 5.597779868883561 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.08, - "y": 5.52 - }, - "prevControl": { - "x": 2.578789979263267, - "y": 5.515804013063089 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.35, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 0.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": true - }, - "reversed": false, - "folder": "MidNote", - "previewStartingState": { - "rotation": -120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.81, + "y": 6.49 + }, + "prevControl": null, + "nextControl": { + "x": 2.883271729453586, + "y": 5.597779868883561 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.08, + "y": 5.52 + }, + "prevControl": { + "x": 2.578789979263267, + "y": 5.515804013063089 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.35, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 0.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": "MidNote", + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of FromFar2ToAMP.path b/src/main/deploy/pathplanner/paths/Copy of FromFar2ToAMP.path index de6519b..7216bf9 100644 --- a/src/main/deploy/pathplanner/paths/Copy of FromFar2ToAMP.path +++ b/src/main/deploy/pathplanner/paths/Copy of FromFar2ToAMP.path @@ -1,74 +1,74 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.0, - "y": 5.831996599799192 - }, - "prevControl": null, - "nextControl": { - "x": 6.266796191224337, - "y": 5.878839945982318 - }, - "isLocked": false, - "linkedName": "Far2" - }, - { - "anchor": { - "x": 3.9255361820281403, - "y": 6.2770083885388885 - }, - "prevControl": { - "x": 5.12534602272021, - "y": 6.311753084559703 - }, - "nextControl": { - "x": 2.7181745602640937, - "y": 6.242045004248598 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 0.9233271996700098, - "y": 7.3828668085203795 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0499999999999998, - "rotationDegrees": -120.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -120.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.0, + "y": 5.831996599799192 + }, + "prevControl": null, + "nextControl": { + "x": 6.266796191224337, + "y": 5.878839945982318 + }, + "isLocked": false, + "linkedName": "Far2" + }, + { + "anchor": { + "x": 3.9255361820281403, + "y": 6.2770083885388885 + }, + "prevControl": { + "x": 5.12534602272021, + "y": 6.311753084559703 + }, + "nextControl": { + "x": 2.7181745602640937, + "y": 6.242045004248598 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 0.9233271996700098, + "y": 7.3828668085203795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0499999999999998, + "rotationDegrees": -120.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.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/Copy of FromMidNoteToMid.path b/src/main/deploy/pathplanner/paths/Copy of FromMidNoteToMid.path index 4ffb20f..1bed3dd 100644 --- a/src/main/deploy/pathplanner/paths/Copy of FromMidNoteToMid.path +++ b/src/main/deploy/pathplanner/paths/Copy of FromMidNoteToMid.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.67, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 2.4600025418243923, - "y": 5.51946682020915 - }, - "isLocked": false, - "linkedName": "StartMidMidNote" - }, - { - "anchor": { - "x": 2.08, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 2.49, - "y": 5.51843359236209 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.67, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 2.4600025418243923, + "y": 5.51946682020915 + }, + "isLocked": false, + "linkedName": "StartMidMidNote" + }, + { + "anchor": { + "x": 2.08, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 2.49, + "y": 5.51843359236209 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.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/Far4ToFar3.path b/src/main/deploy/pathplanner/paths/Far4ToFar3.path index 444a1ca..27e405c 100644 --- a/src/main/deploy/pathplanner/paths/Far4ToFar3.path +++ b/src/main/deploy/pathplanner/paths/Far4ToFar3.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.06, - "y": 2.56 - }, - "prevControl": null, - "nextControl": { - "x": 7.549969715319653, - "y": 2.8120787010804436 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.191320998026612, - "y": 3.6606357828100275 - }, - "prevControl": { - "x": 7.767042457157444, - "y": 3.3251597272424966 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.0, - "rotationDegrees": -90.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.25, - "maxWaypointRelativePos": 0.95, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 0.7, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 200.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 135.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.06, + "y": 2.56 + }, + "prevControl": null, + "nextControl": { + "x": 7.549969715319653, + "y": 2.8120787010804436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.191320998026612, + "y": 3.6606357828100275 + }, + "prevControl": { + "x": 7.767042457157444, + "y": 3.3251597272424966 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -90.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.25, + "maxWaypointRelativePos": 0.95, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 0.7, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 200.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 135.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Far4ToSource.path b/src/main/deploy/pathplanner/paths/Far4ToSource.path index 2a40984..e013c99 100644 --- a/src/main/deploy/pathplanner/paths/Far4ToSource.path +++ b/src/main/deploy/pathplanner/paths/Far4ToSource.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.05, - "y": 2.41243232843099 - }, - "prevControl": null, - "nextControl": { - "x": 4.000531447216742, - "y": 0.6241827827011304 - }, - "isLocked": false, - "linkedName": "far4" - }, - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": { - "x": 1.745553616028773, - "y": 3.0098321241435633 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Source" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.15, - "rotationDegrees": 120.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "StartSlowDown", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.15, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 2.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 120.0, - "rotateFast": true - }, - "reversed": false, - "folder": "Far4", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.05, + "y": 2.41243232843099 + }, + "prevControl": null, + "nextControl": { + "x": 4.000531447216742, + "y": 0.6241827827011304 + }, + "isLocked": false, + "linkedName": "far4" + }, + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": { + "x": 1.745553616028773, + "y": 3.0098321241435633 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.15, + "rotationDegrees": 120.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "StartSlowDown", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.15, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 120.0, + "rotateFast": true + }, + "reversed": false, + "folder": "Far4", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromAMPNoteToAMP.path b/src/main/deploy/pathplanner/paths/FromAMPNoteToAMP.path index 6c958c7..40905cb 100644 --- a/src/main/deploy/pathplanner/paths/FromAMPNoteToAMP.path +++ b/src/main/deploy/pathplanner/paths/FromAMPNoteToAMP.path @@ -1,64 +1,64 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.49, - "y": 6.9679477447399965 - }, - "prevControl": null, - "nextControl": { - "x": 1.7473205845127797, - "y": 7.026501927468906 - }, - "isLocked": false, - "linkedName": "StartAMPAMPNote" - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 1.2320437764983934, - "y": 7.0733452736520315 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.3, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -120.0, - "rotateFast": false - }, - "reversed": false, - "folder": "AmpNote", - "previewStartingState": { - "rotation": -160.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.49, + "y": 6.9679477447399965 + }, + "prevControl": null, + "nextControl": { + "x": 1.7473205845127797, + "y": 7.026501927468906 + }, + "isLocked": false, + "linkedName": "StartAMPAMPNote" + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 1.2320437764983934, + "y": 7.0733452736520315 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.3, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": "AmpNote", + "previewStartingState": { + "rotation": -160.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromAMPNoteToMid.path b/src/main/deploy/pathplanner/paths/FromAMPNoteToMid.path index 7e8b467..5b512f5 100644 --- a/src/main/deploy/pathplanner/paths/FromAMPNoteToMid.path +++ b/src/main/deploy/pathplanner/paths/FromAMPNoteToMid.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.6362321955413157, - "y": 6.630585568863446 - }, - "prevControl": null, - "nextControl": { - "x": 2.6362321955413157, - "y": 6.588361547219053 - }, - "isLocked": false, - "linkedName": "StartMidAmpNote" - }, - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 2.183195351754467, - "y": 5.5610809516083055 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Mid" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -135.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6362321955413157, + "y": 6.630585568863446 + }, + "prevControl": null, + "nextControl": { + "x": 2.6362321955413157, + "y": 6.588361547219053 + }, + "isLocked": false, + "linkedName": "StartMidAmpNote" + }, + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 2.183195351754467, + "y": 5.5610809516083055 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -135.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromAMPNoteToMid2.path b/src/main/deploy/pathplanner/paths/FromAMPNoteToMid2.path index b12af1a..32102c7 100644 --- a/src/main/deploy/pathplanner/paths/FromAMPNoteToMid2.path +++ b/src/main/deploy/pathplanner/paths/FromAMPNoteToMid2.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.49, - "y": 6.9679477447399965 - }, - "prevControl": null, - "nextControl": { - "x": 2.5251325372409488, - "y": 6.9913693764261 - }, - "isLocked": false, - "linkedName": "StartAMPAMPNote" - }, - { - "anchor": { - "x": 1.3239288017037585, - "y": 5.545221859674489 - }, - "prevControl": { - "x": 1.3239288017037585, - "y": 7.075221859674489 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -160.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.49, + "y": 6.9679477447399965 + }, + "prevControl": null, + "nextControl": { + "x": 2.5251325372409488, + "y": 6.9913693764261 + }, + "isLocked": false, + "linkedName": "StartAMPAMPNote" + }, + { + "anchor": { + "x": 1.3239288017037585, + "y": 5.545221859674489 + }, + "prevControl": { + "x": 1.3239288017037585, + "y": 7.075221859674489 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -160.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTrans.path b/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTrans.path index da20474..8e69c3c 100644 --- a/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTrans.path +++ b/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTrans.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.4, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.4, - "y": 6.76 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 2.1481083437078214, - "y": 5.559286838042652 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Mid" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "AmpNote", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.4, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.4, + "y": 6.76 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 2.1481083437078214, + "y": 5.559286838042652 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "AmpNote", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTurn.path b/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTurn.path index db2280b..b7e1436 100644 --- a/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTurn.path +++ b/src/main/deploy/pathplanner/paths/FromAMPNoteToMidTurn.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.6362321955413157, - "y": 6.630585568863446 - }, - "prevControl": null, - "nextControl": { - "x": 2.3303569684062455, - "y": 5.61429045935015 - }, - "isLocked": false, - "linkedName": "StartMidAmpNote" - }, - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 2.044221317356934, - "y": 5.51843359236209 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Mid" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.1, - "rotationDegrees": 180.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.2, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -135.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6362321955413157, + "y": 6.630585568863446 + }, + "prevControl": null, + "nextControl": { + "x": 2.3303569684062455, + "y": 5.61429045935015 + }, + "isLocked": false, + "linkedName": "StartMidAmpNote" + }, + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 2.044221317356934, + "y": 5.51843359236209 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1, + "rotationDegrees": 180.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.2, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -135.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromFar1ToAMP.path b/src/main/deploy/pathplanner/paths/FromFar1ToAMP.path index b304df5..976a866 100644 --- a/src/main/deploy/pathplanner/paths/FromFar1ToAMP.path +++ b/src/main/deploy/pathplanner/paths/FromFar1ToAMP.path @@ -1,64 +1,64 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.399075742516676, - "y": 7.424670370025476 - }, - "prevControl": null, - "nextControl": { - "x": 7.6540916443618965, - "y": 7.424670370025476 - }, - "isLocked": false, - "linkedName": "Far1" - }, - { - "anchor": { - "x": 1.9554131415955156, - "y": 7.403731566014806 - }, - "prevControl": { - "x": 2.6954131415955156, - "y": 7.403731566014806 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.3, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.399075742516676, + "y": 7.424670370025476 + }, + "prevControl": null, + "nextControl": { + "x": 7.6540916443618965, + "y": 7.424670370025476 + }, + "isLocked": false, + "linkedName": "Far1" + }, + { + "anchor": { + "x": 1.9554131415955156, + "y": 7.403731566014806 + }, + "prevControl": { + "x": 2.6954131415955156, + "y": 7.403731566014806 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.3, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.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/FromFar1ToHome.path b/src/main/deploy/pathplanner/paths/FromFar1ToHome.path index c503dde..89e5445 100644 --- a/src/main/deploy/pathplanner/paths/FromFar1ToHome.path +++ b/src/main/deploy/pathplanner/paths/FromFar1ToHome.path @@ -1,64 +1,64 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.399075742516676, - "y": 7.424670370025476 - }, - "prevControl": null, - "nextControl": { - "x": 7.6540916443618965, - "y": 7.424670370025476 - }, - "isLocked": false, - "linkedName": "Far1" - }, - { - "anchor": { - "x": 1.9554131415955156, - "y": 7.403731566014806 - }, - "prevControl": { - "x": 2.6954131415955156, - "y": 7.403731566014806 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.3, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 1.55, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "AmpNote", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.399075742516676, + "y": 7.424670370025476 + }, + "prevControl": null, + "nextControl": { + "x": 7.6540916443618965, + "y": 7.424670370025476 + }, + "isLocked": false, + "linkedName": "Far1" + }, + { + "anchor": { + "x": 1.9554131415955156, + "y": 7.403731566014806 + }, + "prevControl": { + "x": 2.6954131415955156, + "y": 7.403731566014806 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.3, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.55, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "AmpNote", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromFar1ToMid.path b/src/main/deploy/pathplanner/paths/FromFar1ToMid.path index 8325ec1..e593fc7 100644 --- a/src/main/deploy/pathplanner/paths/FromFar1ToMid.path +++ b/src/main/deploy/pathplanner/paths/FromFar1ToMid.path @@ -1,74 +1,74 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.838009234970436, - "y": 7.403731566014806 - }, - "prevControl": null, - "nextControl": { - "x": 5.02403235575702, - "y": 6.393778941404038 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.494656220081673, - "y": 5.979367343350071 - }, - "prevControl": { - "x": 4.757624899865187, - "y": 6.3345772845391854 - }, - "nextControl": { - "x": 1.9393363067888176, - "y": 5.541933617736453 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 1.9267045143127497, - "y": 5.555817905664112 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Mid" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.1, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.7, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Far1", - "previewStartingState": { - "rotation": -179.48356992615493, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.838009234970436, + "y": 7.403731566014806 + }, + "prevControl": null, + "nextControl": { + "x": 5.02403235575702, + "y": 6.393778941404038 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.494656220081673, + "y": 5.979367343350071 + }, + "prevControl": { + "x": 4.757624899865187, + "y": 6.3345772845391854 + }, + "nextControl": { + "x": 1.9393363067888176, + "y": 5.541933617736453 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 1.9267045143127497, + "y": 5.555817905664112 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.7, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Far1", + "previewStartingState": { + "rotation": -179.48356992615493, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromFar2ToAMP.path b/src/main/deploy/pathplanner/paths/FromFar2ToAMP.path index 4175929..286ded8 100644 --- a/src/main/deploy/pathplanner/paths/FromFar2ToAMP.path +++ b/src/main/deploy/pathplanner/paths/FromFar2ToAMP.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.0, - "y": 5.831996599799192 - }, - "prevControl": null, - "nextControl": { - "x": 7.393584169589058, - "y": 6.502315312322933 - }, - "isLocked": false, - "linkedName": "Far2" - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 1.343721444669225, - "y": 7.814958248088736 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.15, - "rotationDegrees": -120.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.15, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 0.9, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -120.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Far2", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.0, + "y": 5.831996599799192 + }, + "prevControl": null, + "nextControl": { + "x": 7.393584169589058, + "y": 6.502315312322933 + }, + "isLocked": false, + "linkedName": "Far2" + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 1.343721444669225, + "y": 7.814958248088736 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.15, + "rotationDegrees": -120.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.15, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 0.9, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Far2", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromFar3ToAMP.path b/src/main/deploy/pathplanner/paths/FromFar3ToAMP.path index 740c808..26bccc9 100644 --- a/src/main/deploy/pathplanner/paths/FromFar3ToAMP.path +++ b/src/main/deploy/pathplanner/paths/FromFar3ToAMP.path @@ -1,90 +1,90 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.854853708173674, - "y": 4.110746948487762 - }, - "prevControl": null, - "nextControl": { - "x": 6.596792016436794, - "y": 4.084974857066278 - }, - "isLocked": false, - "linkedName": "Far3" - }, - { - "anchor": { - "x": 6.584545354673532, - "y": 6.040653618883925 - }, - "prevControl": { - "x": 7.087361455997183, - "y": 5.767044757988204 - }, - "nextControl": { - "x": 5.395979779689622, - "y": 6.687415073449778 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.8933818258740813, - "y": 6.1541196778712575 - }, - "prevControl": { - "x": 4.080289155921437, - "y": 5.975229103488579 - }, - "nextControl": { - "x": 1.6229264892987096, - "y": 6.345602600871185 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 1.0483559659986867, - "y": 7.512062523870317 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 2.0999999999999996, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -120.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Far3", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.854853708173674, + "y": 4.110746948487762 + }, + "prevControl": null, + "nextControl": { + "x": 6.596792016436794, + "y": 4.084974857066278 + }, + "isLocked": false, + "linkedName": "Far3" + }, + { + "anchor": { + "x": 6.584545354673532, + "y": 6.040653618883925 + }, + "prevControl": { + "x": 7.087361455997183, + "y": 5.767044757988204 + }, + "nextControl": { + "x": 5.395979779689622, + "y": 6.687415073449778 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8933818258740813, + "y": 6.1541196778712575 + }, + "prevControl": { + "x": 4.080289155921437, + "y": 5.975229103488579 + }, + "nextControl": { + "x": 1.6229264892987096, + "y": 6.345602600871185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 1.0483559659986867, + "y": 7.512062523870317 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.0999999999999996, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Far3", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromFar3ToMid.path b/src/main/deploy/pathplanner/paths/FromFar3ToMid.path index 42997b2..8202883 100644 --- a/src/main/deploy/pathplanner/paths/FromFar3ToMid.path +++ b/src/main/deploy/pathplanner/paths/FromFar3ToMid.path @@ -1,68 +1,68 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.854853708173674, - "y": 4.110746948487762 - }, - "prevControl": null, - "nextControl": { - "x": 6.55340849143237, - "y": 4.223051523026124 - }, - "isLocked": false, - "linkedName": "Far3" - }, - { - "anchor": { - "x": 3.277583478243881, - "y": 4.93347140540435 - }, - "prevControl": { - "x": 4.34321330181122, - "y": 4.597995349836854 - }, - "nextControl": { - "x": 2.6475700214597997, - "y": 5.131808975132672 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 2.1220371969570895, - "y": 5.444986023928492 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Mid" - } - ], - "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": "Far3", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.854853708173674, + "y": 4.110746948487762 + }, + "prevControl": null, + "nextControl": { + "x": 6.55340849143237, + "y": 4.223051523026124 + }, + "isLocked": false, + "linkedName": "Far3" + }, + { + "anchor": { + "x": 3.277583478243881, + "y": 4.93347140540435 + }, + "prevControl": { + "x": 4.34321330181122, + "y": 4.597995349836854 + }, + "nextControl": { + "x": 2.6475700214597997, + "y": 5.131808975132672 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 2.1220371969570895, + "y": 5.444986023928492 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "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": "Far3", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromFar3ToSource.path b/src/main/deploy/pathplanner/paths/FromFar3ToSource.path index 15626c7..1e35d40 100644 --- a/src/main/deploy/pathplanner/paths/FromFar3ToSource.path +++ b/src/main/deploy/pathplanner/paths/FromFar3ToSource.path @@ -1,91 +1,91 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.19, - "y": 3.66 - }, - "prevControl": null, - "nextControl": { - "x": 4.994431527324595, - "y": 4.992673062269202 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.731462847541081, - "y": 2.950215900431802 - }, - "prevControl": { - "x": 4.777358785486803, - "y": 3.4238291553506186 - }, - "nextControl": { - "x": 2.6047259447750917, - "y": 2.4399954161604116 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": { - "x": 1.5373906813225702, - "y": 3.692227636855142 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Source" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.65, - "rotationDegrees": 120.0, - "rotateFast": false - }, - { - "waypointRelativePos": 0.0, - "rotationDegrees": 150.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.15, - "constraints": { - "maxVelocity": 1.7, - "maxAcceleration": 0.7, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Far4", - "previewStartingState": { - "rotation": -90.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.19, + "y": 3.66 + }, + "prevControl": null, + "nextControl": { + "x": 4.994431527324595, + "y": 4.992673062269202 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.731462847541081, + "y": 2.950215900431802 + }, + "prevControl": { + "x": 4.777358785486803, + "y": 3.4238291553506186 + }, + "nextControl": { + "x": 2.6047259447750917, + "y": 2.4399954161604116 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": { + "x": 1.5373906813225702, + "y": 3.692227636855142 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.65, + "rotationDegrees": 120.0, + "rotateFast": false + }, + { + "waypointRelativePos": 0.0, + "rotationDegrees": 150.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.15, + "constraints": { + "maxVelocity": 1.7, + "maxAcceleration": 0.7, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Far4", + "previewStartingState": { + "rotation": -90.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromFar5ToShot.path b/src/main/deploy/pathplanner/paths/FromFar5ToShot.path index ac9723f..d036735 100644 --- a/src/main/deploy/pathplanner/paths/FromFar5ToShot.path +++ b/src/main/deploy/pathplanner/paths/FromFar5ToShot.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.860377261410728, - "y": 0.7846260485673593 - }, - "prevControl": null, - "nextControl": { - "x": 7.684714713224005, - "y": 1.4521437316769061 - }, - "isLocked": false, - "linkedName": "Far5" - }, - { - "anchor": { - "x": 6.724426116469921, - "y": 2.0845289051491083 - }, - "prevControl": { - "x": 7.438787145762593, - "y": 1.6278062798636295 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.65, - "rotationDegrees": 135.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.65, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.8, - "rotation": 135.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.860377261410728, + "y": 0.7846260485673593 + }, + "prevControl": null, + "nextControl": { + "x": 7.684714713224005, + "y": 1.4521437316769061 + }, + "isLocked": false, + "linkedName": "Far5" + }, + { + "anchor": { + "x": 6.724426116469921, + "y": 2.0845289051491083 + }, + "prevControl": { + "x": 7.438787145762593, + "y": 1.6278062798636295 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.65, + "rotationDegrees": 135.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.65, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.8, + "rotation": 135.0, + "rotateFast": true + }, + "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/FromFar5ToSource.path b/src/main/deploy/pathplanner/paths/FromFar5ToSource.path index 80de13d..5e24a4c 100644 --- a/src/main/deploy/pathplanner/paths/FromFar5ToSource.path +++ b/src/main/deploy/pathplanner/paths/FromFar5ToSource.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.860377261410728, - "y": 0.7846260485673593 - }, - "prevControl": null, - "nextControl": { - "x": 4.301123559314367, - "y": 1.5899547221081427 - }, - "isLocked": false, - "linkedName": "Far5" - }, - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": { - "x": 1.0261535488954907, - "y": 3.8106309505848452 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Source" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.65, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.65, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.860377261410728, + "y": 0.7846260485673593 + }, + "prevControl": null, + "nextControl": { + "x": 4.301123559314367, + "y": 1.5899547221081427 + }, + "isLocked": false, + "linkedName": "Far5" + }, + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": { + "x": 1.0261535488954907, + "y": 3.8106309505848452 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.65, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.65, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.0, + "rotateFast": true + }, + "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/FromMidNoteToAMP.path b/src/main/deploy/pathplanner/paths/FromMidNoteToAMP.path index 6f8ff07..8ec0048 100644 --- a/src/main/deploy/pathplanner/paths/FromMidNoteToAMP.path +++ b/src/main/deploy/pathplanner/paths/FromMidNoteToAMP.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.6125756964032125, - "y": 5.826583787447051 - }, - "prevControl": null, - "nextControl": { - "x": 2.379705757984981, - "y": 6.90939356201109 - }, - "isLocked": false, - "linkedName": "StartAmpMidNote" - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 1.7431512106303075, - "y": 6.993376161206399 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8, - "rotationDegrees": -120.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -120.0, - "rotateFast": false - }, - "reversed": false, - "folder": "MidNote", - "previewStartingState": { - "rotation": 139.87578624216107, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6125756964032125, + "y": 5.826583787447051 + }, + "prevControl": null, + "nextControl": { + "x": 2.379705757984981, + "y": 6.90939356201109 + }, + "isLocked": false, + "linkedName": "StartAmpMidNote" + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 1.7431512106303075, + "y": 6.993376161206399 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8, + "rotationDegrees": -120.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": "MidNote", + "previewStartingState": { + "rotation": 139.87578624216107, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromMidNoteToAmpShort.path b/src/main/deploy/pathplanner/paths/FromMidNoteToAmpShort.path index 67a9a35..98b967a 100644 --- a/src/main/deploy/pathplanner/paths/FromMidNoteToAmpShort.path +++ b/src/main/deploy/pathplanner/paths/FromMidNoteToAmpShort.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.95, - "y": 6.439310331313519 - }, - "prevControl": null, - "nextControl": { - "x": 1.4633223710894683, - "y": 7.178799091921299 - }, - "isLocked": false, - "linkedName": "ToMidNoteShort" - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 0.916492009723599, - "y": 6.925834667510113 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -120.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 126.84955805567365, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.95, + "y": 6.439310331313519 + }, + "prevControl": null, + "nextControl": { + "x": 1.4633223710894683, + "y": 7.178799091921299 + }, + "isLocked": false, + "linkedName": "ToMidNoteShort" + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 0.916492009723599, + "y": 6.925834667510113 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 126.84955805567365, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromMidNoteToMid.path b/src/main/deploy/pathplanner/paths/FromMidNoteToMid.path index a0fe025..ba6d203 100644 --- a/src/main/deploy/pathplanner/paths/FromMidNoteToMid.path +++ b/src/main/deploy/pathplanner/paths/FromMidNoteToMid.path @@ -46,7 +46,7 @@ "folder": null, "previewStartingState": { "rotation": 180.0, - "velocity": 0.2 + "velocity": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromMidNoteToSource.path b/src/main/deploy/pathplanner/paths/FromMidNoteToSource.path index c492709..a739338 100644 --- a/src/main/deploy/pathplanner/paths/FromMidNoteToSource.path +++ b/src/main/deploy/pathplanner/paths/FromMidNoteToSource.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.583279561573679, - "y": 5.289895526115651 - }, - "prevControl": null, - "nextControl": { - "x": 2.123151169379263, - "y": 5.022273890701628 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": { - "x": 1.052123558932485, - "y": 4.274099806884532 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Source" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 120.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.0, - "rotateFast": false - }, - "reversed": false, - "folder": "MidNote", - "previewStartingState": { - "rotation": -138.67410902620415, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.583279561573679, + "y": 5.289895526115651 + }, + "prevControl": null, + "nextControl": { + "x": 2.123151169379263, + "y": 5.022273890701628 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": { + "x": 1.052123558932485, + "y": 4.274099806884532 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 120.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.0, + "rotateFast": false + }, + "reversed": false, + "folder": "MidNote", + "previewStartingState": { + "rotation": -138.67410902620415, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromSourceNoteToMid.path b/src/main/deploy/pathplanner/paths/FromSourceNoteToMid.path index 2ab055c..ba09685 100644 --- a/src/main/deploy/pathplanner/paths/FromSourceNoteToMid.path +++ b/src/main/deploy/pathplanner/paths/FromSourceNoteToMid.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.4783611105683763, - "y": 4.380922607999063 - }, - "prevControl": null, - "nextControl": { - "x": 2.08075568899958, - "y": 5.200681166203767 - }, - "isLocked": false, - "linkedName": "SourceNote2" - }, - { - "anchor": { - "x": 1.34, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 1.969826478882628, - "y": 5.574358195791997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Mid" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3, - "rotationDegrees": 180.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.3, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 0.9, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.25, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "SourceNote", - "previewStartingState": { - "rotation": 147.0, - "velocity": 0.0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.4783611105683763, + "y": 4.380922607999063 + }, + "prevControl": null, + "nextControl": { + "x": 2.08075568899958, + "y": 5.200681166203767 + }, + "isLocked": false, + "linkedName": "SourceNote2" + }, + { + "anchor": { + "x": 1.34, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 1.969826478882628, + "y": 5.574358195791997 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3, + "rotationDegrees": 180.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.3, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 0.9, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.25, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceNote", + "previewStartingState": { + "rotation": 147.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromSourceNoteToMidTrans.path b/src/main/deploy/pathplanner/paths/FromSourceNoteToMidTrans.path index 4947140..803dcb7 100644 --- a/src/main/deploy/pathplanner/paths/FromSourceNoteToMidTrans.path +++ b/src/main/deploy/pathplanner/paths/FromSourceNoteToMidTrans.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.4882280533791854, - "y": 4.09 - }, - "prevControl": null, - "nextControl": { - "x": 2.3303569684062464, - "y": 5.570041421621304 - }, - "isLocked": false, - "linkedName": "SourceNoteTrans" - }, - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 1.773054350955278, - "y": 5.51843359236209 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Mid" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 10.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.2, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "SourceNote", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0.0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.4882280533791854, + "y": 4.09 + }, + "prevControl": null, + "nextControl": { + "x": 2.3303569684062464, + "y": 5.570041421621304 + }, + "isLocked": false, + "linkedName": "SourceNoteTrans" + }, + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 1.773054350955278, + "y": 5.51843359236209 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 10.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.2, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceNote", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FromSourceNoteToSource.path b/src/main/deploy/pathplanner/paths/FromSourceNoteToSource.path index 2d45ea4..3a69fd1 100644 --- a/src/main/deploy/pathplanner/paths/FromSourceNoteToSource.path +++ b/src/main/deploy/pathplanner/paths/FromSourceNoteToSource.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.44, - "y": 3.98 - }, - "prevControl": null, - "nextControl": { - "x": 1.44283883432246, - "y": 3.80602187737899 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.716766968484007, - "y": 4.368142031576504 - }, - "prevControl": { - "x": 0.9861162090369815, - "y": 4.016816935203058 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.65, - "rotationDegrees": 120.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.3, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.44, + "y": 3.98 + }, + "prevControl": null, + "nextControl": { + "x": 1.44283883432246, + "y": 3.80602187737899 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.716766968484007, + "y": 4.368142031576504 + }, + "prevControl": { + "x": 0.9861162090369815, + "y": 4.016816935203058 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.65, + "rotationDegrees": 120.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.3, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.0, + "rotateFast": true + }, + "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/MidPointToAmp.path b/src/main/deploy/pathplanner/paths/MidPointToAmp.path index b2fc6f1..361f8cd 100644 --- a/src/main/deploy/pathplanner/paths/MidPointToAmp.path +++ b/src/main/deploy/pathplanner/paths/MidPointToAmp.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.08, - "y": 5.52 - }, - "prevControl": null, - "nextControl": { - "x": 2.2274648828898216, - "y": 5.7047438054335045 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.813006710178897, - "y": 6.487803446362955 - }, - "prevControl": { - "x": 2.531946633080141, - "y": 6.101345840352166 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.65, - "rotationDegrees": -120.0, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -120.0, - "rotateFast": true - }, - "reversed": false, - "folder": "MidNote", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.08, + "y": 5.52 + }, + "prevControl": null, + "nextControl": { + "x": 2.2274648828898216, + "y": 5.7047438054335045 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.813006710178897, + "y": 6.487803446362955 + }, + "prevControl": { + "x": 2.531946633080141, + "y": 6.101345840352166 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.65, + "rotationDegrees": -120.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": true + }, + "reversed": false, + "folder": "MidNote", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidToFar3.path b/src/main/deploy/pathplanner/paths/MidToFar3.path index a005e70..58b1109 100644 --- a/src/main/deploy/pathplanner/paths/MidToFar3.path +++ b/src/main/deploy/pathplanner/paths/MidToFar3.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 3.7498736338414176, - "y": 5.527514849608872 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 7.860377261410728, - "y": 4.098792791023527 - }, - "prevControl": { - "x": 3.8669819992992345, - "y": 4.169057810298217 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 3.7498736338414176, + "y": 5.527514849608872 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 7.860377261410728, + "y": 4.098792791023527 + }, + "prevControl": { + "x": 3.8669819992992345, + "y": 4.169057810298217 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.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/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path index 46d511c..46de602 100644 --- a/src/main/deploy/pathplanner/paths/New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.62, - "y": 6.636106999470185 - }, - "prevControl": { - "x": 1.62, - "y": 6.636106999470185 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -76.61807219669451, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.62, + "y": 6.636106999470185 + }, + "prevControl": { + "x": 1.62, + "y": 6.636106999470185 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -76.61807219669451, + "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/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path index 5ab97a3..adbfd1e 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.0, - "y": 4.098792791023527 - }, - "prevControl": { - "x": 6.01470916979788, - "y": 4.0811865512688845 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 4.098792791023527 + }, + "prevControl": { + "x": 6.01470916979788, + "y": 4.0811865512688845 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.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/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index c039add..5cef946 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.883271729453586, - "y": 6.979658581285779 - }, - "prevControl": null, - "nextControl": { - "x": 3.8832717294535892, - "y": 6.979658581285779 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.883271729453586, - "y": 5.550936522700434 - }, - "prevControl": { - "x": 1.8832717294535861, - "y": 5.550936522700434 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.883271729453586, + "y": 6.979658581285779 + }, + "prevControl": null, + "nextControl": { + "x": 3.8832717294535892, + "y": 6.979658581285779 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.883271729453586, + "y": 5.550936522700434 + }, + "prevControl": { + "x": 1.8832717294535861, + "y": 5.550936522700434 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.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/SourceShotToFar4.path b/src/main/deploy/pathplanner/paths/SourceShotToFar4.path index 2605e03..b9b7332 100644 --- a/src/main/deploy/pathplanner/paths/SourceShotToFar4.path +++ b/src/main/deploy/pathplanner/paths/SourceShotToFar4.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 6.72, - "y": 2.08 - }, - "prevControl": null, - "nextControl": { - "x": 7.865711885269925, - "y": 1.5491100212969666 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0630507414861, - "y": 2.555538187999453 - }, - "prevControl": { - "x": 8.0630507414861, - "y": 2.5456712451886445 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.8, - "rotation": 135.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 135.0, - "velocity": 0.0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.72, + "y": 2.08 + }, + "prevControl": null, + "nextControl": { + "x": 7.865711885269925, + "y": 1.5491100212969666 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0630507414861, + "y": 2.555538187999453 + }, + "prevControl": { + "x": 8.0630507414861, + "y": 2.5456712451886445 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.8, + "rotation": 135.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 135.0, + "velocity": 0.0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceToFar4.path b/src/main/deploy/pathplanner/paths/SourceToFar4.path index 48d9ea6..6c44ad0 100644 --- a/src/main/deploy/pathplanner/paths/SourceToFar4.path +++ b/src/main/deploy/pathplanner/paths/SourceToFar4.path @@ -1,91 +1,91 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": null, - "nextControl": { - "x": 3.1391431649767725, - "y": 2.014411017752135 - }, - "isLocked": false, - "linkedName": "Source" - }, - { - "anchor": { - "x": 7.157727068656738, - "y": 2.1899264340612286 - }, - "prevControl": { - "x": 6.010065087170149, - "y": 1.0891077987577669 - }, - "nextControl": { - "x": 7.394519798922012, - "y": 2.4170541549279196 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.05, - "y": 2.41243232843099 - }, - "prevControl": { - "x": 7.710385740165237, - "y": 2.377299818793733 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "far4" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9, - "rotationDegrees": 180.0, - "rotateFast": true - }, - { - "waypointRelativePos": 0.15, - "rotationDegrees": 120.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "EndSlowDown", - "minWaypointRelativePos": 0.85, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 2.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": true - }, - "reversed": false, - "folder": "Far4", - "previewStartingState": { - "rotation": 120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": null, + "nextControl": { + "x": 3.1391431649767725, + "y": 2.014411017752135 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 7.157727068656738, + "y": 2.1899264340612286 + }, + "prevControl": { + "x": 6.010065087170149, + "y": 1.0891077987577669 + }, + "nextControl": { + "x": 7.394519798922012, + "y": 2.4170541549279196 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.05, + "y": 2.41243232843099 + }, + "prevControl": { + "x": 7.710385740165237, + "y": 2.377299818793733 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "far4" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": 180.0, + "rotateFast": true + }, + { + "waypointRelativePos": 0.15, + "rotationDegrees": 120.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "EndSlowDown", + "minWaypointRelativePos": 0.85, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": "Far4", + "previewStartingState": { + "rotation": 120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TaxiFromAmpNote.path b/src/main/deploy/pathplanner/paths/TaxiFromAmpNote.path index 894fc37..84d2617 100644 --- a/src/main/deploy/pathplanner/paths/TaxiFromAmpNote.path +++ b/src/main/deploy/pathplanner/paths/TaxiFromAmpNote.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.49, - "y": 6.97 - }, - "prevControl": null, - "nextControl": { - "x": 3.49, - "y": 6.97 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.0, - "y": 6.97 - }, - "prevControl": { - "x": 6.0, - "y": 6.97 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.49, + "y": 6.97 + }, + "prevControl": null, + "nextControl": { + "x": 3.49, + "y": 6.97 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 6.97 + }, + "prevControl": { + "x": 6.0, + "y": 6.97 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.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/TaxiOutDestroyMid.path b/src/main/deploy/pathplanner/paths/TaxiOutDestroyMid.path index 3c2206f..7864f32 100644 --- a/src/main/deploy/pathplanner/paths/TaxiOutDestroyMid.path +++ b/src/main/deploy/pathplanner/paths/TaxiOutDestroyMid.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.44283883432246, - "y": 0.7612043754757987 - }, - "prevControl": null, - "nextControl": { - "x": 4.605827173160298, - "y": 0.7612043754757987 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.4882280533791854, - "y": 4.262519294269358 - }, - "prevControl": { - "x": 1.9729512453647988, - "y": 4.332784313544046 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceNote" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "SourceNote", - "previewStartingState": { - "rotation": 0.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.44283883432246, + "y": 0.7612043754757987 + }, + "prevControl": null, + "nextControl": { + "x": 4.605827173160298, + "y": 0.7612043754757987 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4882280533791854, + "y": 4.262519294269358 + }, + "prevControl": { + "x": 1.9729512453647988, + "y": 4.332784313544046 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceNote", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TaxiOutSourceStart.path b/src/main/deploy/pathplanner/paths/TaxiOutSourceStart.path index 80197e9..2cd58d7 100644 --- a/src/main/deploy/pathplanner/paths/TaxiOutSourceStart.path +++ b/src/main/deploy/pathplanner/paths/TaxiOutSourceStart.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": null, - "nextControl": { - "x": 1.4879152120215795, - "y": 3.853012355439832 - }, - "isLocked": false, - "linkedName": "Source" - }, - { - "anchor": { - "x": 2.4882280533791854, - "y": 4.262519294269358 - }, - "prevControl": { - "x": 1.9729512453647988, - "y": 4.332784313544046 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceNote" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 120.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": null, + "nextControl": { + "x": 1.4879152120215795, + "y": 3.853012355439832 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 2.4882280533791854, + "y": 4.262519294269358 + }, + "prevControl": { + "x": 1.9729512453647988, + "y": 4.332784313544046 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 120.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToAMPFromFar1.path b/src/main/deploy/pathplanner/paths/ToAMPFromFar1.path index f9ca88d..0292cd3 100644 --- a/src/main/deploy/pathplanner/paths/ToAMPFromFar1.path +++ b/src/main/deploy/pathplanner/paths/ToAMPFromFar1.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.399075742516676, - "y": 7.424670370025476 - }, - "prevControl": null, - "nextControl": { - "x": 7.309823090165219, - "y": 7.361944255252341 - }, - "isLocked": false, - "linkedName": "Far1" - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 1.0377874601903068, - "y": 7.696215392430785 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.25, - "rotationDegrees": -120.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "Turn Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.25, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -120.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.399075742516676, + "y": 7.424670370025476 + }, + "prevControl": null, + "nextControl": { + "x": 7.309823090165219, + "y": 7.361944255252341 + }, + "isLocked": false, + "linkedName": "Far1" + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 1.0377874601903068, + "y": 7.696215392430785 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25, + "rotationDegrees": -120.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "Turn Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.25, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -120.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/ToAMPNoteFromAMP.path b/src/main/deploy/pathplanner/paths/ToAMPNoteFromAMP.path index 4520bb3..f8df376 100644 --- a/src/main/deploy/pathplanner/paths/ToAMPNoteFromAMP.path +++ b/src/main/deploy/pathplanner/paths/ToAMPNoteFromAMP.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": null, - "nextControl": { - "x": 1.290597959227301, - "y": 7.155321129472502 - }, - "isLocked": false, - "linkedName": "AMP" - }, - { - "anchor": { - "x": 2.49, - "y": 6.9679477447399965 - }, - "prevControl": { - "x": 1.548236363234494, - "y": 7.167031966018283 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartAMPAMPNote" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8, - "rotationDegrees": -160.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -160.0, - "rotateFast": false - }, - "reversed": false, - "folder": "AmpNote", - "previewStartingState": { - "rotation": -120.75, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": null, + "nextControl": { + "x": 1.290597959227301, + "y": 7.155321129472502 + }, + "isLocked": false, + "linkedName": "AMP" + }, + { + "anchor": { + "x": 2.49, + "y": 6.9679477447399965 + }, + "prevControl": { + "x": 1.548236363234494, + "y": 7.167031966018283 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartAMPAMPNote" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8, + "rotationDegrees": -160.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -160.0, + "rotateFast": false + }, + "reversed": false, + "folder": "AmpNote", + "previewStartingState": { + "rotation": -120.75, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTrans.path b/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTrans.path index 72ee0c5..d426a18 100644 --- a/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTrans.path +++ b/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTrans.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.34, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 2.21575404634404, - "y": 5.972526638348569 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 2.4, - "y": 7.0 - }, - "prevControl": { - "x": 1.4405378029976315, - "y": 6.967870690586496 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 1.6, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.2, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "AmpNote", - "previewStartingState": { - "rotation": 178.92402620035944, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.34, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 2.21575404634404, + "y": 5.972526638348569 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 2.4, + "y": 7.0 + }, + "prevControl": { + "x": 1.4405378029976315, + "y": 6.967870690586496 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 1.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.2, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "AmpNote", + "previewStartingState": { + "rotation": 178.92402620035944, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTurn.path b/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTurn.path index 648593f..a0bedd2 100644 --- a/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTurn.path +++ b/src/main/deploy/pathplanner/paths/ToAMPNoteFromMidTurn.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 1.499152141956209, - "y": 5.51843359236209 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 2.6362321955413157, - "y": 6.630585568863446 - }, - "prevControl": { - "x": 2.5796636530463917, - "y": 6.574017026368522 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartMidAmpNote" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6, - "rotationDegrees": -135.0, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -135.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 178.92402620035944, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 1.499152141956209, + "y": 5.51843359236209 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 2.6362321955413157, + "y": 6.630585568863446 + }, + "prevControl": { + "x": 2.5796636530463917, + "y": 6.574017026368522 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartMidAmpNote" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": -135.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -135.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 178.92402620035944, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToAmpFromAmpNoteShort.path b/src/main/deploy/pathplanner/paths/ToAmpFromAmpNoteShort.path index cfb5666..c24c0e3 100644 --- a/src/main/deploy/pathplanner/paths/ToAmpFromAmpNoteShort.path +++ b/src/main/deploy/pathplanner/paths/ToAmpFromAmpNoteShort.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.8435874253480675, - "y": 6.976562884166525 - }, - "prevControl": null, - "nextControl": { - "x": 1.4411064975704242, - "y": 7.053541652574783 - }, - "isLocked": false, - "linkedName": "ShortAmpNote" - }, - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": { - "x": 0.9013146328702464, - "y": 7.185820990095611 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "AMP" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -120.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8435874253480675, + "y": 6.976562884166525 + }, + "prevControl": null, + "nextControl": { + "x": 1.4411064975704242, + "y": 7.053541652574783 + }, + "isLocked": false, + "linkedName": "ShortAmpNote" + }, + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": { + "x": 0.9013146328702464, + "y": 7.185820990095611 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AMP" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": true + }, + "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/ToAmpNoteFromAmpShort.path b/src/main/deploy/pathplanner/paths/ToAmpNoteFromAmpShort.path index 13f07fd..8ef1a63 100644 --- a/src/main/deploy/pathplanner/paths/ToAmpNoteFromAmpShort.path +++ b/src/main/deploy/pathplanner/paths/ToAmpNoteFromAmpShort.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": null, - "nextControl": { - "x": 1.2507051333243862, - "y": 7.025609222251923 - }, - "isLocked": false, - "linkedName": "AMP" - }, - { - "anchor": { - "x": 1.8435874253480675, - "y": 6.976562884166525 - }, - "prevControl": { - "x": 1.5773358757416203, - "y": 6.974070596612809 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ShortAmpNote" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 2.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -122.41273441309177, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": null, + "nextControl": { + "x": 1.2507051333243862, + "y": 7.025609222251923 + }, + "isLocked": false, + "linkedName": "AMP" + }, + { + "anchor": { + "x": 1.8435874253480675, + "y": 6.976562884166525 + }, + "prevControl": { + "x": 1.5773358757416203, + "y": 6.974070596612809 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ShortAmpNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -122.41273441309177, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar1FromAMP.path b/src/main/deploy/pathplanner/paths/ToFar1FromAMP.path index 0b445ea..95e25eb 100644 --- a/src/main/deploy/pathplanner/paths/ToFar1FromAMP.path +++ b/src/main/deploy/pathplanner/paths/ToFar1FromAMP.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.71, - "y": 6.72 - }, - "prevControl": null, - "nextControl": { - "x": 1.3208629785019899, - "y": 7.608214998301123 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.399075742516676, - "y": 7.424670370025476 - }, - "prevControl": { - "x": 7.260323954941184, - "y": 7.424670370025476 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Far1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.25, - "rotationDegrees": 180.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "Turn Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.25, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -120.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.71, + "y": 6.72 + }, + "prevControl": null, + "nextControl": { + "x": 1.3208629785019899, + "y": 7.608214998301123 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.399075742516676, + "y": 7.424670370025476 + }, + "prevControl": { + "x": 7.260323954941184, + "y": 7.424670370025476 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25, + "rotationDegrees": 180.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "Turn Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.25, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar1FromMid.path b/src/main/deploy/pathplanner/paths/ToFar1FromMid.path index f17c9e3..148cbd2 100644 --- a/src/main/deploy/pathplanner/paths/ToFar1FromMid.path +++ b/src/main/deploy/pathplanner/paths/ToFar1FromMid.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 2.142885055000881, - "y": 5.584689630917723 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 7.84, - "y": 7.4 - }, - "prevControl": { - "x": 6.287001035540534, - "y": 6.808190539458002 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.45, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 1.6, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Far1", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 2.142885055000881, + "y": 5.584689630917723 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 7.84, + "y": 7.4 + }, + "prevControl": { + "x": 6.287001035540534, + "y": 6.808190539458002 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 1.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Far1", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar1RideWall.path b/src/main/deploy/pathplanner/paths/ToFar1RideWall.path index a56fbcc..6f453fe 100644 --- a/src/main/deploy/pathplanner/paths/ToFar1RideWall.path +++ b/src/main/deploy/pathplanner/paths/ToFar1RideWall.path @@ -1,74 +1,74 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3633965729469937, - "y": 7.696215392430785 - }, - "prevControl": null, - "nextControl": { - "x": 4.362963415913297, - "y": 7.696215392430785 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.471483558351733, - "y": 7.696215392430785 - }, - "prevControl": { - "x": 3.4453215060276303, - "y": 7.696215392430785 - }, - "nextControl": { - "x": 6.5140360507451645, - "y": 7.696215392430785 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.87, - "y": 7.3114046228092455 - }, - "prevControl": { - "x": 7.854876907446122, - "y": 7.442141615052462 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.45, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 9.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3633965729469937, + "y": 7.696215392430785 + }, + "prevControl": null, + "nextControl": { + "x": 4.362963415913297, + "y": 7.696215392430785 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.471483558351733, + "y": 7.696215392430785 + }, + "prevControl": { + "x": 3.4453215060276303, + "y": 7.696215392430785 + }, + "nextControl": { + "x": 6.5140360507451645, + "y": 7.696215392430785 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.87, + "y": 7.3114046228092455 + }, + "prevControl": { + "x": 7.854876907446122, + "y": 7.442141615052462 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.45, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 9.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar2FromAMP.path b/src/main/deploy/pathplanner/paths/ToFar2FromAMP.path index 45995ef..a7b0ecc 100644 --- a/src/main/deploy/pathplanner/paths/ToFar2FromAMP.path +++ b/src/main/deploy/pathplanner/paths/ToFar2FromAMP.path @@ -1,107 +1,107 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": null, - "nextControl": { - "x": 1.383130458568611, - "y": 7.183134366268733 - }, - "isLocked": false, - "linkedName": "AMP" - }, - { - "anchor": { - "x": 2.8927727086223416, - "y": 7.035130224106603 - }, - "prevControl": { - "x": 2.281156765879405, - "y": 7.146657923332788 - }, - "nextControl": { - "x": 3.5439909341357154, - "y": 6.916381078916005 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.2771340927297254, - "y": 6.452980598268889 - }, - "prevControl": { - "x": 5.783786952189291, - "y": 6.600984740431019 - }, - "nextControl": { - "x": 6.695728371105758, - "y": 6.327402314756079 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0, - "y": 5.831996599799192 - }, - "prevControl": { - "x": 7.903386084101252, - "y": 5.857798755808134 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Far2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 2.3000000000000003, - "rotationDegrees": 180.0, - "rotateFast": true - }, - { - "waypointRelativePos": 1.6500000000000001, - "rotationDegrees": -110.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 1.6500000000000001, - "maxWaypointRelativePos": 2.4000000000000004, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Far2", - "previewStartingState": { - "rotation": -120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": null, + "nextControl": { + "x": 1.383130458568611, + "y": 7.183134366268733 + }, + "isLocked": false, + "linkedName": "AMP" + }, + { + "anchor": { + "x": 2.8927727086223416, + "y": 7.035130224106603 + }, + "prevControl": { + "x": 2.281156765879405, + "y": 7.146657923332788 + }, + "nextControl": { + "x": 3.5439909341357154, + "y": 6.916381078916005 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.2771340927297254, + "y": 6.452980598268889 + }, + "prevControl": { + "x": 5.783786952189291, + "y": 6.600984740431019 + }, + "nextControl": { + "x": 6.695728371105758, + "y": 6.327402314756079 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 5.831996599799192 + }, + "prevControl": { + "x": 7.903386084101252, + "y": 5.857798755808134 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.3000000000000003, + "rotationDegrees": 180.0, + "rotateFast": true + }, + { + "waypointRelativePos": 1.6500000000000001, + "rotationDegrees": -110.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 1.6500000000000001, + "maxWaypointRelativePos": 2.4000000000000004, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Far2", + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar3FromAMP.path b/src/main/deploy/pathplanner/paths/ToFar3FromAMP.path index 7890bec..8be11de 100644 --- a/src/main/deploy/pathplanner/paths/ToFar3FromAMP.path +++ b/src/main/deploy/pathplanner/paths/ToFar3FromAMP.path @@ -1,90 +1,90 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": null, - "nextControl": { - "x": 1.0910078755740336, - "y": 7.655534717817902 - }, - "isLocked": false, - "linkedName": "AMP" - }, - { - "anchor": { - "x": 2.9120223271121386, - "y": 6.247568100964423 - }, - "prevControl": { - "x": 1.7755628179109662, - "y": 6.3208652025433905 - }, - "nextControl": { - "x": 4.489038241880165, - "y": 6.145856869934599 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.991222307694258, - "y": 4.555515416309007 - }, - "prevControl": { - "x": 4.012473034244791, - "y": 4.950851429373671 - }, - "nextControl": { - "x": 5.638514687340896, - "y": 4.294061338540179 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0, - "y": 5.831996599799192 - }, - "prevControl": { - "x": 6.905302798512627, - "y": 5.683451440450731 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Far2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "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": "Far3", - "previewStartingState": { - "rotation": -120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": null, + "nextControl": { + "x": 1.0910078755740336, + "y": 7.655534717817902 + }, + "isLocked": false, + "linkedName": "AMP" + }, + { + "anchor": { + "x": 2.9120223271121386, + "y": 6.247568100964423 + }, + "prevControl": { + "x": 1.7755628179109662, + "y": 6.3208652025433905 + }, + "nextControl": { + "x": 4.489038241880165, + "y": 6.145856869934599 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.991222307694258, + "y": 4.555515416309007 + }, + "prevControl": { + "x": 4.012473034244791, + "y": 4.950851429373671 + }, + "nextControl": { + "x": 5.638514687340896, + "y": 4.294061338540179 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 5.831996599799192 + }, + "prevControl": { + "x": 6.905302798512627, + "y": 5.683451440450731 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "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": "Far3", + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar3FromMid.path b/src/main/deploy/pathplanner/paths/ToFar3FromMid.path index d884ead..54ad251 100644 --- a/src/main/deploy/pathplanner/paths/ToFar3FromMid.path +++ b/src/main/deploy/pathplanner/paths/ToFar3FromMid.path @@ -1,74 +1,74 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 3.019928992740681, - "y": 5.277247996144744 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 5.211504269162385, - "y": 4.6078622926476624 - }, - "prevControl": { - "x": 3.1690471073249853, - "y": 5.042007776323246 - }, - "nextControl": { - "x": 6.538797496863713, - "y": 4.325732331203902 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0, - "y": 5.831996599799192 - }, - "prevControl": { - "x": 7.069818950567345, - "y": 5.978480585294323 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Far2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "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": "Far3", - "previewStartingState": { - "rotation": -179.22257128604332, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 3.019928992740681, + "y": 5.277247996144744 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 5.211504269162385, + "y": 4.6078622926476624 + }, + "prevControl": { + "x": 3.1690471073249853, + "y": 5.042007776323246 + }, + "nextControl": { + "x": 6.538797496863713, + "y": 4.325732331203902 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 5.831996599799192 + }, + "prevControl": { + "x": 7.069818950567345, + "y": 5.978480585294323 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "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": "Far3", + "previewStartingState": { + "rotation": -179.22257128604332, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar5FromSource.path b/src/main/deploy/pathplanner/paths/ToFar5FromSource.path index 398b8df..62cf01c 100644 --- a/src/main/deploy/pathplanner/paths/ToFar5FromSource.path +++ b/src/main/deploy/pathplanner/paths/ToFar5FromSource.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": null, - "nextControl": { - "x": 4.517207410186798, - "y": 0.9788183638827492 - }, - "isLocked": false, - "linkedName": "Source" - }, - { - "anchor": { - "x": 7.860377261410728, - "y": 0.7846260485673593 - }, - "prevControl": { - "x": 6.98755397510795, - "y": 0.8386901389187392 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Far5" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.1, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.1, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.76, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Far5", - "previewStartingState": { - "rotation": 120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": null, + "nextControl": { + "x": 4.517207410186798, + "y": 0.9788183638827492 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 7.860377261410728, + "y": 0.7846260485673593 + }, + "prevControl": { + "x": 6.98755397510795, + "y": 0.8386901389187392 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far5" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.1, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.76, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Far5", + "previewStartingState": { + "rotation": 120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToFar5FromSourceUnaligned.path b/src/main/deploy/pathplanner/paths/ToFar5FromSourceUnaligned.path index b4d7be5..7dd4d7e 100644 --- a/src/main/deploy/pathplanner/paths/ToFar5FromSourceUnaligned.path +++ b/src/main/deploy/pathplanner/paths/ToFar5FromSourceUnaligned.path @@ -1,70 +1,70 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.4311279977766793, - "y": 1.5809629336805058 - }, - "prevControl": null, - "nextControl": { - "x": 2.567139506250485, - "y": 1.5809629336805058 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0, - "y": 1.28 - }, - "prevControl": { - "x": 6.898620882227743, - "y": 1.28 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.2, - "rotationDegrees": -25.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "Accel", - "minWaypointRelativePos": 0.2, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 9.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -25.0, - "rotateFast": true - }, - "reversed": false, - "folder": "AmpNote", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4311279977766793, + "y": 1.5809629336805058 + }, + "prevControl": null, + "nextControl": { + "x": 2.567139506250485, + "y": 1.5809629336805058 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 1.28 + }, + "prevControl": { + "x": 6.898620882227743, + "y": 1.28 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -25.0, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "Accel", + "minWaypointRelativePos": 0.2, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 9.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -25.0, + "rotateFast": true + }, + "reversed": false, + "folder": "AmpNote", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToMidNoteFromAMP.path b/src/main/deploy/pathplanner/paths/ToMidNoteFromAMP.path index 5f6470c..2d01735 100644 --- a/src/main/deploy/pathplanner/paths/ToMidNoteFromAMP.path +++ b/src/main/deploy/pathplanner/paths/ToMidNoteFromAMP.path @@ -1,58 +1,58 @@ -t{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7050561319382253, - "y": 6.722020177278586 - }, - "prevControl": null, - "nextControl": { - "x": 1.7524651313716104, - "y": 6.552416964369942 - }, - "isLocked": false, - "linkedName": "AMP" - }, - { - "anchor": { - "x": 2.613922488900611, - "y": 5.820285763253409 - }, - "prevControl": { - "x": 1.6607516497686534, - "y": 6.34801314596716 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartAmpMidNote" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.1, - "rotationDegrees": -120.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 136.85072099111756, - "rotateFast": false - }, - "reversed": false, - "folder": "MidNote", - "previewStartingState": { - "rotation": -120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +t{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7050561319382253, + "y": 6.722020177278586 + }, + "prevControl": null, + "nextControl": { + "x": 1.7524651313716104, + "y": 6.552416964369942 + }, + "isLocked": false, + "linkedName": "AMP" + }, + { + "anchor": { + "x": 2.613922488900611, + "y": 5.820285763253409 + }, + "prevControl": { + "x": 1.6607516497686534, + "y": 6.34801314596716 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartAmpMidNote" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1, + "rotationDegrees": -120.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 136.85072099111756, + "rotateFast": false + }, + "reversed": false, + "folder": "MidNote", + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToMidNoteFromAmpShort.path b/src/main/deploy/pathplanner/paths/ToMidNoteFromAmpShort.path index e5459f9..a116121 100644 --- a/src/main/deploy/pathplanner/paths/ToMidNoteFromAmpShort.path +++ b/src/main/deploy/pathplanner/paths/ToMidNoteFromAmpShort.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7, - "y": 6.7 - }, - "prevControl": null, - "nextControl": { - "x": 1.1801216688314018, - "y": 6.844847838167292 - }, - "isLocked": false, - "linkedName": "AMP" - }, - { - "anchor": { - "x": 1.9505676929885454, - "y": 6.439310331313519 - }, - "prevControl": { - "x": 1.7473286966270007, - "y": 6.7592636240585335 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ToMidNoteShort" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 216.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 2.0, - "rotation": -56.31637020202837, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -120.0, - "velocity": 0 - }, - "useDefaultConstraints": true +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 6.7 + }, + "prevControl": null, + "nextControl": { + "x": 1.1801216688314018, + "y": 6.844847838167292 + }, + "isLocked": false, + "linkedName": "AMP" + }, + { + "anchor": { + "x": 1.9505676929885454, + "y": 6.439310331313519 + }, + "prevControl": { + "x": 1.7473286966270007, + "y": 6.7592636240585335 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ToMidNoteShort" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 216.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.0, + "rotation": -56.31637020202837, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToMidNoteFromMid.path b/src/main/deploy/pathplanner/paths/ToMidNoteFromMid.path index 5eedd88..db0a580 100644 --- a/src/main/deploy/pathplanner/paths/ToMidNoteFromMid.path +++ b/src/main/deploy/pathplanner/paths/ToMidNoteFromMid.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.34, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 2.1457154792772144, - "y": 5.51843359236209 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 1.9346939692452838, - "y": 5.51843359236209 - }, - "prevControl": { - "x": 1.4774983135223332, - "y": 5.51843359236209 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 1.52, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 2.0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.34, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 2.1457154792772144, + "y": 5.51843359236209 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 1.9346939692452838, + "y": 5.51843359236209 + }, + "prevControl": { + "x": 1.4774983135223332, + "y": 5.51843359236209 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 1.52, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToMidNoteFromSource.path b/src/main/deploy/pathplanner/paths/ToMidNoteFromSource.path index 6017331..e2ed77a 100644 --- a/src/main/deploy/pathplanner/paths/ToMidNoteFromSource.path +++ b/src/main/deploy/pathplanner/paths/ToMidNoteFromSource.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.715, - "y": 4.38 - }, - "prevControl": null, - "nextControl": { - "x": 1.6444467497931763, - "y": 4.457065546934253 - }, - "isLocked": false, - "linkedName": "Source" - }, - { - "anchor": { - "x": 2.6841875081753006, - "y": 5.340141464876368 - }, - "prevControl": { - "x": 1.7011111470080238, - "y": 4.9245156873002145 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.1, - "rotationDegrees": 120.37, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -138.13, - "rotateFast": false - }, - "reversed": false, - "folder": "MidNote", - "previewStartingState": { - "rotation": 120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.715, + "y": 4.38 + }, + "prevControl": null, + "nextControl": { + "x": 1.6444467497931763, + "y": 4.457065546934253 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 2.6841875081753006, + "y": 5.340141464876368 + }, + "prevControl": { + "x": 1.7011111470080238, + "y": 4.9245156873002145 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1, + "rotationDegrees": 120.37, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -138.13, + "rotateFast": false + }, + "reversed": false, + "folder": "MidNote", + "previewStartingState": { + "rotation": 120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToSourceNoteFromMid.path b/src/main/deploy/pathplanner/paths/ToSourceNoteFromMid.path index 701ca4f..39b408e 100644 --- a/src/main/deploy/pathplanner/paths/ToSourceNoteFromMid.path +++ b/src/main/deploy/pathplanner/paths/ToSourceNoteFromMid.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.34, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 1.7381684425565709, - "y": 5.164478916689645 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 2.4783611105683763, - "y": 4.380922607999063 - }, - "prevControl": { - "x": 1.9519364869959832, - "y": 4.91962108910501 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceNote2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.65, - "rotationDegrees": 147.91, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0.5, - "rotation": 147.91, - "rotateFast": false - }, - "reversed": false, - "folder": "SourceNote", - "previewStartingState": { - "rotation": -179.38303924933098, - "velocity": 0.0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.34, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 1.7381684425565709, + "y": 5.164478916689645 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 2.4783611105683763, + "y": 4.380922607999063 + }, + "prevControl": { + "x": 1.9519364869959832, + "y": 4.91962108910501 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceNote2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.65, + "rotationDegrees": 147.91, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0.5, + "rotation": 147.91, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceNote", + "previewStartingState": { + "rotation": -179.38303924933098, + "velocity": 0.0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToSourceNoteFromMidTrans.path b/src/main/deploy/pathplanner/paths/ToSourceNoteFromMidTrans.path index 6652ea0..c830011 100644 --- a/src/main/deploy/pathplanner/paths/ToSourceNoteFromMidTrans.path +++ b/src/main/deploy/pathplanner/paths/ToSourceNoteFromMidTrans.path @@ -1,52 +1,52 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3491521419562087, - "y": 5.51843359236209 - }, - "prevControl": null, - "nextControl": { - "x": 2.250886555981385, - "y": 5.550936522700434 - }, - "isLocked": false, - "linkedName": "Mid" - }, - { - "anchor": { - "x": 2.4882280533791854, - "y": 4.09 - }, - "prevControl": { - "x": 1.325730468864645, - "y": 4.148779391124018 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceNoteTrans" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": "SourceNote", - "previewStartingState": { - "rotation": -179.38303924933098, - "velocity": 0.0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3491521419562087, + "y": 5.51843359236209 + }, + "prevControl": null, + "nextControl": { + "x": 2.250886555981385, + "y": 5.550936522700434 + }, + "isLocked": false, + "linkedName": "Mid" + }, + { + "anchor": { + "x": 2.4882280533791854, + "y": 4.09 + }, + "prevControl": { + "x": 1.325730468864645, + "y": 4.148779391124018 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceNoteTrans" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceNote", + "previewStartingState": { + "rotation": -179.38303924933098, + "velocity": 0.0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToSourceNoteFromSource.path b/src/main/deploy/pathplanner/paths/ToSourceNoteFromSource.path index 6e1a88d..a52bcf1 100644 --- a/src/main/deploy/pathplanner/paths/ToSourceNoteFromSource.path +++ b/src/main/deploy/pathplanner/paths/ToSourceNoteFromSource.path @@ -1,64 +1,64 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.71, - "y": 4.38 - }, - "prevControl": null, - "nextControl": { - "x": 1.244993259217289, - "y": 3.808639924972158 - }, - "isLocked": false, - "linkedName": "Source" - }, - { - "anchor": { - "x": 2.44, - "y": 4.0 - }, - "prevControl": { - "x": 1.738340399757724, - "y": 3.690236611242454 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": true - }, - "reversed": false, - "folder": "SourceNote", - "previewStartingState": { - "rotation": 120.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.71, + "y": 4.38 + }, + "prevControl": null, + "nextControl": { + "x": 1.244993259217289, + "y": 3.808639924972158 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 2.44, + "y": 4.0 + }, + "prevControl": { + "x": 1.738340399757724, + "y": 3.690236611242454 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": "SourceNote", + "previewStartingState": { + "rotation": 120.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TurnTesting.path b/src/main/deploy/pathplanner/paths/TurnTesting.path index 36ad2b2..a2f1adf 100644 --- a/src/main/deploy/pathplanner/paths/TurnTesting.path +++ b/src/main/deploy/pathplanner/paths/TurnTesting.path @@ -1,58 +1,58 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.1, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.729005010078493, - "y": 7.0 - }, - "prevControl": { - "x": 5.629005010078493, - "y": 7.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.75, - "rotationDegrees": 60.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 180.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 60.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.1, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.729005010078493, + "y": 7.0 + }, + "prevControl": { + "x": 5.629005010078493, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 60.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 60.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/theFuckMeMovement.path b/src/main/deploy/pathplanner/paths/theFuckMeMovement.path index 505df1a..dc615d7 100644 --- a/src/main/deploy/pathplanner/paths/theFuckMeMovement.path +++ b/src/main/deploy/pathplanner/paths/theFuckMeMovement.path @@ -1,74 +1,74 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.87, - "y": 7.3114046228092455 - }, - "prevControl": null, - "nextControl": { - "x": 7.765312713702351, - "y": 7.51144604443055 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.865711885269925, - "y": 2.5851390164318797 - }, - "prevControl": { - "x": 8.232602517195753, - "y": 3.0498671502045953 - }, - "nextControl": { - "x": 7.421699458783534, - "y": 2.022723276215783 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.865711885269925, - "y": 0.9570934526484439 - }, - "prevControl": { - "x": 7.880512299486138, - "y": 0.6660186397295862 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.35, - "rotationDegrees": 50.0, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 1.7, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 50.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.87, + "y": 7.3114046228092455 + }, + "prevControl": null, + "nextControl": { + "x": 7.765312713702351, + "y": 7.51144604443055 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.865711885269925, + "y": 2.5851390164318797 + }, + "prevControl": { + "x": 8.232602517195753, + "y": 3.0498671502045953 + }, + "nextControl": { + "x": 7.421699458783534, + "y": 2.022723276215783 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.865711885269925, + "y": 0.9570934526484439 + }, + "prevControl": { + "x": 7.880512299486138, + "y": 0.6660186397295862 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 50.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 1.7, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 50.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/theFuckYouMovement.path b/src/main/deploy/pathplanner/paths/theFuckYouMovement.path index bf849c4..2f327d5 100644 --- a/src/main/deploy/pathplanner/paths/theFuckYouMovement.path +++ b/src/main/deploy/pathplanner/paths/theFuckYouMovement.path @@ -1,100 +1,100 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.0, - "y": 1.28 - }, - "prevControl": null, - "nextControl": { - "x": 7.895312713702351, - "y": 1.4800414216213047 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0, - "y": 2.1904613039995318 - }, - "prevControl": { - "x": 7.53877954448983, - "y": 1.5401408641562089 - }, - "nextControl": { - "x": 8.44786151110764, - "y": 2.821945643891289 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.181454055215804, - "y": 4.01584572399914 - }, - "prevControl": { - "x": 8.181454055215804, - "y": 3.366536061434566 - }, - "nextControl": { - "x": 8.181454055215804, - "y": 4.889724926192899 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.865711885269925, - "y": 5.239346632539419 - }, - "prevControl": { - "x": 8.189419042752274, - "y": 5.039822551114432 - }, - "nextControl": { - "x": 7.5420047277875755, - "y": 5.438870713964406 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0, - "y": 5.870830972431175 - }, - "prevControl": { - "x": 7.613542393989211, - "y": 5.531216712603511 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 9.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 360.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -50.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -25.0, - "velocity": 0 - }, - "useDefaultConstraints": false +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.0, + "y": 1.28 + }, + "prevControl": null, + "nextControl": { + "x": 7.895312713702351, + "y": 1.4800414216213047 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 2.1904613039995318 + }, + "prevControl": { + "x": 7.53877954448983, + "y": 1.5401408641562089 + }, + "nextControl": { + "x": 8.44786151110764, + "y": 2.821945643891289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.181454055215804, + "y": 4.01584572399914 + }, + "prevControl": { + "x": 8.181454055215804, + "y": 3.366536061434566 + }, + "nextControl": { + "x": 8.181454055215804, + "y": 4.889724926192899 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.865711885269925, + "y": 5.239346632539419 + }, + "prevControl": { + "x": 8.189419042752274, + "y": 5.039822551114432 + }, + "nextControl": { + "x": 7.5420047277875755, + "y": 5.438870713964406 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 5.870830972431175 + }, + "prevControl": { + "x": 7.613542393989211, + "y": 5.531216712603511 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 9.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -50.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -25.0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 77c0857..ee1532c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,144 +1,144 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - public static final Mode currentMode = Mode.REAL; // TODO: change this if sim - // public static final boolean isSYSID = true; // TODO: change this if sysid - public static enum Mode { - /** Running on a real robot. */ - REAL, - - /** Running a physics simulator. */ - SIM, - - /** Replaying from a log file. */ - REPLAY_REAL, - - REPLAY_SIM, - } - - public static class OperatorConstants { - public static final int kDRIVER_CONTROLLER_PORT = 0; - - // Joystick Deadband - // yes, this high - public static final double LEFT_X_DEADBAND = 0.09; - public static final double LEFT_Y_DEADBAND = 0.09; - - public static final double RIGHT_X_DEADBAND = 0.09; - - } - - public static final class Auton - { - - public static final PIDConstants TRANSLATION_PID_CONFIG = new PIDConstants(3, 0, 0); - public static final PIDConstants ANGLE_PID_CONFIG = new PIDConstants(6.5, 0, 0.03, Math.PI * 2 * .12); - - public static final double MAX_MODULE_SPEED = 4.5; - public static final double MAX_ACCELERATION = 2; - public static final double MAX_ANGULAR_VELO_RPS = 0.6; // 1 - public static final double MAX_ANGULAR_ACCEL_RPS_SQUARED = 1; // 0.5 - - public static final HolonomicPathFollowerConfig DRIVE_CONTROLLER_CONFIG = new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - Constants.Auton.TRANSLATION_PID_CONFIG, // Translation PID constants - Constants.Auton.ANGLE_PID_CONFIG, // Rotation PID constants - Constants.Auton.MAX_MODULE_SPEED, // Max module speed, in m/s - // I LOVE THE PYTHAGPREAN THEOREM - 0.5384061785684372, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options here - ); - - } - - public static class DrivetrainConstants { - // TODO: CHANGE THEESEANDKJANSDKLJWENF LKJDSBLGKJFNadzs - public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND = 2; - public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED = 2; - public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED = 2; - - public static final double kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND = 2 * Math.PI * 0.6; - public static final double kMAX_DRIVETRAIN_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 2 * Math.PI; - public static final double kMAX_DRIVETRAIN_ANGULAR_DECELERATION_RADIANS_PER_SECOND_SQUARED = 2 * Math.PI; - - public static final double kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND = 4.5; - public static final double kMAX_MODULE_DRIVE_ACCELERATION_METERS_PER_SECOND = 2.5; - public static final double kMAX_MODULE_DRIVE_DECELERATION_METERS_PER_SECOND = 2.5; - - // x, y from center of the robot - public static final Translation2d kFRONT_RIGHT_POSITION_METERS = new Translation2d(0.38, -0.38); - public static final Translation2d kBACK_RIGHT_POSITION_METERS = new Translation2d(-0.38, -0.38); - public static final Translation2d kFRONT_LEFT_POSITION_METERS = new Translation2d(0.38, 0.38); - public static final Translation2d kBACK_LEFT_POSITION_METERS = new Translation2d(-0.38, 0.38); - - public static final double kFRONT_RIGHT_ANGLE_OFFSET_DEG = 293.37890625; - public static final double kBACK_RIGHT_ANGLE_OFFSET_DEG = 182.900390625; - public static final double kFRONT_LEFT_ANGLE_OFFSET_DEG = 225.966796875; - public static final double kBACK_LEFT_ANGLE_OFFSET_DEG = 257.51953125; - - public static final double kDRIVE_MOTOR_TO_OUTPUT_SHAFT_RATIO = 6.12/2; - public static final double kANGLE_MOTOR_TO_OUTPUT_SHAFT_RATIO = 16.8; - - public static final double kDRIVE_WHEEL_RADIUS_METERS = 0.0508; - - public static final double kMAX_ANGLE_VOLTAGE = 12; - public static final double kMAX_DRIVE_VOLTAGE = 12; - } - - public static class FieldConstants { - // far 1, 2, 3, 4, 5 (top to bottom), ampside, mid, source - // THIS IS FROM BLUE ORIGIN - public static final Translation3d[] kNOTE_ARR = new Translation3d[] { - new Translation3d(8.28, 7.44, 0), - new Translation3d(8.28, 5.78, 0), - new Translation3d(8.28, 4.11, 0), - new Translation3d(8.28, 2.44, 0), - new Translation3d(8.28, 0.77, 0), - new Translation3d(2.89, 6.98, 0), - new Translation3d(2.89, 5.54, 0), - new Translation3d(2.89, 4.10, 0), - }; - - public static final double kNOTE_DIAMETER_METERS = 0.36; - } - - public static class VisionConstants { - // public static final Pose3d kNOTE_DETECTOR_CAMERA_POSE = // REAL - // new Pose3d(new Translation3d(-0.15, -0.47, 0.7), new Rotation3d(Math.toRadians(0), Math.toRadians(30), Math.toRadians(5))); - - public static final Pose3d kNOTE_DETECTOR_CAMERA_POSE = // SIM - new Pose3d(new Translation3d(0.076, 0.0, 0.47), new Rotation3d(Math.toRadians(0), Math.toRadians(15.5), Math.toRadians(0))); - public static final double kNOTE_DETECTOR_CAMERA_FOV_X_RAD = Math.toRadians(29.8 * 2); - public static final double kNOTE_DETECTOR_CAMERA_FOV_Y_RAD = Math.toRadians(24.85 * 2); - public static final double kNOTE_DETECTOR_CAMERA_MAX_RANGE_METERS = 2; - public static final double kNOTE_DETECTOR_CAMERA_BLIND_SPOT_DISTANCE_METERS = 0.3; - - } - - public static class IntakeConstants { - public static final double kROLLER_RADIUS_METERS = 0.025; - public static final Translation3d KINTAKE_TRANSLATION3D = new Translation3d(-.38, 0, 0); - } - -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final Mode currentMode = Mode.REAL; // TODO: change this if sim + // public static final boolean isSYSID = true; // TODO: change this if sysid + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY_REAL, + + REPLAY_SIM, + } + + public static class OperatorConstants { + public static final int kDRIVER_CONTROLLER_PORT = 0; + + // Joystick Deadband + // yes, this high + public static final double LEFT_X_DEADBAND = 0.09; + public static final double LEFT_Y_DEADBAND = 0.09; + + public static final double RIGHT_X_DEADBAND = 0.09; + + } + + public static final class Auton + { + + public static final PIDConstants TRANSLATION_PID_CONFIG = new PIDConstants(3, 0, 0); + public static final PIDConstants ANGLE_PID_CONFIG = new PIDConstants(6.5, 0, 0.03, Math.PI * 2 * .12); + + public static final double MAX_MODULE_SPEED = 4.5; + public static final double MAX_ACCELERATION = 2; + public static final double MAX_ANGULAR_VELO_RPS = 0.6; // 1 + public static final double MAX_ANGULAR_ACCEL_RPS_SQUARED = 1; // 0.5 + + public static final HolonomicPathFollowerConfig DRIVE_CONTROLLER_CONFIG = new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + Constants.Auton.TRANSLATION_PID_CONFIG, // Translation PID constants + Constants.Auton.ANGLE_PID_CONFIG, // Rotation PID constants + Constants.Auton.MAX_MODULE_SPEED, // Max module speed, in m/s + // I LOVE THE PYTHAGPREAN THEOREM + 0.5384061785684372, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options here + ); + + } + + public static class DrivetrainConstants { + // TODO: CHANGE THEESEANDKJANSDKLJWENF LKJDSBLGKJFNadzs + public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND = 4; + public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED = 3.5; + public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED = 7; + + public static final double kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND = 2 * Math.PI * 0.6; + public static final double kMAX_DRIVETRAIN_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 12.46; + public static final double kMAX_DRIVETRAIN_ANGULAR_DECELERATION_RADIANS_PER_SECOND_SQUARED = 18.7; + + public static final double kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND = 4.5; + public static final double kMAX_MODULE_DRIVE_ACCELERATION_METERS_PER_SECOND = 3.5; + public static final double kMAX_MODULE_DRIVE_DECELERATION_METERS_PER_SECOND = 7; + + // x, y from center of the robot + public static final Translation2d kFRONT_RIGHT_POSITION_METERS = new Translation2d(0.38, -0.38); + public static final Translation2d kBACK_RIGHT_POSITION_METERS = new Translation2d(-0.38, -0.38); + public static final Translation2d kFRONT_LEFT_POSITION_METERS = new Translation2d(0.38, 0.38); + public static final Translation2d kBACK_LEFT_POSITION_METERS = new Translation2d(-0.38, 0.38); + + public static final double kFRONT_RIGHT_ANGLE_OFFSET_DEG = 293.37890625; + public static final double kBACK_RIGHT_ANGLE_OFFSET_DEG = 182.900390625; + public static final double kFRONT_LEFT_ANGLE_OFFSET_DEG = 225.966796875; + public static final double kBACK_LEFT_ANGLE_OFFSET_DEG = 257.51953125; + + public static final double kDRIVE_MOTOR_TO_OUTPUT_SHAFT_RATIO = 6.12/2; + public static final double kANGLE_MOTOR_TO_OUTPUT_SHAFT_RATIO = 16.8; + + public static final double kDRIVE_WHEEL_RADIUS_METERS = 0.0508; + + public static final double kMAX_ANGLE_VOLTAGE = 12; + public static final double kMAX_DRIVE_VOLTAGE = 12; + } + + public static class FieldConstants { + // far 1, 2, 3, 4, 5 (top to bottom), ampside, mid, source + // THIS IS FROM BLUE ORIGIN + public static final Translation3d[] kNOTE_ARR = new Translation3d[] { + new Translation3d(8.28, 7.44, 0), + new Translation3d(8.28, 5.78, 0), + new Translation3d(8.28, 4.11, 0), + new Translation3d(8.28, 2.44, 0), + new Translation3d(8.28, 0.77, 0), + new Translation3d(2.89, 6.98, 0), + new Translation3d(2.89, 5.54, 0), + new Translation3d(2.89, 4.10, 0), + }; + + public static final double kNOTE_DIAMETER_METERS = 0.36; + } + + public static class VisionConstants { + // public static final Pose3d kNOTE_DETECTOR_CAMERA_POSE = // REAL + // new Pose3d(new Translation3d(-0.15, -0.47, 0.7), new Rotation3d(Math.toRadians(0), Math.toRadians(30), Math.toRadians(5))); + + public static final Pose3d kNOTE_DETECTOR_CAMERA_POSE = // SIM + new Pose3d(new Translation3d(0.076, 0.0, 0.47), new Rotation3d(Math.toRadians(0), Math.toRadians(15.5), Math.toRadians(0))); + public static final double kNOTE_DETECTOR_CAMERA_FOV_X_RAD = Math.toRadians(29.8 * 2); + public static final double kNOTE_DETECTOR_CAMERA_FOV_Y_RAD = Math.toRadians(24.85 * 2); + public static final double kNOTE_DETECTOR_CAMERA_MAX_RANGE_METERS = 2; + public static final double kNOTE_DETECTOR_CAMERA_BLIND_SPOT_DISTANCE_METERS = 0.3; + + } + + public static class IntakeConstants { + public static final double kROLLER_RADIUS_METERS = 0.025; + public static final Translation3d KINTAKE_TRANSLATION3D = new Translation3d(-.38, 0, 0); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..e43b3f2 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -1,25 +1,25 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ -public final class Main { - private Main() {} - - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9d8b15e..abba7f2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,140 +1,140 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.NT4Publisher; - -import com.ctre.phoenix6.SignalLogger; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ -public class Robot extends LoggedRobot { - private Command m_autonomousCommand; - private RobotContainer m_robotContainer; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - - String logPath = "/Users/conne/Downloads/"; // TODO: Remember to change this value guys (Edan) - - @Override - public void robotInit() { - - SignalLogger.enableAutoLogging(true); // TODO: ABSOLUTELY NEED TO BE HERE FOR COMPS - Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value - - switch (Constants.currentMode) { - // Running on a real robot, log to a USB stick - case REAL: - // Logger.addDataReceiver(new WPILOGWriter("D:/")); - Logger.addDataReceiver(new NT4Publisher()); - break; - - // Running a physics simulator, log to local folder - case SIM: - // Logger.addDataReceiver(new WPILOGWriter(logPath)); - Logger.addDataReceiver(new NT4Publisher()); - break; - - // Replaying a log, set up replay source - default: - setUseTiming(false); - // String logPath = LogFileUtil.findReplayLog(); - // Logger.setReplaySource(new WPILOGReader(logPath)); - // // Logger.addDataReceiver(new NT4Publisher()); - // Logger.addDataReceiver(new WPILOGWriter(logPath)); - break; - } - - Logger.start(); - - m_robotContainer = new RobotContainer(); - - - } - - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - - // m_robotContainer.offsetAngle(); - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; + +import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends LoggedRobot { + private Command m_autonomousCommand; + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + + String logPath = "/Users/conne/Downloads/"; // TODO: Remember to change this value guys (Edan) + + @Override + public void robotInit() { + + SignalLogger.enableAutoLogging(true); // TODO: ABSOLUTELY NEED TO BE HERE FOR COMPS + Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value + + switch (Constants.currentMode) { + // Running on a real robot, log to a USB stick + case REAL: + // Logger.addDataReceiver(new WPILOGWriter("D:/")); + Logger.addDataReceiver(new NT4Publisher()); + break; + + // Running a physics simulator, log to local folder + case SIM: + // Logger.addDataReceiver(new WPILOGWriter(logPath)); + Logger.addDataReceiver(new NT4Publisher()); + break; + + // Replaying a log, set up replay source + default: + setUseTiming(false); + // String logPath = LogFileUtil.findReplayLog(); + // Logger.setReplaySource(new WPILOGReader(logPath)); + // // Logger.addDataReceiver(new NT4Publisher()); + // Logger.addDataReceiver(new WPILOGWriter(logPath)); + break; + } + + Logger.start(); + + m_robotContainer = new RobotContainer(); + + + } + + /** + * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** This function is called once each time the robot enters Disabled mode. */ + @Override + public void disabledInit() {} + + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + + // m_robotContainer.offsetAngle(); + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 026c38f..de4adca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,217 +1,220 @@ -package frc.robot; - -import com.pathplanner.lib.auto.NamedCommands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.commands.AutoRunner; -import frc.robot.commands.autoAligment.DriveToPose; -import frc.robot.commands.autoAligment.NotePresent; -import frc.robot.commands.compositions.CancelIntakeNote; -import frc.robot.commands.compositions.FeedAndHoldNote; -import frc.robot.commands.compositions.IntakeNote; -import frc.robot.commands.compositions.IntakeNoteAuto; -import frc.robot.commands.compositions.IntakeNoteManual; -import frc.robot.commands.compositions.ScoreAMP; -import frc.robot.commands.compositions.ShootNote; -import frc.robot.commands.compositions.ShootNoteAuto; -import frc.robot.commands.compositions.ShootNoteTele; -import frc.robot.commands.drivetrain.AbsoluteFieldDrive; -import frc.robot.commands.elevator.MoveElevatorAMP; -import frc.robot.commands.elevator.MoveElevatorToggle; -import frc.robot.commands.elevator.MoveElevatorTurtle; -import frc.robot.commands.intake.InIntake; -import frc.robot.commands.intake.RollIntakeEject; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.pivot.PivotToTorus; -import frc.robot.commands.shooterComp.ShooterStop; -import frc.robot.commands.shooterComp.ShooterWindReverse; -import frc.robot.commands.shooterComp.ShooterWindup; -import frc.robot.commands.shooterComp.ShooterWindupLob; -import frc.robot.lib.input.XboxController; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import frc.robot.subsystems.drivetrain.vision.NoteDetector; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.elevator.ElevatorIO; -import frc.robot.subsystems.elevator.ElevatorIOFalcon; -import frc.robot.subsystems.elevator.ElevatorIOSim; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intakeComp.Intake; -import frc.robot.subsystems.intakeComp.IntakeIO; -import frc.robot.subsystems.intakeComp.IntakeIONeo; -import frc.robot.subsystems.intakeComp.IntakeIOSim; -import frc.robot.subsystems.pivotComp.Pivot; -import frc.robot.subsystems.pivotComp.PivotIO; -import frc.robot.subsystems.pivotComp.PivotIONeo; -import frc.robot.subsystems.pivotComp.PivotIOSim; -// import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel; -import frc.robot.subsystems.shooterComp.Shooter; -import frc.robot.subsystems.shooterComp.ShooterIO; -import frc.robot.subsystems.shooterComp.ShooterIONeo; -import frc.robot.subsystems.shooterComp.ShooterIOSim; - -public class RobotContainer { - public static RobotContainer instance = null; - - private final XboxController xboxTester; - private final XboxController xboxDriver; - private final XboxController xboxOperator; - - // private final Flywheel flywheelSubsystem; - - private final SwerveDrive swerveDrive; - - private final AutoRunner autoRunner; - - private final NoteDetector noteDetector; - - private final Intake intake; // from comp - private final Pivot pivot; - - private final Indexer indexer; - - private final Elevator elevator; - - private final Shooter shooter; - - - public RobotContainer() { - this.xboxTester = new XboxController(1); - this.xboxOperator = new XboxController(2); - this.xboxDriver = new XboxController(3); - - swerveDrive = new SwerveDrive(); - // flywheelSubsystem = new Flywheel(); - noteDetector = new NoteDetector(swerveDrive); - indexer = new Indexer(swerveDrive, noteDetector); - - switch (Constants.currentMode) { - case SIM: - pivot = Pivot.setInstance(new Pivot(new PivotIOSim())); - intake = Intake.setInstance(new Intake(new IntakeIOSim(indexer))); //Assigns the instance object(pointer) to the variable so no new changes are needed. - shooter = Shooter.setInstance(new Shooter(new ShooterIOSim(indexer))); - elevator = Elevator.setInstance(new Elevator(new ElevatorIOSim())); - - break; - - case REAL: - intake = Intake.setInstance(new Intake(new IntakeIONeo(indexer))); - pivot = Pivot.setInstance(new Pivot(new PivotIONeo())); - shooter = Shooter.setInstance(new Shooter(new ShooterIONeo(indexer))); - elevator = Elevator.setInstance(new Elevator(new ElevatorIOFalcon())); - - break; - - default: - pivot = Pivot.setInstance(new Pivot(new PivotIO(){})); - intake = Intake.setInstance(new Intake(new IntakeIO(){})); - shooter = Shooter.setInstance(new Shooter(new ShooterIO(){})); - elevator = Elevator.setInstance(new Elevator(new ElevatorIO(){})); - - break; - } - - - // intake = new Intake(indexer); - indexer.setSubsystem(intake, pivot); - - - autoRunner = new AutoRunner(swerveDrive); - - NamedCommands.registerCommand("MoveElevatorAMP", new MoveElevatorAMP()); - NamedCommands.registerCommand("MoveElevatorTurtle", new MoveElevatorTurtle()); - NamedCommands.registerCommand("ShooterWindUp", new ShooterWindup()); - NamedCommands.registerCommand("RollIntakeIn", new RollIntakeIn()); - NamedCommands.registerCommand("RollIntakeEject", new RollIntakeEject()); - NamedCommands.registerCommand("StopIntake", new StopIntake()); - NamedCommands.registerCommand("IntakeNote", new IntakeNote(swerveDrive, intake, noteDetector)); - NamedCommands.registerCommand("ShooterStop", new ShooterStop()); - NamedCommands.registerCommand("ShooterWindReverse", new ShooterWindReverse()); - NamedCommands.registerCommand("ShootNote", new ShootNote()); - NamedCommands.registerCommand("ShootNoteAuto", new ShootNoteAuto()); - NamedCommands.registerCommand("PivotToTorus", new PivotToTorus()); - NamedCommands.registerCommand("CancelIntakeNote", new CancelIntakeNote(null, null, swerveDrive)); - NamedCommands.registerCommand("RollIntakeIn", new RollIntakeIn()); - NamedCommands.registerCommand("LobNoteAuto", new SequentialCommandGroup( - new ParallelCommandGroup(new WaitCommand(0.4), new ShooterWindup(30)), - new RollIntakeIn(), - new StopIntake(), - new ShooterStop())); - NamedCommands.registerCommand("VariableShoot", new InstantCommand(() -> Shooter.getInstance().setVelocityRadSec(0, true, 65, 17.5))); - NamedCommands.registerCommand("InIntake", new InIntake()); - NamedCommands.registerCommand("AmpNotePresent", new NotePresent(noteDetector, intake, swerveDrive, 5, false)); - NamedCommands.registerCommand("MidNotePresent", new NotePresent(noteDetector, intake, swerveDrive, 6, false)); - NamedCommands.registerCommand("AmpNoteNotPresent", new NotePresent(noteDetector, intake, swerveDrive, 5, true)); - - NamedCommands.registerCommand("DriveToMidNoteFromAmp", DriveToPose.getCommand(new Pose2d(new Translation2d(2.62, 6.64), new Rotation2d(Math.toRadians(-75))))); - - - // OP Controlls - SequentialCommandGroup intakeG, feedHold; - intakeG = null; - swerveDrive.setDefaultCommand(new AbsoluteFieldDrive(swerveDrive, - () -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND), - () -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND))); - - this.xboxOperator.getRightBumper().onTrue(new ShooterWindup()); - this.xboxOperator.getXButton().onTrue(new MoveElevatorToggle()); - this.xboxOperator.getYButton().onTrue(new ScoreAMP()); // changed - this.xboxOperator.getAButton().onTrue(new ShootNoteTele()); // change back to shootNoteTele - this.xboxOperator.getBButton().onTrue(feedHold = new FeedAndHoldNote()); - this.xboxOperator.getLeftBumper().onTrue(new ShooterStop(feedHold)); - // this.xboxOperator.getRightMiddleButton().onTrue(new ParallelCommandGroup(new MoveElevatorAMP(), new IntakeNote())); - this.xboxOperator.getLeftMiddleButton().onTrue(new ShooterWindupLob()); - this.xboxOperator.getRightMiddleButton().onTrue(new RollIntakeEject()); - - // driver controlls - this.xboxDriver.getXButton().onTrue(new InstantCommand(() -> swerveDrive.zeroGyro())); - // this.xboxDriver.getLeftBumper().onTrue(new IntakeNote(swerveDrive, intake, noteDetector)); - this.xboxDriver.getLeftBumper().onTrue(new IntakeNoteManual()); - - this.xboxDriver.getRightMiddleButton().onTrue(new RollIntakeEject()); - this.xboxDriver.getRightBumper().onTrue(new CancelIntakeNote(intakeG, feedHold, swerveDrive)); - // this.xboxDriver.getLeftMiddleButton().onTrue(new InstantCommand(()-> pivot.zeroAngle())); - this.xboxDriver.getYButton().onTrue(new InstantCommand(()-> Pivot.getInstance().TorusAngleReset())); - - // SYSID STUFF - // xboxTester.getAButton().whileTrue(swerveDrive.sysIDriveQuasistatic(Direction.kForward)); - xboxTester.getAButton().whileTrue(DriveToPose.getCommand(new Pose2d(new Translation2d(7.11, 6.48), new Rotation2d(Math.toRadians(161.27))))); - - xboxTester.getBButton().whileTrue(swerveDrive.sysIDriveQuasistatic(Direction.kReverse)); - xboxTester.getXButton().whileTrue(swerveDrive.sysIdDriveDynamic(Direction.kForward)); - xboxTester.getYButton().whileTrue(swerveDrive.sysIdDriveDynamic(Direction.kReverse)); - - } - - public static RobotContainer getInstance() { - if (instance == null) { - instance = new RobotContainer(); - } - return instance; - } - - public Command getAutonomousCommand() { - return autoRunner.getAutonomousCommand(swerveDrive, intake, noteDetector); - } - - public String getSelectedAuto() { - return autoRunner.getSelectedAutoName(); - } - - public void offsetAngle() { - swerveDrive.resetPose(new Pose2d( - swerveDrive.getPose().getTranslation(), - swerveDrive.getPose().getRotation().plus(new Rotation2d(Math.PI / 2)))); - } -} - +package frc.robot; + +import com.pathplanner.lib.auto.NamedCommands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.commands.AutoRunner; +import frc.robot.commands.autoAligment.DriveToPose; +import frc.robot.commands.autoAligment.NotePresent; +import frc.robot.commands.compositions.CancelIntakeNote; +import frc.robot.commands.compositions.FeedAndHoldNote; +import frc.robot.commands.compositions.IntakeNote; +import frc.robot.commands.compositions.IntakeNoteAuto; +import frc.robot.commands.compositions.IntakeNoteManual; +import frc.robot.commands.compositions.ScoreAMP; +import frc.robot.commands.compositions.ShootNote; +import frc.robot.commands.compositions.ShootNoteAuto; +import frc.robot.commands.compositions.ShootNoteTele; +import frc.robot.commands.drivetrain.AbsoluteFieldDrive; +import frc.robot.commands.elevator.MoveElevatorAMP; +import frc.robot.commands.elevator.MoveElevatorToggle; +import frc.robot.commands.elevator.MoveElevatorTurtle; +import frc.robot.commands.intake.InIntake; +import frc.robot.commands.intake.RollIntakeEject; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.pivot.PivotToTorus; +import frc.robot.commands.shooterComp.ShooterAmp; +import frc.robot.commands.shooterComp.ShooterStop; +import frc.robot.commands.shooterComp.ShooterWindReverse; +import frc.robot.commands.shooterComp.ShooterWindup; +import frc.robot.commands.shooterComp.ShooterWindupLob; +import frc.robot.lib.input.XboxController; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import frc.robot.subsystems.drivetrain.vision.NoteDetector; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorIO; +import frc.robot.subsystems.elevator.ElevatorIOFalcon; +import frc.robot.subsystems.elevator.ElevatorIOSim; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intakeComp.Intake; +import frc.robot.subsystems.intakeComp.IntakeIO; +import frc.robot.subsystems.intakeComp.IntakeIONeo; +import frc.robot.subsystems.intakeComp.IntakeIOSim; +import frc.robot.subsystems.pivotComp.Pivot; +import frc.robot.subsystems.pivotComp.PivotIO; +import frc.robot.subsystems.pivotComp.PivotIONeo; +import frc.robot.subsystems.pivotComp.PivotIOSim; +// import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel; +import frc.robot.subsystems.shooterComp.Shooter; +import frc.robot.subsystems.shooterComp.ShooterIO; +import frc.robot.subsystems.shooterComp.ShooterIONeo; +import frc.robot.subsystems.shooterComp.ShooterIOSim; + +public class RobotContainer { + public static RobotContainer instance = null; + + private final XboxController xboxTester; + private final XboxController xboxDriver; + private final XboxController xboxOperator; + + // private final Flywheel flywheelSubsystem; + + private final SwerveDrive swerveDrive; + + private final AutoRunner autoRunner; + + private final NoteDetector noteDetector; + + private final Intake intake; // from comp + private final Pivot pivot; + + private final Indexer indexer; + + private final Elevator elevator; + + private final Shooter shooter; + + + public RobotContainer() { + this.xboxTester = new XboxController(1); + this.xboxOperator = new XboxController(2); + this.xboxDriver = new XboxController(3); + + swerveDrive = new SwerveDrive(); + // flywheelSubsystem = new Flywheel(); + noteDetector = new NoteDetector(swerveDrive); + indexer = new Indexer(swerveDrive, noteDetector); + + switch (Constants.currentMode) { + case SIM: + pivot = Pivot.setInstance(new Pivot(new PivotIOSim())); + intake = Intake.setInstance(new Intake(new IntakeIOSim(indexer))); //Assigns the instance object(pointer) to the variable so no new changes are needed. + shooter = Shooter.setInstance(new Shooter(new ShooterIOSim(indexer))); + elevator = Elevator.setInstance(new Elevator(new ElevatorIOSim())); + + break; + + case REAL: + intake = Intake.setInstance(new Intake(new IntakeIONeo(indexer))); + pivot = Pivot.setInstance(new Pivot(new PivotIONeo())); + shooter = Shooter.setInstance(new Shooter(new ShooterIONeo(indexer))); + elevator = Elevator.setInstance(new Elevator(new ElevatorIOFalcon())); + + break; + + default: + pivot = Pivot.setInstance(new Pivot(new PivotIO(){})); + intake = Intake.setInstance(new Intake(new IntakeIO(){})); + shooter = Shooter.setInstance(new Shooter(new ShooterIO(){})); + elevator = Elevator.setInstance(new Elevator(new ElevatorIO(){})); + + break; + } + + + // intake = new Intake(indexer); + indexer.setSubsystem(intake, pivot); + + + autoRunner = new AutoRunner(swerveDrive); + + NamedCommands.registerCommand("MoveElevatorAMP", new MoveElevatorAMP()); + NamedCommands.registerCommand("MoveElevatorTurtle", new MoveElevatorTurtle()); + NamedCommands.registerCommand("ShooterWindUp", new ShooterWindup()); + NamedCommands.registerCommand("RollIntakeIn", new RollIntakeIn()); + NamedCommands.registerCommand("RollIntakeEject", new RollIntakeEject()); + NamedCommands.registerCommand("StopIntake", new StopIntake()); + NamedCommands.registerCommand("IntakeNote", new IntakeNote(swerveDrive, intake, noteDetector)); + NamedCommands.registerCommand("ShooterStop", new ShooterStop()); + NamedCommands.registerCommand("ShooterWindReverse", new ShooterWindReverse()); + NamedCommands.registerCommand("ShootNote", new ShootNote()); + NamedCommands.registerCommand("ShootNoteAuto", new ShootNoteAuto()); + NamedCommands.registerCommand("PivotToTorus", new PivotToTorus()); + NamedCommands.registerCommand("CancelIntakeNote", new CancelIntakeNote(null, null, swerveDrive)); + NamedCommands.registerCommand("RollIntakeIn", new RollIntakeIn()); + NamedCommands.registerCommand("LobNoteAuto", new SequentialCommandGroup( + new ParallelCommandGroup(new WaitCommand(0.4), new ShooterWindup(30)), + new RollIntakeIn(), + new StopIntake(), + new ShooterStop())); + NamedCommands.registerCommand("VariableShoot", new InstantCommand(() -> Shooter.getInstance().setVelocityRadSec(0, true, 65, 17.5))); + NamedCommands.registerCommand("InIntake", new InIntake()); + NamedCommands.registerCommand("AmpNotePresent", new NotePresent(noteDetector, intake, swerveDrive, 5, false)); + NamedCommands.registerCommand("MidNotePresent", new NotePresent(noteDetector, intake, swerveDrive, 6, false)); + NamedCommands.registerCommand("AmpNoteNotPresent", new NotePresent(noteDetector, intake, swerveDrive, 5, true)); + + NamedCommands.registerCommand("DriveToMidNoteFromAmp", DriveToPose.getCommand(new Pose2d(new Translation2d(2.62, 6.64), new Rotation2d(Math.toRadians(-75))))); + + + // OP Controlls + SequentialCommandGroup intakeG, feedHold; + intakeG = null; + swerveDrive.setDefaultCommand(new AbsoluteFieldDrive(swerveDrive, + () -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND), + () -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND))); + + this.xboxOperator.getRightBumper().onTrue(new ShooterWindup()); + // this.xboxOperator.getRightBumper().onTrue(new ShooterAmp()); + this.xboxOperator.getXButton().onTrue(new MoveElevatorToggle()); + this.xboxOperator.getYButton().onTrue(new ScoreAMP()); // changed + this.xboxOperator.getAButton().onTrue(new ShootNoteTele()); // change back to shootNoteTele + this.xboxOperator.getBButton().onTrue(feedHold = new FeedAndHoldNote()); + this.xboxOperator.getLeftBumper().onTrue(new ShooterStop(feedHold)); + // this.xboxOperator.getRightMiddleButton().onTrue(new ParallelCommandGroup(new MoveElevatorAMP(), new IntakeNote())); + this.xboxOperator.getLeftMiddleButton().onTrue(new ShooterWindupLob()); + this.xboxOperator.getRightMiddleButton().onTrue(new RollIntakeEject()); + + // driver controlls + this.xboxDriver.getXButton().onTrue(new InstantCommand(() -> swerveDrive.zeroGyro())); + this.xboxDriver.getLeftBumper().onTrue(new IntakeNoteAuto()); + + // this.xboxDriver.getLeftBumper().onTrue(new IntakeNoteManual()); + + this.xboxDriver.getRightMiddleButton().onTrue(new RollIntakeEject()); + this.xboxDriver.getRightBumper().onTrue(new CancelIntakeNote(intakeG, feedHold, swerveDrive)); + // this.xboxDriver.getLeftMiddleButton().onTrue(new InstantCommand(()-> pivot.zeroAngle())); + this.xboxDriver.getYButton().onTrue(new InstantCommand(()-> Pivot.getInstance().TorusAngleReset())); + + // SYSID STUFF + // xboxTester.getAButton().whileTrue(swerveDrive.sysIDriveQuasistatic(Direction.kForward)); + xboxTester.getAButton().whileTrue(DriveToPose.getCommand(new Pose2d(new Translation2d(7.11, 6.48), new Rotation2d(Math.toRadians(161.27))))); + + xboxTester.getBButton().whileTrue(swerveDrive.sysIDriveQuasistatic(Direction.kReverse)); + xboxTester.getXButton().whileTrue(swerveDrive.sysIdDriveDynamic(Direction.kForward)); + xboxTester.getYButton().whileTrue(swerveDrive.sysIdDriveDynamic(Direction.kReverse)); + + } + + public static RobotContainer getInstance() { + if (instance == null) { + instance = new RobotContainer(); + } + return instance; + } + + public Command getAutonomousCommand() { + return autoRunner.getAutonomousCommand(swerveDrive, intake, noteDetector); + } + + public String getSelectedAuto() { + return autoRunner.getSelectedAutoName(); + } + + public void offsetAngle() { + swerveDrive.resetPose(new Pose2d( + swerveDrive.getPose().getTranslation(), + swerveDrive.getPose().getRotation().plus(new Rotation2d(Math.PI / 2)))); + } +} + diff --git a/src/main/java/frc/robot/commands/AutoCommand.java b/src/main/java/frc/robot/commands/AutoCommand.java index 2e55dc4..39ef318 100644 --- a/src/main/java/frc/robot/commands/AutoCommand.java +++ b/src/main/java/frc/robot/commands/AutoCommand.java @@ -1,207 +1,207 @@ -package frc.robot.commands; - -import org.littletonrobotics.junction.Logger; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathPlannerPath; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import frc.robot.Constants; -import frc.robot.commands.autoAligment.DriveToPose; -import frc.robot.commands.autoAligment.NotePresent; -import frc.robot.commands.compositions.IntakeNote; -import frc.robot.commands.compositions.ShootNoteAuto; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import frc.robot.subsystems.drivetrain.vision.NoteDetector; -import frc.robot.subsystems.intakeComp.Intake; - -public class AutoCommand extends Command { - private final String[] args; - private final SwerveDrive swerveDrive; - private final Intake intake; - private final NoteDetector noteDetector; - private boolean isRed = false; - - private Command currentCommand = new Command() { - @Override - public boolean isFinished() { - return true; // i had no better way of doing this \_^_^_/ - } - }; - - private int i = 0; - - /* - ARG: Array of Strings representing tasks for the funcion to composit into a auto. - - It is important that all paths are properly named to convetion for the parser to work properly. - - Follow a path -> pass the path name as used in pathplanner. - If this is a "To" path, the next arg needs to be the indended note and follow with the desired fallbacks ie. amp, mid, source, far1, ... far5. - These fallbacks will be executed in the order they are provided. - Score -> "score" - - Example: { "ToFar1FromAmp", "far1", far2", "far3", "FromFar1ToAmp", "score" } - */ - public AutoCommand(String[] args, SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { - this.args = args; - this.swerveDrive = swerveDrive; - this.noteDetector = noteDetector; - this.intake = intake; - } - - @Override - public void initialize() { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - isRed = alliance.get() == DriverStation.Alliance.Red; - } - - PathPlannerPath path = PathPlannerPath.fromPathFile(args[0]); - if (isRed) { - path = path.flipPath(); - - Pose2d p = path.getPreviewStartingHolonomicPose(); - new InstantCommand(() -> swerveDrive.resetPose( - new Pose2d(p.getTranslation(), - new Rotation2d(p.getRotation().unaryMinus().getRadians() + Math.PI / 2 + Math.PI))) - ).schedule(); - } - else { - Pose2d p = path.getPreviewStartingHolonomicPose(); - new InstantCommand(() -> swerveDrive.resetPose( - new Pose2d(p.getTranslation(), - new Rotation2d(p.getRotation().unaryMinus().getRadians() + Math.PI / 2))) - ).schedule(); - } - - - - } - @Override - public void execute() { - Logger.recordOutput("AutoCommand/currentCommand.isFinished()", currentCommand.isFinished()); - if (i >= args.length) { - return; - } - - if (!currentCommand.isFinished()) { - return; - } - - String cmd = args[i]; - Logger.recordOutput("AutoCommand/cmd", cmd); - - if (cmd == "score") { - Logger.recordOutput("AutoCommand/currentCommand", "score"); - - currentCommand = new ShootNoteAuto(); - } - - - else if (cmd.substring(0, 2).equals("To") || - cmd.substring(0, 2).equals("Fr")) { - Logger.recordOutput("AutoCommand/currentCommand", "driveToNote"); - - PathPlannerPath path = PathPlannerPath.fromPathFile(cmd); - currentCommand = AutoBuilder.followPath(path); - } - - else if (NotePresent.notePresent(noteDetector, intake, swerveDrive, getIndex(cmd), false)) { - Logger.recordOutput("AutoCommand/currentCommand", "pickUpNote"); - - currentCommand = new ParallelRaceGroup( - new NotePresent(noteDetector, intake, swerveDrive, getIndex(cmd), false), - new IntakeNote(swerveDrive, intake, noteDetector) - ); - } - - else if (i+1 < args.length && !(args[i+1].substring(0, 2).equals("To") || args[i+1].substring(0, 2).equals("Fr"))) { - Logger.recordOutput("AutoCommand/currentCommand", "replan"); - - Translation2d notePose = Constants.FieldConstants.kNOTE_ARR[getIndex(args[i+1])].toTranslation2d(); - - Translation2d offset; - if (swerveDrive.getPose().getY() > notePose.getY()) { - offset = new Translation2d(-0.5, 0.5); - } - else { - offset = new Translation2d(-0.5, -0.5); - } - - if (isRed) { - offset = new Translation2d(-offset.getX(), offset.getY()); - } - - Translation2d finalT = notePose.plus(offset); - - currentCommand = DriveToPose.getCommand( - new Pose2d( - finalT, - new Rotation2d( - Math.atan2(finalT.getY() - notePose.getY(), finalT.getX() - notePose.getX()))) - ); - } - - currentCommand.schedule(); - - i++; - } - - @Override - public boolean isFinished() { - return false; - } - - private int getIndex(String cmd) { - int index; - // LOL - switch (cmd) { - case "far1": - index = 0; - break; - - case "far2": - index = 1; - break; - - case "far3": - index = 2; - break; - - case "far4": - index = 3; - break; - - case "far5": - index = 4; - break; - - case "amp": - index = 5; - break; - - case "mid": - index = 6; - break; - - case "source": - index = 7; - break; - - default: - index = 0; - break; - } - - return index; - } -} +package frc.robot.commands; + +import org.littletonrobotics.junction.Logger; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import frc.robot.Constants; +import frc.robot.commands.autoAligment.DriveToPose; +import frc.robot.commands.autoAligment.NotePresent; +import frc.robot.commands.compositions.IntakeNote; +import frc.robot.commands.compositions.ShootNoteAuto; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import frc.robot.subsystems.drivetrain.vision.NoteDetector; +import frc.robot.subsystems.intakeComp.Intake; + +public class AutoCommand extends Command { + private final String[] args; + private final SwerveDrive swerveDrive; + private final Intake intake; + private final NoteDetector noteDetector; + private boolean isRed = false; + + private Command currentCommand = new Command() { + @Override + public boolean isFinished() { + return true; // i had no better way of doing this \_^_^_/ + } + }; + + private int i = 0; + + /* + ARG: Array of Strings representing tasks for the funcion to composit into a auto. + + It is important that all paths are properly named to convetion for the parser to work properly. + + Follow a path -> pass the path name as used in pathplanner. + If this is a "To" path, the next arg needs to be the indended note and follow with the desired fallbacks ie. amp, mid, source, far1, ... far5. + These fallbacks will be executed in the order they are provided. + Score -> "score" + + Example: { "ToFar1FromAmp", "far1", far2", "far3", "FromFar1ToAmp", "score" } + */ + public AutoCommand(String[] args, SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { + this.args = args; + this.swerveDrive = swerveDrive; + this.noteDetector = noteDetector; + this.intake = intake; + } + + @Override + public void initialize() { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + isRed = alliance.get() == DriverStation.Alliance.Red; + } + + PathPlannerPath path = PathPlannerPath.fromPathFile(args[0]); + if (isRed) { + path = path.flipPath(); + + Pose2d p = path.getPreviewStartingHolonomicPose(); + new InstantCommand(() -> swerveDrive.resetPose( + new Pose2d(p.getTranslation(), + new Rotation2d(p.getRotation().unaryMinus().getRadians() + Math.PI / 2 + Math.PI))) + ).schedule(); + } + else { + Pose2d p = path.getPreviewStartingHolonomicPose(); + new InstantCommand(() -> swerveDrive.resetPose( + new Pose2d(p.getTranslation(), + new Rotation2d(p.getRotation().unaryMinus().getRadians() + Math.PI / 2))) + ).schedule(); + } + + + + } + @Override + public void execute() { + Logger.recordOutput("AutoCommand/currentCommand.isFinished()", currentCommand.isFinished()); + if (i >= args.length) { + return; + } + + if (!currentCommand.isFinished()) { + return; + } + + String cmd = args[i]; + Logger.recordOutput("AutoCommand/cmd", cmd); + + if (cmd == "score") { + Logger.recordOutput("AutoCommand/currentCommand", "score"); + + currentCommand = new ShootNoteAuto(); + } + + + else if (cmd.substring(0, 2).equals("To") || + cmd.substring(0, 2).equals("Fr")) { + Logger.recordOutput("AutoCommand/currentCommand", "driveToNote"); + + PathPlannerPath path = PathPlannerPath.fromPathFile(cmd); + currentCommand = AutoBuilder.followPath(path); + } + + else if (NotePresent.notePresent(noteDetector, intake, swerveDrive, getIndex(cmd), false)) { + Logger.recordOutput("AutoCommand/currentCommand", "pickUpNote"); + + currentCommand = new ParallelRaceGroup( + new NotePresent(noteDetector, intake, swerveDrive, getIndex(cmd), false), + new IntakeNote(swerveDrive, intake, noteDetector) + ); + } + + else if (i+1 < args.length && !(args[i+1].substring(0, 2).equals("To") || args[i+1].substring(0, 2).equals("Fr"))) { + Logger.recordOutput("AutoCommand/currentCommand", "replan"); + + Translation2d notePose = Constants.FieldConstants.kNOTE_ARR[getIndex(args[i+1])].toTranslation2d(); + + Translation2d offset; + if (swerveDrive.getPose().getY() > notePose.getY()) { + offset = new Translation2d(-0.5, 0.5); + } + else { + offset = new Translation2d(-0.5, -0.5); + } + + if (isRed) { + offset = new Translation2d(-offset.getX(), offset.getY()); + } + + Translation2d finalT = notePose.plus(offset); + + currentCommand = DriveToPose.getCommand( + new Pose2d( + finalT, + new Rotation2d( + Math.atan2(finalT.getY() - notePose.getY(), finalT.getX() - notePose.getX()))) + ); + } + + currentCommand.schedule(); + + i++; + } + + @Override + public boolean isFinished() { + return false; + } + + private int getIndex(String cmd) { + int index; + // LOL + switch (cmd) { + case "far1": + index = 0; + break; + + case "far2": + index = 1; + break; + + case "far3": + index = 2; + break; + + case "far4": + index = 3; + break; + + case "far5": + index = 4; + break; + + case "amp": + index = 5; + break; + + case "mid": + index = 6; + break; + + case "source": + index = 7; + break; + + default: + index = 0; + break; + } + + return index; + } +} diff --git a/src/main/java/frc/robot/commands/AutoRunner.java b/src/main/java/frc/robot/commands/AutoRunner.java index dd0ba80..5e17c99 100644 --- a/src/main/java/frc/robot/commands/AutoRunner.java +++ b/src/main/java/frc/robot/commands/AutoRunner.java @@ -1,115 +1,115 @@ -package frc.robot.commands; - -import java.util.HashMap; -import java.util.List; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.commands.PathfindThenFollowPathHolonomic; -import com.pathplanner.lib.path.PathPlannerPath; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import frc.robot.subsystems.drivetrain.vision.NoteDetector; -import frc.robot.subsystems.intakeComp.Intake; - -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.ReplanningConfig; -import frc.robot.commands.Autos; - -// acts more like a helper class rather than a subsystem or command. -public class AutoRunner { - - - private final SendableChooser pathChooser = new SendableChooser(); - private String pathChosen = "taxi"; - private static final HashMap PATH_CHOSEN_TO_NAME_HASH_MAP = new HashMap<>(); - private static final HashMap EVENT_MAP = new HashMap<>(); - // private static PathPlannerAuto pathPlanner = new PathPlannerAuto(); - // private static PPSwerveControllerCommand scc; - // private static HolonomicDriveController hdc = new HolonomicDriveController( - // new PIDController(0.1, 0, 0), new PIDController(.1, 0, 0), new ProfiledPIDController(0.1, 0, 0,new Constraints(Constants.Auton.MAX_SPEED, Constants.Auton.MAX_ACCELERATION))); - private static List pathList; - private static PathPlannerPath path; - private static PathfindThenFollowPathHolonomic pathCommand; - - - static { - PATH_CHOSEN_TO_NAME_HASH_MAP.put("3PMidTop", "3PMidTop"); - PATH_CHOSEN_TO_NAME_HASH_MAP.put("3PMidMA", "3PMidMA"); - } - - public AutoRunner (SwerveDrive swerveSubsystem) { - //NamedCommands.registerCommand("MoveElevatorAMP", new MoveElevatorAMP(elevatorSubsystem)); - - PATH_CHOSEN_TO_NAME_HASH_MAP.forEach((pathName, pathFile) -> pathChooser.addOption(pathName, pathFile)); - - Shuffleboard.getTab("Auto").add("Path Chooser", pathChooser); - // Shuffleboard.getTab("Auto").add("Update Selected Command Output", - // new InstantCommand( () -> loadPath())); - - AutoBuilder.configureHolonomic( - // visionSubsystem::getBotPose2d, - swerveSubsystem::getPose, // Robot pose supplier - swerveSubsystem::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) - swerveSubsystem::getMeasuredRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - swerveSubsystem::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - Constants.Auton.TRANSLATION_PID_CONFIG, // Translation PID constants - Constants.Auton.ANGLE_PID_CONFIG, // Rotation PID constants - Constants.Auton.MAX_MODULE_SPEED, // Max module speed, in m/s - 0.5384061785684372, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options here - ), - () -> { - // 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 true; - }, - swerveSubsystem // Reference to this subsystem to set requirements - ); - } - - private void loadPath() { - pathChosen = pathChooser.getSelected(); - Shuffleboard.getTab("Auto").add("Selected Path", pathChosen); - } - - - public Command getAutonomousCommand(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { - //TODO:!! HERE CHANGE THIS - //4PMidMASScoreTurn - //3PAmpAM << Amp Side 3 piece - //2PAmp << 2 piece Amp' - //AmpMidlineDisrupt << Starts form AMp - //3.5PMidMSFar1 - //3PAmpA1 - //3PMidMAScoreTurn - //TT << turnTest - //4PAmpA12 - //OUCH4PMidMASScoreTurn - //3PMidAmpVariable - //4PMidMASScoreTrans - //2.5PAmpA1 - //3PAmpA2 << FAR 2 - pathChosen = "4PMidMASScoreTurn"; - // return new PathPlannerAuto(pathChosen); - // return Autos.Far1(swerveDrive, intake, noteDetector); - // return Autos.configAuto(swerveDrive); - return Autos.fourPeiceMid(swerveDrive, intake, noteDetector); - } - - public String getSelectedAutoName() { - return pathChosen; - } -} +package frc.robot.commands; + +import java.util.HashMap; +import java.util.List; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.commands.PathfindThenFollowPathHolonomic; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import frc.robot.subsystems.drivetrain.vision.NoteDetector; +import frc.robot.subsystems.intakeComp.Intake; + +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.ReplanningConfig; +import frc.robot.commands.Autos; + +// acts more like a helper class rather than a subsystem or command. +public class AutoRunner { + + + private final SendableChooser pathChooser = new SendableChooser(); + private String pathChosen = "taxi"; + private static final HashMap PATH_CHOSEN_TO_NAME_HASH_MAP = new HashMap<>(); + private static final HashMap EVENT_MAP = new HashMap<>(); + // private static PathPlannerAuto pathPlanner = new PathPlannerAuto(); + // private static PPSwerveControllerCommand scc; + // private static HolonomicDriveController hdc = new HolonomicDriveController( + // new PIDController(0.1, 0, 0), new PIDController(.1, 0, 0), new ProfiledPIDController(0.1, 0, 0,new Constraints(Constants.Auton.MAX_SPEED, Constants.Auton.MAX_ACCELERATION))); + private static List pathList; + private static PathPlannerPath path; + private static PathfindThenFollowPathHolonomic pathCommand; + + + static { + PATH_CHOSEN_TO_NAME_HASH_MAP.put("3PMidTop", "3PMidTop"); + PATH_CHOSEN_TO_NAME_HASH_MAP.put("3PMidMA", "3PMidMA"); + } + + public AutoRunner (SwerveDrive swerveSubsystem) { + //NamedCommands.registerCommand("MoveElevatorAMP", new MoveElevatorAMP(elevatorSubsystem)); + + PATH_CHOSEN_TO_NAME_HASH_MAP.forEach((pathName, pathFile) -> pathChooser.addOption(pathName, pathFile)); + + Shuffleboard.getTab("Auto").add("Path Chooser", pathChooser); + // Shuffleboard.getTab("Auto").add("Update Selected Command Output", + // new InstantCommand( () -> loadPath())); + + AutoBuilder.configureHolonomic( + // visionSubsystem::getBotPose2d, + swerveSubsystem::getPose, // Robot pose supplier + swerveSubsystem::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) + swerveSubsystem::getMeasuredRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + swerveSubsystem::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + Constants.Auton.TRANSLATION_PID_CONFIG, // Translation PID constants + Constants.Auton.ANGLE_PID_CONFIG, // Rotation PID constants + Constants.Auton.MAX_MODULE_SPEED, // Max module speed, in m/s + 0.5384061785684372, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options here + ), + () -> { + // 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 true; + }, + swerveSubsystem // Reference to this subsystem to set requirements + ); + } + + private void loadPath() { + pathChosen = pathChooser.getSelected(); + Shuffleboard.getTab("Auto").add("Selected Path", pathChosen); + } + + + public Command getAutonomousCommand(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { + //TODO:!! HERE CHANGE THIS + //4PMidMASScoreTurn + //3PAmpAM << Amp Side 3 piece + //2PAmp << 2 piece Amp' + //AmpMidlineDisrupt << Starts form AMp + //3.5PMidMSFar1 + //3PAmpA1 + //3PMidMAScoreTurn + //TT << turnTest + //4PAmpA12 + //OUCH4PMidMASScoreTurn + //3PMidAmpVariable + //4PMidMASScoreTrans + //2.5PAmpA1 + //3PAmpA2 << FAR 2 + pathChosen = "4PMidMASScoreTurn"; + // return new PathPlannerAuto(pathChosen); + // return Autos.Far1(swerveDrive, intake, noteDetector); + // return Autos.configAuto(swerveDrive); + return Autos.fourPeiceMid(swerveDrive, intake, noteDetector); + } + + public String getSelectedAutoName() { + return pathChosen; + } +} diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index e6f24e9..e20b3ee 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -1,109 +1,137 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.commands.autoAligment.DriveToPose; -import frc.robot.commands.autoAligment.NotePresent; -import frc.robot.commands.compositions.IntakeNote; -import frc.robot.commands.compositions.ShootNoteAuto; -import frc.robot.commands.pivot.PivotToTorus; -import frc.robot.subsystems.ExampleSubsystem; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import frc.robot.subsystems.drivetrain.vision.NoteDetector; -import frc.robot.subsystems.intakeComp.Intake; - -import java.util.ArrayList; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public final class Autos { - /** Example static factory for an autonomous command. */ - public static Command exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); - } - - public static Command Far1(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { - return new AutoCommand(new String[] {"ToFar1FromAMP", "far1", "far2", "far3", "ToAMPFromFar1"}, swerveDrive, intake, noteDetector); - } - - public static Command configAuto(SwerveDrive swerveDrive) { - Pose2d p = PathPlannerPath.fromPathFile("configtpath").getPreviewStartingHolonomicPose(); - return new SequentialCommandGroup( - new InstantCommand(() -> swerveDrive.resetPose(new Pose2d(p.getTranslation(), p.getRotation()/*new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2)))*/))), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("configtpath")), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("configt2path"))); - - } - - public static Command fourPeiceMid(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { - Pose2d p = PathPlannerPath.fromPathFile("ToMidNoteFromMid").getPreviewStartingHolonomicPose(); - return new SequentialCommandGroup( - new InstantCommand(() -> swerveDrive.resetPose(new Pose2d(p.getTranslation(), p.getRotation()/*new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2)))*/))), - new PivotToTorus(), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToMidNoteFromMid")), - new IntakeNote(swerveDrive, intake, noteDetector), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("FromMidNoteToMid")), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToSourceNoteFromMid")), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("FromSourceNoteToMid")), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToAMPNoteFromMidTurn")), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("FromAMPNoteToMidTurn"))); - } - - public static Command adaptableTest(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { - Pose2d p = PathPlannerPath.fromPathFile("ToAmpNoteFromAmpShort").getPreviewStartingHolonomicPose(); - // return new SequentialCommandGroup(null) - - Command midNote = new SequentialCommandGroup( - DriveToPose.getCommand(new Pose2d(new Translation2d(2.62, 6.64), new Rotation2d(Math.toRadians(76.62)))), - new ConditionalCommand( - new ParallelRaceGroup( - new IntakeNote(swerveDrive, intake, noteDetector), - new NotePresent(noteDetector, intake, swerveDrive, 6, false) - ), - new InstantCommand(), - () -> !noteDetector.checked(6) - ) - ); - - - Command midNoteInter = midNote.asProxy(); - - Command ampNote = new ParallelRaceGroup( - new IntakeNote(swerveDrive, intake, noteDetector), - new NotePresent(noteDetector, intake, swerveDrive, 5, false) - ); - - - return new SequentialCommandGroup( - new InstantCommand(() -> swerveDrive.resetPose(new Pose2d(p.getTranslation(), new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2)))), - // new InstantCommand(() -> System.out.println(new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2).getDegrees())), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToAmpNoteFromAmpShort")), - new ConditionalCommand( - ampNote, - midNote, - () -> !noteDetector.checked(5) - ), - new ConditionalCommand( - midNoteInter, - new InstantCommand(), - () -> noteDetector.checked(5) && !noteDetector.checked(6) - ), - AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToAmpFromAmpNoteShort")) - ); - } +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import frc.robot.Constants; +import frc.robot.commands.autoAligment.DriveToPose; +import frc.robot.commands.autoAligment.NotePresent; +import frc.robot.commands.compositions.IntakeNote; +import frc.robot.commands.compositions.ShootNoteAuto; +import frc.robot.commands.drivetrain.DriveToNote; +import frc.robot.commands.intake.InIntake; +import frc.robot.commands.intake.OutIntake; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.RollIntakeInSlow; +import frc.robot.commands.intake.RollIntakeOut; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.pivot.PivotToTorus; +import frc.robot.commands.pivot.PivotTurtle; +import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import frc.robot.subsystems.drivetrain.vision.NoteDetector; +import frc.robot.subsystems.intakeComp.Intake; + +import java.util.ArrayList; + +import com.fasterxml.jackson.databind.SequenceWriter; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public final class Autos { + /** Example static factory for an autonomous command. */ + public static Command exampleAuto(ExampleSubsystem subsystem) { + return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); + } + + public static Command Far1(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { + return new AutoCommand(new String[] {"ToFar1FromAMP", "far1", "far2", "far3", "ToAMPFromFar1"}, swerveDrive, intake, noteDetector); + } + + public static Command configAuto(SwerveDrive swerveDrive) { + Pose2d p = PathPlannerPath.fromPathFile("configtpath").getPreviewStartingHolonomicPose(); + return new SequentialCommandGroup( + new InstantCommand(() -> swerveDrive.resetPose(new Pose2d(p.getTranslation(), p.getRotation()/*new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2)))*/))), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("configtpath")), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("configt2path"))); + + } + + public static Command fourPeiceMid(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { + Pose2d p = PathPlannerPath.fromPathFile("ToMidNoteFromMid").getPreviewStartingHolonomicPose(); + return new SequentialCommandGroup( + new InstantCommand(() -> swerveDrive.resetPose(new Pose2d(p.getTranslation(), p.getRotation()/*new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2)))*/))), + new ParallelCommandGroup( + new RollIntakeIn(), // Begin rolling the intake in. + new PivotToTorus() // Pivot to the torus position. + ), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToMidNoteFromMid")), + new DriveToNote(swerveDrive, Intake.getInstance(), noteDetector), + new ParallelCommandGroup( + AutoBuilder.followPath(PathPlannerPath.fromPathFile("FromMidNoteToMid")), + new SequentialCommandGroup( + new StopIntake(), // Stop the intake mechanism. + new PivotTurtle(), // Pivot to a turtle position. + new RollIntakeOut(), // Roll the intake out. + new WaitCommand(0.15), // Wait for 0.15 seconds. + new OutIntake(), // Push the object out of the intake. + new StopIntake(), // Stop the intake again. + new RollIntakeInSlow(), // Slowly roll the intake in. + new InIntake(), // Bring the object into the intake. + new WaitCommand(0.1), // Wait for 0.1 seconds. + new StopIntake() + ) + ), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToSourceNoteFromMid")), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("FromSourceNoteToMid")), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToAMPNoteFromMidTurn")), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("FromAMPNoteToMidTurn"))); + } + + public static Command adaptableTest(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector) { + Pose2d p = PathPlannerPath.fromPathFile("ToAmpNoteFromAmpShort").getPreviewStartingHolonomicPose(); + // return new SequentialCommandGroup(null) + + Command midNote = new SequentialCommandGroup( + DriveToPose.getCommand(new Pose2d(new Translation2d(2.62, 6.64), new Rotation2d(Math.toRadians(76.62)))), + new ConditionalCommand( + new ParallelRaceGroup( + new IntakeNote(swerveDrive, intake, noteDetector), + new NotePresent(noteDetector, intake, swerveDrive, 6, false) + ), + new InstantCommand(), + () -> !noteDetector.checked(6) + ) + ); + + + Command midNoteInter = midNote.asProxy(); + + Command ampNote = new ParallelRaceGroup( + new IntakeNote(swerveDrive, intake, noteDetector), + new NotePresent(noteDetector, intake, swerveDrive, 5, false) + ); + + + return new SequentialCommandGroup( + new InstantCommand(() -> swerveDrive.resetPose(new Pose2d(p.getTranslation(), new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2)))), + // new InstantCommand(() -> System.out.println(new Rotation2d(p.getRotation().unaryMinus().getRadians()+Math.PI/2).getDegrees())), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToAmpNoteFromAmpShort")), + new ConditionalCommand( + ampNote, + midNote, + () -> !noteDetector.checked(5) + ), + new ConditionalCommand( + midNoteInter, + new InstantCommand(), + () -> noteDetector.checked(5) && !noteDetector.checked(6) + ), + AutoBuilder.followPath(PathPlannerPath.fromPathFile("ToAmpFromAmpNoteShort")) + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java index a78c21f..29ca0f1 100644 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/src/main/java/frc/robot/commands/ExampleCommand.java @@ -1,43 +1,43 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - *Q - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class ExampleCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ExampleSubsystem m_subsystem; + + /** + * Creates a new ExampleCommand. + *Q + * @param subsystem The subsystem used by this command. + */ + public ExampleCommand(ExampleSubsystem subsystem) { + m_subsystem = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/autoAligment/AutoAlignTrap.java b/src/main/java/frc/robot/commands/autoAligment/AutoAlignTrap.java index 580bc01..2c7223f 100644 --- a/src/main/java/frc/robot/commands/autoAligment/AutoAlignTrap.java +++ b/src/main/java/frc/robot/commands/autoAligment/AutoAlignTrap.java @@ -1,57 +1,57 @@ -// package frc.robot.commands.autoAligment; - -// import org.littletonrobotics.junction.Logger; - -// import com.pathplanner.lib.auto.AutoBuilder; - -// import com.pathplanner.lib.path.PathConstraints; - -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.CommandScheduler; -// import frc.robot.Constants; - -// public class AutoAlignTrap extends Command { -// private Command followPathHolonomic; -// private SwerveSubsystem swerveSubsystem; - -// public AutoAlignTrap(SwerveSubsystem swerveSubsystem) { -// this.swerveSubsystem = swerveSubsystem; -// } - -// // Called when the command is initially scheduled. -// @Override -// public void initialize() { -// Pose2d currentPose = swerveSubsystem.getPose(); -// Pose2d bestPose2d = Constants.FieldConstants.autoAlignTrapPose[0]; -// for (int i = 1; i < 3; i++) { -// double dist = calculateDistance(currentPose, Constants.FieldConstants.autoAlignTrapPose[i]); -// if (dist < calculateDistance(currentPose, bestPose2d)) { -// bestPose2d = Constants.FieldConstants.autoAlignTrapPose[i]; -// } -// } - -// followPathHolonomic = AutoBuilder.pathfindToPose(bestPose2d, new PathConstraints(Constants.Auton.MAX_SPEED, Constants.Auton.MAX_ACCELERATION, -// Constants.Auton.MAX_ANGULAR_VELO_RPS, Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED), 0, 0); - -// CommandScheduler.getInstance().schedule(followPathHolonomic); -// } - -// @Override -// public boolean isFinished() { -// Logger.recordOutput("AutoAlign/finished", followPathHolonomic.isFinished()); -// return followPathHolonomic.isFinished(); -// } - -// @Override -// public void end(boolean interrupted) { -// if (interrupted) { -// followPathHolonomic.cancel(); -// } -// } - -// private double calculateDistance(Pose2d p1, Pose2d p2) { -// return p1.getTranslation().getDistance(p2.getTranslation()); -// } -// } +// package frc.robot.commands.autoAligment; + +// import org.littletonrobotics.junction.Logger; + +// import com.pathplanner.lib.auto.AutoBuilder; + +// import com.pathplanner.lib.path.PathConstraints; + +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.CommandScheduler; +// import frc.robot.Constants; + +// public class AutoAlignTrap extends Command { +// private Command followPathHolonomic; +// private SwerveSubsystem swerveSubsystem; + +// public AutoAlignTrap(SwerveSubsystem swerveSubsystem) { +// this.swerveSubsystem = swerveSubsystem; +// } + +// // Called when the command is initially scheduled. +// @Override +// public void initialize() { +// Pose2d currentPose = swerveSubsystem.getPose(); +// Pose2d bestPose2d = Constants.FieldConstants.autoAlignTrapPose[0]; +// for (int i = 1; i < 3; i++) { +// double dist = calculateDistance(currentPose, Constants.FieldConstants.autoAlignTrapPose[i]); +// if (dist < calculateDistance(currentPose, bestPose2d)) { +// bestPose2d = Constants.FieldConstants.autoAlignTrapPose[i]; +// } +// } + +// followPathHolonomic = AutoBuilder.pathfindToPose(bestPose2d, new PathConstraints(Constants.Auton.MAX_SPEED, Constants.Auton.MAX_ACCELERATION, +// Constants.Auton.MAX_ANGULAR_VELO_RPS, Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED), 0, 0); + +// CommandScheduler.getInstance().schedule(followPathHolonomic); +// } + +// @Override +// public boolean isFinished() { +// Logger.recordOutput("AutoAlign/finished", followPathHolonomic.isFinished()); +// return followPathHolonomic.isFinished(); +// } + +// @Override +// public void end(boolean interrupted) { +// if (interrupted) { +// followPathHolonomic.cancel(); +// } +// } + +// private double calculateDistance(Pose2d p1, Pose2d p2) { +// return p1.getTranslation().getDistance(p2.getTranslation()); +// } +// } diff --git a/src/main/java/frc/robot/commands/autoAligment/DriveToPose.java b/src/main/java/frc/robot/commands/autoAligment/DriveToPose.java index 91717eb..abc2512 100644 --- a/src/main/java/frc/robot/commands/autoAligment/DriveToPose.java +++ b/src/main/java/frc/robot/commands/autoAligment/DriveToPose.java @@ -1,27 +1,27 @@ -package frc.robot.commands.autoAligment; - -import com.pathplanner.lib.auto.AutoBuilder; - -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.util.GeometryUtil; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; - -import frc.robot.Constants; - -public final class DriveToPose { - - // Builds a follow path holonomic path using field constants. - public static Command getCommand(Pose2d endGoal) { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - endGoal = GeometryUtil.flipFieldPose(endGoal); - } - - return AutoBuilder.pathfindToPose(endGoal, new PathConstraints(Constants.Auton.MAX_MODULE_SPEED, Constants.Auton.MAX_ACCELERATION, - Constants.Auton.MAX_ANGULAR_VELO_RPS, Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED), 0, 0); - } - -} +package frc.robot.commands.autoAligment; + +import com.pathplanner.lib.auto.AutoBuilder; + +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.util.GeometryUtil; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; + +import frc.robot.Constants; + +public final class DriveToPose { + + // Builds a follow path holonomic path using field constants. + public static Command getCommand(Pose2d endGoal) { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + endGoal = GeometryUtil.flipFieldPose(endGoal); + } + + return AutoBuilder.pathfindToPose(endGoal, new PathConstraints(Constants.Auton.MAX_MODULE_SPEED, Constants.Auton.MAX_ACCELERATION, + Constants.Auton.MAX_ANGULAR_VELO_RPS, Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED), 0, 0); + } + +} diff --git a/src/main/java/frc/robot/commands/autoAligment/LocalADStarAK.java b/src/main/java/frc/robot/commands/autoAligment/LocalADStarAK.java index 385ec97..e858f60 100644 --- a/src/main/java/frc/robot/commands/autoAligment/LocalADStarAK.java +++ b/src/main/java/frc/robot/commands/autoAligment/LocalADStarAK.java @@ -1,142 +1,142 @@ -package frc.robot.commands.autoAligment; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Translation2d; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPoint; -import com.pathplanner.lib.pathfinding.Pathfinder; -import com.pathplanner.lib.pathfinding.LocalADStar; -import java.util.ArrayList; -import java.util.Collections; -import java.util.List; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggableInputs; -public class LocalADStarAK implements Pathfinder { -private final ADStarIO io = new ADStarIO(); - -/** -* Get if a new path has been calculated since the last time a path was retrieved -* -* @return True if a new path is available -*/ -@Override -public boolean isNewPathAvailable() { - if (!Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } - - Logger.processInputs("LocalADStarAK", io); - return io.isNewPathAvailable; -} - -/** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ -@Override -public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (!Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); - } - - Logger.processInputs("LocalADStarAK", io); - if (io.currentPathPoints.isEmpty()) { - return null; - } - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); -} - -/** -* Set the start position to pathfind from -* -* @param startPosition Start position on the field. If this is within an obstacle it will be -* moved to the nearest non-obstacle node. -*/ -@Override -public void setStartPosition(Translation2d startPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } -} - -/** -* Set the goal position to pathfind to -* -* @param goalPosition Goal position on the field. f this is within an obstacle it will be moved -* to the nearest non-obstacle node. -*/ -@Override -public void setGoalPosition(Translation2d goalPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); - } -} - -/** -* Set the dynamic obstacles that should be avoided while pathfinding. -* -* @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents -* opposite corners of a bounding box. -* @param currentRobotPos The current position of the robot. This is needed to change the start -* position of the path to properly avoid obstacles -*/ -@Override -public void setDynamicObstacles(List> obs, Translation2d currentRobotPos) { - if (!Logger.hasReplaySource()) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } -} - -private static class ADStarIO implements LoggableInputs { - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); - - @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); - } - - @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); - List pathPoints = new ArrayList<>(); - - for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); - } - - currentPathPoints = pathPoints; - } - - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); - } - - public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } - } - } -} +package frc.robot.commands.autoAligment; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.Pathfinder; +import com.pathplanner.lib.pathfinding.LocalADStar; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; +public class LocalADStarAK implements Pathfinder { +private final ADStarIO io = new ADStarIO(); + +/** +* Get if a new path has been calculated since the last time a path was retrieved +* +* @return True if a new path is available +*/ +@Override +public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + return io.isNewPathAvailable; +} + +/** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ +@Override +public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + if (io.currentPathPoints.isEmpty()) { + return null; + } + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); +} + +/** +* Set the start position to pathfind from +* +* @param startPosition Start position on the field. If this is within an obstacle it will be +* moved to the nearest non-obstacle node. +*/ +@Override +public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } +} + +/** +* Set the goal position to pathfind to +* +* @param goalPosition Goal position on the field. f this is within an obstacle it will be moved +* to the nearest non-obstacle node. +*/ +@Override +public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } +} + +/** +* Set the dynamic obstacles that should be avoided while pathfinding. +* +* @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents +* opposite corners of a bounding box. +* @param currentRobotPos The current position of the robot. This is needed to change the start +* position of the path to properly avoid obstacles +*/ +@Override +public void setDynamicObstacles(List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } +} + +private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + List pathPoints = new ArrayList<>(); + + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/commands/autoAligment/NotePresent.java b/src/main/java/frc/robot/commands/autoAligment/NotePresent.java index b76c9a8..f78e7cc 100644 --- a/src/main/java/frc/robot/commands/autoAligment/NotePresent.java +++ b/src/main/java/frc/robot/commands/autoAligment/NotePresent.java @@ -1,87 +1,87 @@ -package frc.robot.commands.autoAligment; - -import org.littletonrobotics.junction.Logger; // For logging information. - -import edu.wpi.first.math.geometry.Pose2d; // 2D pose representation. -import edu.wpi.first.math.geometry.Rotation2d; // Representation of rotation. -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands. -import frc.robot.Constants; // Contains robot constants. -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem. -import frc.robot.subsystems.drivetrain.vision.NoteDetector; // Vision subsystem for detecting notes. -import frc.robot.subsystems.intakeComp.Intake; // Intake subsystem for handling objects. - -public class NotePresent extends Command { - - private final NoteDetector noteDetector; // Instance for note detection. - private final Intake intakeSubsystem; // Instance for controlling the intake. - private final SwerveDrive swerveSubsystem; // Instance for swerve drive control. - private final int index; // Index for identifying the note. - private final boolean useNotPresent; // Flag indicating whether to use 'not present' logic. - - // Constructor to initialize dependencies. - public NotePresent(NoteDetector noteDetector, Intake intakeSubsystem, SwerveDrive swerveSubsystem, int index, boolean useNotPresent) { - this.noteDetector = noteDetector; - this.intakeSubsystem = intakeSubsystem; - this.swerveSubsystem = swerveSubsystem; - this.index = index; - this.useNotPresent = useNotPresent; - } - - // Helper method to check if two poses are almost equal. - private static boolean almost_equal(Pose2d a, Pose2d b) { - double rot = Math.abs(Math.atan(a.getY() / a.getX()) - Math.atan(b.getY() / b.getX())); // Calculate rotation difference. - Logger.recordOutput("NotePresent/almostRot", rot); // Log rotation difference. - - // Check if the rotation and distance between poses are within thresholds. - return rot < Math.toRadians(20) && Math.abs(a.getTranslation().getDistance(b.getTranslation())) <= 1.5; // Degrees and meters. - } - - // Static method to determine if a note is present based on sensor input and robot position. - public static boolean notePresent(NoteDetector noteDetector, Intake intakeSubsystem, SwerveDrive swerveSubsystem, int index, boolean useNotPresent) { - Pose2d curr_pose = swerveSubsystem.getPose(); // Get current pose of the swerve drive. - Pose2d ideal = new Pose2d(Constants.FieldConstants.kNOTE_ARR[index].toTranslation2d(), new Rotation2d()).relativeTo(curr_pose); // Get ideal pose based on note's position. - Pose2d measured = new Pose2d(noteDetector.getNoteFieldRelativePose(), new Rotation2d()).relativeTo(curr_pose); // Get measured pose from note detector. - - // Calculate the rotation difference to the ideal pose. - double rotDelta = Math.abs(Math.PI + Math.atan2((Constants.FieldConstants.kNOTE_ARR[index].getY() - curr_pose.getTranslation().getY()), - (Constants.FieldConstants.kNOTE_ARR[index].getX() - curr_pose.getTranslation().getX()))) - curr_pose.getRotation().getRadians(); - - // Adjust rotation delta if it exceeds π radians. - if (rotDelta >= Math.PI) { - rotDelta = Math.PI * 2 - rotDelta; - } - - Logger.recordOutput("NotePresent/rotDeltaDeg", Math.toDegrees(rotDelta)); // Log rotation delta in degrees. - - double dist = curr_pose.getTranslation().getDistance(Constants.FieldConstants.kNOTE_ARR[index].toTranslation2d()); // Calculate distance to ideal. - Logger.recordOutput("NotePresent/distMeters", dist); // Log distance. - - // Determine if the note is present based on rotation, distance, and note detection state. - boolean present = rotDelta >= Math.toRadians(20) || dist >= 2 || (noteDetector.hasTargets() && almost_equal(ideal, measured)); - - // If the intake is active, set the present status to false. - if (intakeSubsystem.inIntake()) { - present = false; - } - - Logger.recordOutput("NotePresent", present); // Log the presence state. - - // Mark the note as checked if it’s not present. - if (!present) { - noteDetector.setCheked(index); - } - - // Return presence status based on the useNotPresent flag. - return useNotPresent ? present : present; - } - - @Override - public boolean isFinished() { // Determine if the command is finished. - return !notePresent(noteDetector, intakeSubsystem, swerveSubsystem, index, useNotPresent); - } - - @Override - public void end(boolean interrupted) { // Handle cleanup after command ends. - Logger.recordOutput("NotePresent/interrupted", interrupted); // Log if the command was interrupted. - } -} +package frc.robot.commands.autoAligment; + +import org.littletonrobotics.junction.Logger; // For logging information. + +import edu.wpi.first.math.geometry.Pose2d; // 2D pose representation. +import edu.wpi.first.math.geometry.Rotation2d; // Representation of rotation. +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands. +import frc.robot.Constants; // Contains robot constants. +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem. +import frc.robot.subsystems.drivetrain.vision.NoteDetector; // Vision subsystem for detecting notes. +import frc.robot.subsystems.intakeComp.Intake; // Intake subsystem for handling objects. + +public class NotePresent extends Command { + + private final NoteDetector noteDetector; // Instance for note detection. + private final Intake intakeSubsystem; // Instance for controlling the intake. + private final SwerveDrive swerveSubsystem; // Instance for swerve drive control. + private final int index; // Index for identifying the note. + private final boolean useNotPresent; // Flag indicating whether to use 'not present' logic. + + // Constructor to initialize dependencies. + public NotePresent(NoteDetector noteDetector, Intake intakeSubsystem, SwerveDrive swerveSubsystem, int index, boolean useNotPresent) { + this.noteDetector = noteDetector; + this.intakeSubsystem = intakeSubsystem; + this.swerveSubsystem = swerveSubsystem; + this.index = index; + this.useNotPresent = useNotPresent; + } + + // Helper method to check if two poses are almost equal. + private static boolean almost_equal(Pose2d a, Pose2d b) { + double rot = Math.abs(Math.atan(a.getY() / a.getX()) - Math.atan(b.getY() / b.getX())); // Calculate rotation difference. + Logger.recordOutput("NotePresent/almostRot", rot); // Log rotation difference. + + // Check if the rotation and distance between poses are within thresholds. + return rot < Math.toRadians(20) && Math.abs(a.getTranslation().getDistance(b.getTranslation())) <= 1.5; // Degrees and meters. + } + + // Static method to determine if a note is present based on sensor input and robot position. + public static boolean notePresent(NoteDetector noteDetector, Intake intakeSubsystem, SwerveDrive swerveSubsystem, int index, boolean useNotPresent) { + Pose2d curr_pose = swerveSubsystem.getPose(); // Get current pose of the swerve drive. + Pose2d ideal = new Pose2d(Constants.FieldConstants.kNOTE_ARR[index].toTranslation2d(), new Rotation2d()).relativeTo(curr_pose); // Get ideal pose based on note's position. + Pose2d measured = new Pose2d(noteDetector.getNoteFieldRelativePose(), new Rotation2d()).relativeTo(curr_pose); // Get measured pose from note detector. + + // Calculate the rotation difference to the ideal pose. + double rotDelta = Math.abs(Math.PI + Math.atan2((Constants.FieldConstants.kNOTE_ARR[index].getY() - curr_pose.getTranslation().getY()), + (Constants.FieldConstants.kNOTE_ARR[index].getX() - curr_pose.getTranslation().getX()))) - curr_pose.getRotation().getRadians(); + + // Adjust rotation delta if it exceeds π radians. + if (rotDelta >= Math.PI) { + rotDelta = Math.PI * 2 - rotDelta; + } + + Logger.recordOutput("NotePresent/rotDeltaDeg", Math.toDegrees(rotDelta)); // Log rotation delta in degrees. + + double dist = curr_pose.getTranslation().getDistance(Constants.FieldConstants.kNOTE_ARR[index].toTranslation2d()); // Calculate distance to ideal. + Logger.recordOutput("NotePresent/distMeters", dist); // Log distance. + + // Determine if the note is present based on rotation, distance, and note detection state. + boolean present = rotDelta >= Math.toRadians(20) || dist >= 2 || (noteDetector.hasTargets() && almost_equal(ideal, measured)); + + // If the intake is active, set the present status to false. + if (intakeSubsystem.inIntake()) { + present = false; + } + + Logger.recordOutput("NotePresent", present); // Log the presence state. + + // Mark the note as checked if it’s not present. + if (!present) { + noteDetector.setCheked(index); + } + + // Return presence status based on the useNotPresent flag. + return useNotPresent ? present : present; + } + + @Override + public boolean isFinished() { // Determine if the command is finished. + return !notePresent(noteDetector, intakeSubsystem, swerveSubsystem, index, useNotPresent); + } + + @Override + public void end(boolean interrupted) { // Handle cleanup after command ends. + Logger.recordOutput("NotePresent/interrupted", interrupted); // Log if the command was interrupted. + } +} diff --git a/src/main/java/frc/robot/commands/compositions/CancelIntakeNote.java b/src/main/java/frc/robot/commands/compositions/CancelIntakeNote.java index 4790985..195c1a4 100644 --- a/src/main/java/frc/robot/commands/compositions/CancelIntakeNote.java +++ b/src/main/java/frc/robot/commands/compositions/CancelIntakeNote.java @@ -1,56 +1,56 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.InstantCommand; // Command that runs once immediately. -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group for running commands in sequence. - -import frc.robot.commands.drivetrain.StopDrive; // Command to stop the drive subsystem. -import frc.robot.commands.intake.StopIntake; // Command to stop the intake subsystem. -import frc.robot.commands.pivot.PivotTurtle; // Command to adjust the pivot to a predefined position. -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem. - -public class CancelIntakeNote extends SequentialCommandGroup { - - // Constructor to initialize the CancelIntakeNote command group. - public CancelIntakeNote(SequentialCommandGroup c, SequentialCommandGroup c2, SwerveDrive swerveDrive) { - - // Check if both command groups are not null. - if(c != null && c2 != null) { - addCommands( - new StopDrive(swerveDrive), // Stop the drive subsystem. - new InstantCommand(()->c.cancel()), // Cancel the first command group. - new InstantCommand(()->c2.cancel()), // Cancel the second command group. - new StopIntake(), // Stop the intake subsystem. - new PivotTurtle() // Pivot the robot to a turtle position. - ); - } - - // If only the first command group is not null. - else if(c != null) { - addCommands( - new StopDrive(swerveDrive), // Stop the drive subsystem. - new InstantCommand(()->c.cancel()), // Cancel the first command group. - new StopIntake(), // Stop the intake subsystem. - new PivotTurtle() // Pivot the robot to a turtle position. - ); - } - - // If only the second command group is not null. - else if(c2 != null) { - addCommands( - new StopDrive(swerveDrive), // Stop the drive subsystem. - new InstantCommand(()->c2.cancel()), // Cancel the second command group. - new StopIntake(), // Stop the intake subsystem. - new PivotTurtle() // Pivot the robot to a turtle position. - ); - } - - // If both command groups are null, just stop drive and intake and pivot. - else { - addCommands( - new StopDrive(swerveDrive), // Stop the drive subsystem. - new StopIntake(), // Stop the intake subsystem. - new PivotTurtle() // Pivot the robot to a turtle position. - ); - } - } -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.InstantCommand; // Command that runs once immediately. +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group for running commands in sequence. + +import frc.robot.commands.drivetrain.StopDrive; // Command to stop the drive subsystem. +import frc.robot.commands.intake.StopIntake; // Command to stop the intake subsystem. +import frc.robot.commands.pivot.PivotTurtle; // Command to adjust the pivot to a predefined position. +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem. + +public class CancelIntakeNote extends SequentialCommandGroup { + + // Constructor to initialize the CancelIntakeNote command group. + public CancelIntakeNote(SequentialCommandGroup c, SequentialCommandGroup c2, SwerveDrive swerveDrive) { + + // Check if both command groups are not null. + if(c != null && c2 != null) { + addCommands( + // new StopDrive(swerveDrive), // Stop the drive subsystem. + new InstantCommand(()->c.cancel()), // Cancel the first command group. + new InstantCommand(()->c2.cancel()), // Cancel the second command group. + new StopIntake(), // Stop the intake subsystem. + new PivotTurtle() // Pivot the robot to a turtle position. + ); + } + + // If only the first command group is not null. + else if(c != null) { + addCommands( + // new StopDrive(swerveDrive), // Stop the drive subsystem. + new InstantCommand(()->c.cancel()), // Cancel the first command group. + new StopIntake(), // Stop the intake subsystem. + new PivotTurtle() // Pivot the robot to a turtle position. + ); + } + + // If only the second command group is not null. + else if(c2 != null) { + addCommands( + new StopDrive(swerveDrive), // Stop the drive subsystem. + new InstantCommand(()->c2.cancel()), // Cancel the second command group. + new StopIntake(), // Stop the intake subsystem. + new PivotTurtle() // Pivot the robot to a turtle position. + ); + } + + // If both command groups are null, just stop drive and intake and pivot. + else { + addCommands( + new StopDrive(swerveDrive), // Stop the drive subsystem. + new StopIntake(), // Stop the intake subsystem. + new PivotTurtle() // Pivot the robot to a turtle position. + ); + } + } +} diff --git a/src/main/java/frc/robot/commands/compositions/DriveToNote copy.java b/src/main/java/frc/robot/commands/compositions/DriveToNote copy.java index a03afd3..540a845 100644 --- a/src/main/java/frc/robot/commands/compositions/DriveToNote copy.java +++ b/src/main/java/frc/robot/commands/compositions/DriveToNote copy.java @@ -1,193 +1,193 @@ -// package frc.robot.commands.compositions; - -// import java.io.SequenceInputStream; -// import java.net.InetSocketAddress; - -// import org.littletonrobotics.junction.Logger; - -// import com.pathplanner.lib.path.PathPlannerTrajectory.State; - -// import edu.wpi.first.math.controller.HolonomicDriveController; -// import edu.wpi.first.math.controller.PIDController; -// import edu.wpi.first.math.controller.ProfiledPIDController; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.geometry.Translation3d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -// import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -// import edu.wpi.first.wpilibj2.command.WaitCommand; -// import frc.robot.Constants; -// import frc.robot.commands.intake.InIntake; -// import frc.robot.commands.intake.OutIntake; -// import frc.robot.commands.intake.RollIntakeIn; -// import frc.robot.commands.intake.RollIntakeInSlow; -// import frc.robot.commands.intake.RollIntakeOut; -// import frc.robot.commands.intake.StopIntake; -// import frc.robot.commands.pivot.PivotToTorus; -// import frc.robot.commands.pivot.PivotTurtle; -// import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -// import frc.robot.subsystems.drivetrain.vision.NoteDetector; -// import frc.robot.subsystems.intakeComp.Intake; -// import frc.robot.subsystems.pivotComp.Pivot; - -// public class DriveToNote extends Command { -// private final Intake intake; -// private final NoteDetector noteDetector; -// private final SwerveDrive swerveDrive; -// private Translation2d notePose = new Translation2d(); - -// private final PIDController m_translationalController; -// private final ProfiledPIDController m_rotationalController; -// // private final HolonomicDriveController controller; - -// private final Pivot pivot; -// private Rotation2d initialYaw = new Rotation2d(); -// private boolean over = false; - -// private SequentialCommandGroup startGroup; - -// public DriveToNote(SwerveDrive swerveDrive, Intake intakeSubsystem, NoteDetector noteDetector, Pivot pivot) { -// this.intake = intakeSubsystem; -// this.noteDetector = noteDetector; -// this.swerveDrive = swerveDrive; -// this.pivot = pivot; - -// switch (Constants.currentMode) { -// case SIM: -// m_translationalController = new PIDController(1.2, 0, 0); -// m_rotationalController = new ProfiledPIDController(1.2, 0, 0, -// new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, -// Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); -// break; - -// default: -// m_translationalController = new PIDController(1, 0, 0); -// m_rotationalController = new ProfiledPIDController(1, 0, 0, -// new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, -// Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); - -// m_rotationalController.setTolerance(Math.toRadians(5)); -// break; -// } -// // controller = new HolonomicDriveController(m_translationalController, m_translationalController, m_rotationalController); - -// addRequirements(swerveDrive); -// } - -// @Override -// public void initialize() { -// startGroup = new SequentialCommandGroup( -// new PivotToTorus(), -// new RollIntakeIn() -// ); -// startGroup.schedule(); -// notePose = noteDetector.getNoteFieldRelativePose(); -// initialYaw = swerveDrive.getPose().getRotation(); -// } - -// @Override -// public void execute() { -// Rotation2d robotYaw = swerveDrive.getPose().getRotation(); -// Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); -// intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); -// intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); - -// Translation3d intakeTranslation3d = new Translation3d( -// intakeTranslation2d.getX(), -// intakeTranslation2d.getY(), -// Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); - -// Logger.recordOutput("IntakeNoteCommand/notePresent", noteDetector.notePresent()); -// if (noteDetector.notePresent()) { -// ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); -// desiredSpeeds.vxMetersPerSecond = -// m_translationalController.calculate(intakeTranslation3d.getX(), noteDetector.getNoteFieldRelativePose().getX()); -// desiredSpeeds.vyMetersPerSecond = -// m_translationalController.calculate(intakeTranslation3d.getY(), noteDetector.getNoteFieldRelativePose().getY()); - -// double desiredRotation = Math.atan2( -// noteDetector.getNoteFieldRelativePose().getY() - swerveDrive.getPose().getY(), -// noteDetector.getNoteFieldRelativePose().getX() - swerveDrive.getPose().getX()); - -// initialYaw = new Rotation2d(desiredRotation); - -// desiredSpeeds.omegaRadiansPerSecond = -// m_rotationalController.calculate( -// robotYaw.getRadians(), desiredRotation); - - - -// Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", desiredRotation); -// Logger.recordOutput("IntakeNoteCommand/desiredSpeeds", desiredSpeeds); - -// swerveDrive.driveFieldRelative(desiredSpeeds); -// } -// else { -// ChassisSpeeds desiredSpeeds = new ChassisSpeeds(0, 0, 0); - -// if (over) { -// double goal = initialYaw.getRadians() - Math.toRadians(30); -// m_rotationalController.setGoal(goal); -// if (m_rotationalController.atGoal()) { -// over = false; -// } -// else { -// desiredSpeeds.omegaRadiansPerSecond = desiredSpeeds.omegaRadiansPerSecond = -// m_rotationalController.calculate(robotYaw.getRadians()); -// Logger.recordOutput("IntakeNoteCommand/desiredRotationRad",goal); -// } - -// } -// else { -// double goal = initialYaw.getRadians() + Math.toRadians(30); -// m_rotationalController.setGoal(goal); -// if (m_rotationalController.atGoal()) { -// over = true; -// } -// else { -// desiredSpeeds.omegaRadiansPerSecond = desiredSpeeds.omegaRadiansPerSecond = -// m_rotationalController.calculate(robotYaw.getRadians()); -// Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", goal); -// } -// } - -// swerveDrive.driveFieldRelative(desiredSpeeds); -// } - -// } - -// @Override -// public boolean isFinished() { -// return intake.inIntake(); -// } - -// @Override -// public void end(boolean interrupted) { - -// Logger.recordOutput("IntakeNoteCommand/interrupted", interrupted); - -// SequentialCommandGroup endGroup = new SequentialCommandGroup( -// new StopIntake(), -// new PivotTurtle(), -// new RollIntakeOut(), -// new WaitCommand(0.15), -// new OutIntake(), -// new StopIntake(), -// new RollIntakeInSlow(), -// new InIntake(), -// new WaitCommand(0.1), -// new StopIntake() -// ); - -// if (interrupted) { -// Command stop = new CancelIntakeNote(endGroup, null); -// stop.schedule(); -// return; -// } - -// endGroup.schedule(); -// } -// } - +// package frc.robot.commands.compositions; + +// import java.io.SequenceInputStream; +// import java.net.InetSocketAddress; + +// import org.littletonrobotics.junction.Logger; + +// import com.pathplanner.lib.path.PathPlannerTrajectory.State; + +// import edu.wpi.first.math.controller.HolonomicDriveController; +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.geometry.Translation3d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.commands.intake.InIntake; +// import frc.robot.commands.intake.OutIntake; +// import frc.robot.commands.intake.RollIntakeIn; +// import frc.robot.commands.intake.RollIntakeInSlow; +// import frc.robot.commands.intake.RollIntakeOut; +// import frc.robot.commands.intake.StopIntake; +// import frc.robot.commands.pivot.PivotToTorus; +// import frc.robot.commands.pivot.PivotTurtle; +// import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +// import frc.robot.subsystems.drivetrain.vision.NoteDetector; +// import frc.robot.subsystems.intakeComp.Intake; +// import frc.robot.subsystems.pivotComp.Pivot; + +// public class DriveToNote extends Command { +// private final Intake intake; +// private final NoteDetector noteDetector; +// private final SwerveDrive swerveDrive; +// private Translation2d notePose = new Translation2d(); + +// private final PIDController m_translationalController; +// private final ProfiledPIDController m_rotationalController; +// // private final HolonomicDriveController controller; + +// private final Pivot pivot; +// private Rotation2d initialYaw = new Rotation2d(); +// private boolean over = false; + +// private SequentialCommandGroup startGroup; + +// public DriveToNote(SwerveDrive swerveDrive, Intake intakeSubsystem, NoteDetector noteDetector, Pivot pivot) { +// this.intake = intakeSubsystem; +// this.noteDetector = noteDetector; +// this.swerveDrive = swerveDrive; +// this.pivot = pivot; + +// switch (Constants.currentMode) { +// case SIM: +// m_translationalController = new PIDController(1.2, 0, 0); +// m_rotationalController = new ProfiledPIDController(1.2, 0, 0, +// new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, +// Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); +// break; + +// default: +// m_translationalController = new PIDController(1, 0, 0); +// m_rotationalController = new ProfiledPIDController(1, 0, 0, +// new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, +// Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); + +// m_rotationalController.setTolerance(Math.toRadians(5)); +// break; +// } +// // controller = new HolonomicDriveController(m_translationalController, m_translationalController, m_rotationalController); + +// addRequirements(swerveDrive); +// } + +// @Override +// public void initialize() { +// startGroup = new SequentialCommandGroup( +// new PivotToTorus(), +// new RollIntakeIn() +// ); +// startGroup.schedule(); +// notePose = noteDetector.getNoteFieldRelativePose(); +// initialYaw = swerveDrive.getPose().getRotation(); +// } + +// @Override +// public void execute() { +// Rotation2d robotYaw = swerveDrive.getPose().getRotation(); +// Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); +// intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); +// intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); + +// Translation3d intakeTranslation3d = new Translation3d( +// intakeTranslation2d.getX(), +// intakeTranslation2d.getY(), +// Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); + +// Logger.recordOutput("IntakeNoteCommand/notePresent", noteDetector.notePresent()); +// if (noteDetector.notePresent()) { +// ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); +// desiredSpeeds.vxMetersPerSecond = +// m_translationalController.calculate(intakeTranslation3d.getX(), noteDetector.getNoteFieldRelativePose().getX()); +// desiredSpeeds.vyMetersPerSecond = +// m_translationalController.calculate(intakeTranslation3d.getY(), noteDetector.getNoteFieldRelativePose().getY()); + +// double desiredRotation = Math.atan2( +// noteDetector.getNoteFieldRelativePose().getY() - swerveDrive.getPose().getY(), +// noteDetector.getNoteFieldRelativePose().getX() - swerveDrive.getPose().getX()); + +// initialYaw = new Rotation2d(desiredRotation); + +// desiredSpeeds.omegaRadiansPerSecond = +// m_rotationalController.calculate( +// robotYaw.getRadians(), desiredRotation); + + + +// Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", desiredRotation); +// Logger.recordOutput("IntakeNoteCommand/desiredSpeeds", desiredSpeeds); + +// swerveDrive.driveFieldRelative(desiredSpeeds); +// } +// else { +// ChassisSpeeds desiredSpeeds = new ChassisSpeeds(0, 0, 0); + +// if (over) { +// double goal = initialYaw.getRadians() - Math.toRadians(30); +// m_rotationalController.setGoal(goal); +// if (m_rotationalController.atGoal()) { +// over = false; +// } +// else { +// desiredSpeeds.omegaRadiansPerSecond = desiredSpeeds.omegaRadiansPerSecond = +// m_rotationalController.calculate(robotYaw.getRadians()); +// Logger.recordOutput("IntakeNoteCommand/desiredRotationRad",goal); +// } + +// } +// else { +// double goal = initialYaw.getRadians() + Math.toRadians(30); +// m_rotationalController.setGoal(goal); +// if (m_rotationalController.atGoal()) { +// over = true; +// } +// else { +// desiredSpeeds.omegaRadiansPerSecond = desiredSpeeds.omegaRadiansPerSecond = +// m_rotationalController.calculate(robotYaw.getRadians()); +// Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", goal); +// } +// } + +// swerveDrive.driveFieldRelative(desiredSpeeds); +// } + +// } + +// @Override +// public boolean isFinished() { +// return intake.inIntake(); +// } + +// @Override +// public void end(boolean interrupted) { + +// Logger.recordOutput("IntakeNoteCommand/interrupted", interrupted); + +// SequentialCommandGroup endGroup = new SequentialCommandGroup( +// new StopIntake(), +// new PivotTurtle(), +// new RollIntakeOut(), +// new WaitCommand(0.15), +// new OutIntake(), +// new StopIntake(), +// new RollIntakeInSlow(), +// new InIntake(), +// new WaitCommand(0.1), +// new StopIntake() +// ); + +// if (interrupted) { +// Command stop = new CancelIntakeNote(endGroup, null); +// stop.schedule(); +// return; +// } + +// endGroup.schedule(); +// } +// } + diff --git a/src/main/java/frc/robot/commands/compositions/FeedAndHoldNote.java b/src/main/java/frc/robot/commands/compositions/FeedAndHoldNote.java index be2a7b2..9cc758d 100644 --- a/src/main/java/frc/robot/commands/compositions/FeedAndHoldNote.java +++ b/src/main/java/frc/robot/commands/compositions/FeedAndHoldNote.java @@ -1,33 +1,33 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; // Group for running commands in parallel. -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group for running commands in sequence. -import edu.wpi.first.wpilibj2.command.WaitCommand; // Command that pauses for a specified time. - -import frc.robot.commands.intake.OutIntake; // Command to push objects out of the intake. -import frc.robot.commands.intake.RollIntakeInSlow; // Command to slowly roll the intake in. -import frc.robot.commands.intake.StopIntake; // Command to stop the intake. -import frc.robot.commands.shooterComp.InShooter; // Command to shoot objects into the shooter. -import frc.robot.commands.shooterComp.ShooterHold; // Command to hold the shooter in place. -import frc.robot.commands.shooterComp.ShooterStop; // Command to stop the shooter. - -public class FeedAndHoldNote extends SequentialCommandGroup { - - // Constructor to initialize the FeedAndHoldNote command group. - public FeedAndHoldNote() { - addCommands( - new ShooterHold(), // Hold the shooter in its position. - new WaitCommand(0.48), // Wait for 0.48 seconds. - new RollIntakeInSlow(144), // Slowly roll the intake in for 144 units (e.g., rotations). - // Run the OutIntake and InShooter commands in parallel. - new ParallelCommandGroup(new OutIntake(), new InShooter()), - // Stop the intake and shooter simultaneously. - new ParallelCommandGroup(new StopIntake(), new ShooterStop()) - ); - } - - // Method to cancel the ongoing command group. - void Callcancel() { - this.cancel(); // Cancel the FeedAndHoldNote command group. - } -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; // Group for running commands in parallel. +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group for running commands in sequence. +import edu.wpi.first.wpilibj2.command.WaitCommand; // Command that pauses for a specified time. + +import frc.robot.commands.intake.OutIntake; // Command to push objects out of the intake. +import frc.robot.commands.intake.RollIntakeInSlow; // Command to slowly roll the intake in. +import frc.robot.commands.intake.StopIntake; // Command to stop the intake. +import frc.robot.commands.shooterComp.InShooter; // Command to shoot objects into the shooter. +import frc.robot.commands.shooterComp.ShooterHold; // Command to hold the shooter in place. +import frc.robot.commands.shooterComp.ShooterStop; // Command to stop the shooter. + +public class FeedAndHoldNote extends SequentialCommandGroup { + + // Constructor to initialize the FeedAndHoldNote command group. + public FeedAndHoldNote() { + addCommands( + new ShooterHold(), // Hold the shooter in its position. + new WaitCommand(0.48), // Wait for 0.48 seconds. + new RollIntakeInSlow(144), // Slowly roll the intake in for 144 units (e.g., rotations). + // Run the OutIntake and InShooter commands in parallel. + new ParallelCommandGroup(new OutIntake(), new InShooter()), + // Stop the intake and shooter simultaneously. + new ParallelCommandGroup(new StopIntake(), new ShooterStop()) + ); + } + + // Method to cancel the ongoing command group. + void Callcancel() { + this.cancel(); // Cancel the FeedAndHoldNote command group. + } +} diff --git a/src/main/java/frc/robot/commands/compositions/IntakeNote.java b/src/main/java/frc/robot/commands/compositions/IntakeNote.java index 8190f08..5cee747 100644 --- a/src/main/java/frc/robot/commands/compositions/IntakeNote.java +++ b/src/main/java/frc/robot/commands/compositions/IntakeNote.java @@ -1,62 +1,66 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; // Group for running commands in parallel. -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group for executing commands in sequence. -import edu.wpi.first.wpilibj2.command.WaitCommand; // Command that pauses execution for a specified time. - -import frc.robot.Constants; // Robot constants, including operation modes. -import frc.robot.commands.drivetrain.DriveToNote; // Command to drive the robot to a detected note. -import frc.robot.commands.intake.InIntake; // Command for bringing objects into the intake. -import frc.robot.commands.intake.OutIntake; // Command for pushing objects out of the intake. -import frc.robot.commands.intake.RollIntakeIn; // Command to roll the intake mechanism in. -import frc.robot.commands.intake.RollIntakeInSlow; // Command to slowly roll the intake mechanism in. -import frc.robot.commands.intake.RollIntakeOut; // Command to roll the intake mechanism out. -import frc.robot.commands.intake.StopIntake; // Command to stop the intake mechanism. -import frc.robot.commands.pivot.PivotToTorus; // Command to pivot to a position relative to a torus. -import frc.robot.commands.pivot.PivotTurtle; // Command to pivot to a turtle position. -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem for movement. -import frc.robot.subsystems.drivetrain.vision.NoteDetector; // Vision system for detecting notes. -import frc.robot.subsystems.intakeComp.Intake; - -public class IntakeNote extends SequentialCommandGroup { - - // Constructor to initialize the IntakeNote command group. - public IntakeNote(SwerveDrive swerveDrive, Intake intakeSubsystem, NoteDetector noteDetector) { - - // Check if the robot is in simulation mode. - if (Constants.currentMode == Constants.Mode.SIM) { - addCommands( - // Run rolling the intake in and pivoting in parallel. - new ParallelCommandGroup( - new RollIntakeIn(), // Begin rolling the intake in. - new PivotToTorus() // Pivot to the torus position. - ), - // Drive to the detected note. - new DriveToNote(swerveDrive, intakeSubsystem, noteDetector), - new StopIntake(), // Stop the intake mechanism. - new PivotTurtle() // Pivot to a turtle position. - ); - - } else { - addCommands( - // Run rolling the intake in and pivoting in parallel. - new ParallelCommandGroup( - new RollIntakeIn(), // Begin rolling the intake in. - new PivotToTorus() // Pivot to the torus position. - ), - // Drive to the detected note. - new DriveToNote(swerveDrive, intakeSubsystem, noteDetector), - new StopIntake(), // Stop the intake mechanism. - new PivotTurtle(), // Pivot to a turtle position. - new RollIntakeOut(), // Roll the intake out. - new WaitCommand(0.15), // Wait for 0.15 seconds. - new OutIntake(), // Push the object out of the intake. - new StopIntake(), // Stop the intake again. - new RollIntakeInSlow(), // Slowly roll the intake in. - new InIntake(), // Bring the object into the intake. - new WaitCommand(0.1), // Wait for 0.1 seconds. - new StopIntake() // Finally stop the intake. - ); - } - } -} +package frc.robot.commands.compositions; + +import java.time.Instant; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; // Group for running commands in parallel. +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group for executing commands in sequence. +import edu.wpi.first.wpilibj2.command.WaitCommand; // Command that pauses execution for a specified time. + +import frc.robot.Constants; // Robot constants, including operation modes. +import frc.robot.commands.drivetrain.DriveToNote; // Command to drive the robot to a detected note. +import frc.robot.commands.intake.InIntake; // Command for bringing objects into the intake. +import frc.robot.commands.intake.OutIntake; // Command for pushing objects out of the intake. +import frc.robot.commands.intake.RollIntakeIn; // Command to roll the intake mechanism in. +import frc.robot.commands.intake.RollIntakeInSlow; // Command to slowly roll the intake mechanism in. +import frc.robot.commands.intake.RollIntakeOut; // Command to roll the intake mechanism out. +import frc.robot.commands.intake.StopIntake; // Command to stop the intake mechanism. +import frc.robot.commands.pivot.PivotToTorus; // Command to pivot to a position relative to a torus. +import frc.robot.commands.pivot.PivotTurtle; // Command to pivot to a turtle position. +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem for movement. +import frc.robot.subsystems.drivetrain.vision.NoteDetector; // Vision system for detecting notes. +import frc.robot.subsystems.intakeComp.Intake; + +public class IntakeNote extends SequentialCommandGroup { + + // Constructor to initialize the IntakeNote command group. + public IntakeNote(SwerveDrive swerveDrive, Intake intakeSubsystem, NoteDetector noteDetector) { + + // Check if the robot is in simulation mode. + if (Constants.currentMode == Constants.Mode.SIM) { + addCommands( + // Run rolling the intake in and pivoting in parallel. + new ParallelCommandGroup( + new RollIntakeIn(), // Begin rolling the intake in. + new PivotToTorus() // Pivot to the torus position. + ), + // Drive to the detected note. + new DriveToNote(swerveDrive, intakeSubsystem, noteDetector), + new StopIntake(), // Stop the intake mechanism. + new PivotTurtle() // Pivot to a turtle position. + ); + + } else { + addCommands( + // Run rolling the intake in and pivoting in parallel. + new ParallelCommandGroup( + new RollIntakeIn(), // Begin rolling the intake in. + new PivotToTorus() // Pivot to the torus position. + ), + new WaitCommand(.5), + // Drive to the detected note. + new DriveToNote(swerveDrive, intakeSubsystem, noteDetector), + new StopIntake(), // Stop the intake mechanism. + new PivotTurtle(), // Pivot to a turtle position. + new RollIntakeOut(), // Roll the intake out. + new WaitCommand(0.15), // Wait for 0.15 seconds. + new OutIntake(), // Push the object out of the intake. + new StopIntake(), // Stop the intake again. + new RollIntakeInSlow(), // Slowly roll the intake in. + new InIntake(), // Bring the object into the intake. + new WaitCommand(0.1), // Wait for 0.1 seconds. + new StopIntake() // Finally stop the intake. + ); + } + } +} diff --git a/src/main/java/frc/robot/commands/compositions/IntakeNoteAuto.java b/src/main/java/frc/robot/commands/compositions/IntakeNoteAuto.java index d4b4bf5..0359df4 100644 --- a/src/main/java/frc/robot/commands/compositions/IntakeNoteAuto.java +++ b/src/main/java/frc/robot/commands/compositions/IntakeNoteAuto.java @@ -1,36 +1,36 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group to execute commands in sequence. -import edu.wpi.first.wpilibj2.command.WaitCommand; // Command to add a pause in the sequence. - -import frc.robot.commands.intake.InIntake; // Command to bring objects into the intake. -import frc.robot.commands.intake.OutIntake; // Command to push objects out of the intake. -import frc.robot.commands.intake.RollIntakeIn; // Command to roll the intake mechanism in. -import frc.robot.commands.intake.RollIntakeOut; // Command to roll the intake mechanism out. -import frc.robot.commands.intake.StopIntake; // Command to stop the intake. -import frc.robot.commands.pivot.PivotMidway; // Command to pivot to a midway position. -import frc.robot.commands.pivot.PivotToTorus; // Command to pivot to the torus position. -import frc.robot.commands.pivot.PivotTurtle; // Command to pivot to a turtle position. -import frc.robot.commands.intake.RollIntakeInSlow; // Command to slowly roll the intake in. - -public class IntakeNoteAuto extends SequentialCommandGroup { - - // Constructor to initialize the IntakeNoteAuto command group. - public IntakeNoteAuto() { - addCommands( - new StopIntake(), // Ensure intake is stopped before starting. - new PivotToTorus(), // Pivot to the torus position. - new RollIntakeIn(), // Begin rolling the intake mechanism in. - new InIntake(), // Activate intake to bring an object in. - new StopIntake(), // Stop the intake after bringing the object in. - new PivotMidway(), // Pivot to a midway position. - new RollIntakeOut(), // Roll the intake out. - new OutIntake(), // Push the object out of the intake. - new StopIntake(), // Stop the intake after pushing out. - new RollIntakeInSlow(), // Slowly roll the intake in. - new WaitCommand(0.1), // Wait for 0.1 seconds. - new StopIntake(), // Stop the intake. - new PivotTurtle() // Pivot to the turtle position. - ); - } -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Group to execute commands in sequence. +import edu.wpi.first.wpilibj2.command.WaitCommand; // Command to add a pause in the sequence. + +import frc.robot.commands.intake.InIntake; // Command to bring objects into the intake. +import frc.robot.commands.intake.OutIntake; // Command to push objects out of the intake. +import frc.robot.commands.intake.RollIntakeIn; // Command to roll the intake mechanism in. +import frc.robot.commands.intake.RollIntakeOut; // Command to roll the intake mechanism out. +import frc.robot.commands.intake.StopIntake; // Command to stop the intake. +import frc.robot.commands.pivot.PivotMidway; // Command to pivot to a midway position. +import frc.robot.commands.pivot.PivotToTorus; // Command to pivot to the torus position. +import frc.robot.commands.pivot.PivotTurtle; // Command to pivot to a turtle position. +import frc.robot.commands.intake.RollIntakeInSlow; // Command to slowly roll the intake in. + +public class IntakeNoteAuto extends SequentialCommandGroup { + + // Constructor to initialize the IntakeNoteAuto command group. + public IntakeNoteAuto() { + addCommands( + new StopIntake(), // Ensure intake is stopped before starting. + new PivotToTorus(), // Pivot to the torus position. + new RollIntakeIn(), // Begin rolling the intake mechanism in. + new InIntake(), // Activate intake to bring an object in. + new StopIntake(), // Stop the intake after bringing the object in. + new PivotMidway(), // Pivot to a midway position. + new RollIntakeOut(), // Roll the intake out. + new OutIntake(), // Push the object out of the intake. + new StopIntake(), // Stop the intake after pushing out. + new RollIntakeInSlow(), // Slowly roll the intake in. + new WaitCommand(0.1), // Wait for 0.1 seconds. + new StopIntake(), // Stop the intake. + new PivotTurtle() // Pivot to the turtle position. + ); + } +} diff --git a/src/main/java/frc/robot/commands/compositions/IntakeNoteManual.java b/src/main/java/frc/robot/commands/compositions/IntakeNoteManual.java index c21b3b4..6f7a6dd 100644 --- a/src/main/java/frc/robot/commands/compositions/IntakeNoteManual.java +++ b/src/main/java/frc/robot/commands/compositions/IntakeNoteManual.java @@ -1,42 +1,42 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.intake.InIntake; -import frc.robot.commands.intake.OutIntake; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.intake.RollIntakeInSlow; -import frc.robot.commands.intake.RollIntakeOut; -import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.pivot.PivotToTorus; -import frc.robot.commands.pivot.PivotTurtle; - -public class IntakeNoteManual extends SequentialCommandGroup { - public IntakeNoteManual() { - addCommands( - // new InstantCommand(()-> Intake.getInstance().setIntakeStatus(true)), - new PivotToTorus(), - new RollIntakeIn(), - new InIntake(), - new StopIntake(), - new PivotTurtle(), - new RollIntakeOut(), - new WaitCommand(0.15), - new OutIntake(), - new StopIntake(), - new RollIntakeInSlow(), - new InIntake(), - new WaitCommand(0.1), - new StopIntake() - // new InstantCommand(()-> Intake.getInstance().setIntakeStatus(false)) - // new LEDController(0.77) // green - ); - - } - - void Callcancel(){ - this.cancel(); - } - - +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.intake.InIntake; +import frc.robot.commands.intake.OutIntake; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.RollIntakeInSlow; +import frc.robot.commands.intake.RollIntakeOut; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.pivot.PivotToTorus; +import frc.robot.commands.pivot.PivotTurtle; + +public class IntakeNoteManual extends SequentialCommandGroup { + public IntakeNoteManual() { + addCommands( + // new InstantCommand(()-> Intake.getInstance().setIntakeStatus(true)), + new PivotToTorus(), + new RollIntakeIn(), + new InIntake(), + new StopIntake(), + new PivotTurtle(), + new RollIntakeOut(), + new WaitCommand(0.15), + new OutIntake(), + new StopIntake(), + new RollIntakeInSlow(), + new InIntake(), + new WaitCommand(0.1), + new StopIntake() + // new InstantCommand(()-> Intake.getInstance().setIntakeStatus(false)) + // new LEDController(0.77) // green + ); + + } + + void Callcancel(){ + this.cancel(); + } + + } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/compositions/PickUpTorus.java b/src/main/java/frc/robot/commands/compositions/PickUpTorus.java index abb1878..a4f4830 100644 --- a/src/main/java/frc/robot/commands/compositions/PickUpTorus.java +++ b/src/main/java/frc/robot/commands/compositions/PickUpTorus.java @@ -1,18 +1,18 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.intake.InIntake; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.pivot.PivotToTorus; - -public class PickUpTorus extends SequentialCommandGroup { - public PickUpTorus() { - //Race the rolling intake in and InIntake, rolling intake never finishes, therefore it only ends when the InIntake ends. - addCommands( - new ParallelCommandGroup(new ParallelRaceGroup(new RollIntakeIn(), new InIntake()), - new PivotToTorus()) - ); - } - } +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.intake.InIntake; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.pivot.PivotToTorus; + +public class PickUpTorus extends SequentialCommandGroup { + public PickUpTorus() { + //Race the rolling intake in and InIntake, rolling intake never finishes, therefore it only ends when the InIntake ends. + addCommands( + new ParallelCommandGroup(new ParallelRaceGroup(new RollIntakeIn(), new InIntake()), + new PivotToTorus()) + ); + } + } diff --git a/src/main/java/frc/robot/commands/compositions/ScoreAMP.java b/src/main/java/frc/robot/commands/compositions/ScoreAMP.java index 8b5bbe7..f9b800a 100644 --- a/src/main/java/frc/robot/commands/compositions/ScoreAMP.java +++ b/src/main/java/frc/robot/commands/compositions/ScoreAMP.java @@ -1,41 +1,41 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.elevator.MoveElevatorAMP; -import frc.robot.commands.elevator.MoveElevatorTurtle; -import frc.robot.commands.shooterComp.ShooterStop; -import frc.robot.commands.shooterComp.ShooterWindReverse; - -public class ScoreAMP extends SequentialCommandGroup { - - public ScoreAMP() { - addCommands( - // new ShooterStop(), // for what reason?? - // new MoveElevatorAMP(), - // new WaitCommand(0.6), - // new ShooterWindReverse(), - // new WaitCommand(0.8), - // new ShooterStop(), - // new MoveElevatorTurtle() - new ShooterWindReverse(), - new WaitCommand(0.8), - new ShooterStop() - ); - } - - // public ScoreAMP() { - // addCommands( - // new FeedAndHoldNote(), - // new ShooterStop(), - // new MoveElevatorAMP(), - // new WaitCommand(0.6), - // new ShooterWindReverse(), - // new WaitCommand(0.4), - // new ShooterStop(), - // new MoveElevatorTurtle() - // ); - // } - -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.elevator.MoveElevatorAMP; +import frc.robot.commands.elevator.MoveElevatorTurtle; +import frc.robot.commands.shooterComp.ShooterStop; +import frc.robot.commands.shooterComp.ShooterWindReverse; + +public class ScoreAMP extends SequentialCommandGroup { + + public ScoreAMP() { + addCommands( + // new ShooterStop(), // for what reason?? + // new MoveElevatorAMP(), + // new WaitCommand(0.6), + // new ShooterWindReverse(), + // new WaitCommand(0.8), + // new ShooterStop(), + // new MoveElevatorTurtle() + new ShooterWindReverse(), + new WaitCommand(0.8), + new ShooterStop() + ); + } + + // public ScoreAMP() { + // addCommands( + // new FeedAndHoldNote(), + // new ShooterStop(), + // new MoveElevatorAMP(), + // new WaitCommand(0.6), + // new ShooterWindReverse(), + // new WaitCommand(0.4), + // new ShooterStop(), + // new MoveElevatorTurtle() + // ); + // } + +} diff --git a/src/main/java/frc/robot/commands/compositions/ShootNote.java b/src/main/java/frc/robot/commands/compositions/ShootNote.java index fef62d6..7aac487 100644 --- a/src/main/java/frc/robot/commands/compositions/ShootNote.java +++ b/src/main/java/frc/robot/commands/compositions/ShootNote.java @@ -1,26 +1,26 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.shooterComp.ShooterStop; -import frc.robot.commands.shooterComp.ShooterWindup; - -/** - * A command group that represents shooting a note. - */ -public class ShootNote extends SequentialCommandGroup { - public ShootNote() { - addCommands( - new ShooterWindup(), - new WaitCommand(0.70), //Modify this later on. - new RollIntakeIn(), - new WaitCommand(0.4), - new StopIntake(), - new ShooterStop() - ); - } - - -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.shooterComp.ShooterStop; +import frc.robot.commands.shooterComp.ShooterWindup; + +/** + * A command group that represents shooting a note. + */ +public class ShootNote extends SequentialCommandGroup { + public ShootNote() { + addCommands( + new ShooterWindup(), + new WaitCommand(0.70), //Modify this later on. + new RollIntakeIn(), + new WaitCommand(0.4), + new StopIntake(), + new ShooterStop() + ); + } + + +} diff --git a/src/main/java/frc/robot/commands/compositions/ShootNoteAuto.java b/src/main/java/frc/robot/commands/compositions/ShootNoteAuto.java index 4c2fb1b..f83e759 100644 --- a/src/main/java/frc/robot/commands/compositions/ShootNoteAuto.java +++ b/src/main/java/frc/robot/commands/compositions/ShootNoteAuto.java @@ -1,22 +1,22 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.pivot.PivotTurtle; -import frc.robot.commands.shooterComp.ShooterStop; -import frc.robot.commands.shooterComp.ShooterWindup; - -public class ShootNoteAuto extends SequentialCommandGroup { - public ShootNoteAuto() { - addCommands( - // new WaitCommand(0.2), - // new PivotTurtle(), - // new WaitCommand(0.25), unneeded - new RollIntakeIn(), - new WaitCommand(0.22), - new StopIntake() - ); - } -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.pivot.PivotTurtle; +import frc.robot.commands.shooterComp.ShooterStop; +import frc.robot.commands.shooterComp.ShooterWindup; + +public class ShootNoteAuto extends SequentialCommandGroup { + public ShootNoteAuto() { + addCommands( + // new WaitCommand(0.2), + // new PivotTurtle(), + // new WaitCommand(0.25), unneeded + new RollIntakeIn(), + new WaitCommand(0.22), + new StopIntake() + ); + } +} diff --git a/src/main/java/frc/robot/commands/compositions/ShootNoteTele.java b/src/main/java/frc/robot/commands/compositions/ShootNoteTele.java index 2b1bf3b..ea5661a 100644 --- a/src/main/java/frc/robot/commands/compositions/ShootNoteTele.java +++ b/src/main/java/frc/robot/commands/compositions/ShootNoteTele.java @@ -1,21 +1,21 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.shooterComp.ShooterStop; - -public class ShootNoteTele extends SequentialCommandGroup { - public ShootNoteTele() { - addCommands( - //Modify this later on. - new RollIntakeIn(), - new WaitCommand(0.55), - new StopIntake(), - new ShooterStop() - ); - } - - -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.shooterComp.ShooterStop; + +public class ShootNoteTele extends SequentialCommandGroup { + public ShootNoteTele() { + addCommands( + //Modify this later on. + new RollIntakeIn(), + new WaitCommand(0.55), + new StopIntake(), + new ShooterStop() + ); + } + + +} diff --git a/src/main/java/frc/robot/commands/compositions/ShootNoteVariable.java b/src/main/java/frc/robot/commands/compositions/ShootNoteVariable.java index b613150..2242790 100644 --- a/src/main/java/frc/robot/commands/compositions/ShootNoteVariable.java +++ b/src/main/java/frc/robot/commands/compositions/ShootNoteVariable.java @@ -1,25 +1,25 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.pivot.PivotTurtle; -import frc.robot.commands.shooterComp.ShooterStop; -import frc.robot.commands.shooterComp.ShooterWindup; -import frc.robot.subsystems.shooterComp.Shooter;; - -public class ShootNoteVariable extends SequentialCommandGroup { - public ShootNoteVariable() { - addCommands( - // new WaitCommand(0.2), - // new PivotTurtle(),, - new WaitCommand(0.25), - new RollIntakeIn(), - new WaitCommand(0.3), - new StopIntake() - ); - } -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.pivot.PivotTurtle; +import frc.robot.commands.shooterComp.ShooterStop; +import frc.robot.commands.shooterComp.ShooterWindup; +import frc.robot.subsystems.shooterComp.Shooter;; + +public class ShootNoteVariable extends SequentialCommandGroup { + public ShootNoteVariable() { + addCommands( + // new WaitCommand(0.2), + // new PivotTurtle(),, + new WaitCommand(0.25), + new RollIntakeIn(), + new WaitCommand(0.3), + new StopIntake() + ); + } +} diff --git a/src/main/java/frc/robot/commands/compositions/ShootSpeaker.java b/src/main/java/frc/robot/commands/compositions/ShootSpeaker.java index 6faa73b..4875b56 100644 --- a/src/main/java/frc/robot/commands/compositions/ShootSpeaker.java +++ b/src/main/java/frc/robot/commands/compositions/ShootSpeaker.java @@ -1,27 +1,27 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; - -import frc.robot.commands.shooterComp.ShooterStop; -// import frc.robot.commands.shooter.AlignWithTargetPoint; -import frc.robot.commands.intake.RollIntakeIn; -import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.intake.InIntake; -import frc.robot.commands.elevator.MoveElevatorTurtle; -import frc.robot.commands.pivot.PivotTurtle; - -// import frc.robot.subsystems.limelight.PoseLimelight; - -public class ShootSpeaker extends SequentialCommandGroup { - public ShootSpeaker(/*PoseLimelight visionSubsystem SwerveSubsystem swerveSubsystem*/) { - addCommands( - // new ParallelRaceGroup(new AlignWithTargetPoint(swerveSubsystem, visionSubsystem), new ShooterWindup(RebelUtil.flywheelSpeed)), - new ParallelCommandGroup(new MoveElevatorTurtle(), new PivotTurtle()), - new ParallelRaceGroup(new RollIntakeIn(), new InIntake()), - new StopIntake(), - new ShooterStop() - ); - } -} +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; + +import frc.robot.commands.shooterComp.ShooterStop; +// import frc.robot.commands.shooter.AlignWithTargetPoint; +import frc.robot.commands.intake.RollIntakeIn; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.intake.InIntake; +import frc.robot.commands.elevator.MoveElevatorTurtle; +import frc.robot.commands.pivot.PivotTurtle; + +// import frc.robot.subsystems.limelight.PoseLimelight; + +public class ShootSpeaker extends SequentialCommandGroup { + public ShootSpeaker(/*PoseLimelight visionSubsystem SwerveSubsystem swerveSubsystem*/) { + addCommands( + // new ParallelRaceGroup(new AlignWithTargetPoint(swerveSubsystem, visionSubsystem), new ShooterWindup(RebelUtil.flywheelSpeed)), + new ParallelCommandGroup(new MoveElevatorTurtle(), new PivotTurtle()), + new ParallelRaceGroup(new RollIntakeIn(), new InIntake()), + new StopIntake(), + new ShooterStop() + ); + } +} diff --git a/src/main/java/frc/robot/commands/compositions/ShooterTest.java b/src/main/java/frc/robot/commands/compositions/ShooterTest.java index 551a07b..6e0364a 100644 --- a/src/main/java/frc/robot/commands/compositions/ShooterTest.java +++ b/src/main/java/frc/robot/commands/compositions/ShooterTest.java @@ -1,22 +1,22 @@ -package frc.robot.commands.compositions; - -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.shooterComp.ShooterHold; -import frc.robot.commands.shooterComp.ShooterStop; - -/** - * This class represents a test sequence for the shooter mechanism. - * It consists of a series of commands executed sequentially. - */ -public class ShooterTest extends SequentialCommandGroup { - public ShooterTest() { - addCommands( - new ParallelRaceGroup( - new ShooterHold(), - new WaitCommand(0.2)), - new ShooterStop() - ); - } +package frc.robot.commands.compositions; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.shooterComp.ShooterHold; +import frc.robot.commands.shooterComp.ShooterStop; + +/** + * This class represents a test sequence for the shooter mechanism. + * It consists of a series of commands executed sequentially. + */ +public class ShooterTest extends SequentialCommandGroup { + public ShooterTest() { + addCommands( + new ParallelRaceGroup( + new ShooterHold(), + new WaitCommand(0.2)), + new ShooterStop() + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drivetrain/AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/drivetrain/AbsoluteFieldDrive.java index ae6f918..d0fa058 100644 --- a/src/main/java/frc/robot/commands/drivetrain/AbsoluteFieldDrive.java +++ b/src/main/java/frc/robot/commands/drivetrain/AbsoluteFieldDrive.java @@ -1,69 +1,71 @@ -package frc.robot.commands.drivetrain; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; // Class to handle chassis speed calculations. -import edu.wpi.first.wpilibj.DriverStation; // Driver station for accessing driver settings like alliance. -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands. -import frc.robot.Constants; // Constants including drivetrain specifications. -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem for robot movement. - -public class AbsoluteFieldDrive extends Command { - - private final SwerveDrive swerve; // Reference to the swerve drive subsystem. - private final DoubleSupplier vX, vY, heading; // Supplier functions for velocity inputs and heading. - private int invert = 1; // Variable to invert direction based on alliance color. - - // Constructor to initialize the AbsoluteFieldDrive command. - public AbsoluteFieldDrive(SwerveDrive swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.heading = heading; - - addRequirements(swerve); // Specify that this command requires the swerve subsystem. - } - - // Called when the command is initialized. - @Override - public void initialize() { - boolean isRed = false; - var alliance = DriverStation.getAlliance(); // Get the current alliance. - - // Check if the alliance is Red and set invert accordingly. - if (alliance.isPresent()) { - isRed = alliance.get() == DriverStation.Alliance.Red; - } - - // Invert drive direction if the robot is on the Red alliance. - if (isRed) { - invert = -1; - } - } - - // Called repeatedly while the command is scheduled. - @Override - public void execute() { - // Calculate speeds based on input and max speed constants. - ChassisSpeeds speeds = new ChassisSpeeds( - vX.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND * invert, - vY.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND * invert, - heading.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND * invert - ); - - // Drive the swerve drive subsystem with field-relative speeds. - swerve.driveFieldRelative(speeds); - } - - // Called when the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - // Cleanup or reset logic can be added here if necessary. - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; // This command runs indefinitely until interrupted. - } -} +package frc.robot.commands.drivetrain; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; // Class to handle chassis speed calculations. +import edu.wpi.first.wpilibj.DriverStation; // Driver station for accessing driver settings like alliance. +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands. +import frc.robot.Constants; // Constants including drivetrain specifications. +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; // Swerve drive subsystem for robot movement. + +public class AbsoluteFieldDrive extends Command { + + private final SwerveDrive swerve; // Reference to the swerve drive subsystem. + private final DoubleSupplier vX, vY, heading; // Supplier functions for velocity inputs and heading. + private int invert = 1; // Variable to invert direction based on alliance color. + + // Constructor to initialize the AbsoluteFieldDrive command. + public AbsoluteFieldDrive(SwerveDrive swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.heading = heading; + + addRequirements(swerve); // Specify that this command requires the swerve subsystem. + } + + // Called when the command is initialized. + @Override + public void initialize() { + boolean isRed = false; + var alliance = DriverStation.getAlliance(); // Get the current alliance. + + // Check if the alliance is Red and set invert accordingly. + if (alliance.isPresent()) { + isRed = alliance.get() == DriverStation.Alliance.Red; + } + + // Invert drive direction if the robot is on the Red alliance. + if (isRed) { + invert = -1; + } + } + + // Called repeatedly while the command is scheduled. + @Override + public void execute() { + // Calculate speeds based on input and max speed constants. + ChassisSpeeds speeds = new ChassisSpeeds( + vX.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND * invert, + vY.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND * invert, + heading.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND * invert + ); + + // Drive the swerve drive subsystem with field-relative speeds. + if (!DriveToNote.isRunning) { + swerve.driveFieldRelative(speeds); + } + } + + // Called when the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + // Cleanup or reset logic can be added here if necessary. + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; // This command runs indefinitely until interrupted. + } +} diff --git a/src/main/java/frc/robot/commands/drivetrain/DriveToNote.java b/src/main/java/frc/robot/commands/drivetrain/DriveToNote.java index 742633c..959b225 100644 --- a/src/main/java/frc/robot/commands/drivetrain/DriveToNote.java +++ b/src/main/java/frc/robot/commands/drivetrain/DriveToNote.java @@ -1,164 +1,196 @@ -package frc.robot.commands.drivetrain; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.lib.util.RebelUtil; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import frc.robot.subsystems.drivetrain.vision.NoteDetector; -import frc.robot.subsystems.intakeComp.Intake; - -// Command for driving the robot towards a detected note -public class DriveToNote extends Command { - private final Intake intake; // Reference to the intake subsystem - private final NoteDetector noteDetector; // Reference to the note detector subsystem - private final SwerveDrive swerveDrive; // Reference to the swerve drive subsystem - - // PID controllers for translational and rotational control - private final PIDController m_translationalController; - private final ProfiledPIDController m_rotationalController; - - private Rotation2d initialYaw = new Rotation2d(); - private boolean over = false; - private double drivingSpeedMps = 1; - - // Constructor for the DriveToNote command - public DriveToNote(SwerveDrive swerveDrive, Intake intakeSubsystem, NoteDetector noteDetector) { - this.intake = intakeSubsystem; // Initialize intake reference - this.noteDetector = noteDetector; // Initialize note detector reference - this.swerveDrive = swerveDrive; // Initialize swerve drive reference - - // Configure PID controllers based on current mode (SIM or REAL) - switch (Constants.currentMode) { - case SIM: - m_translationalController = new PIDController(1.2, 0, 0); // Simulated translational controller gains - m_rotationalController = new ProfiledPIDController(2, 0, 0, - new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, - Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); // Simulated rotational controller - break; - - default: - m_translationalController = new PIDController(1.6, 0, 0); - m_rotationalController = new ProfiledPIDController(1.2, 0, 0, - new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, - Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); // Real rotational controller - - m_rotationalController.setTolerance(Math.toRadians(5)); // Set tolerance for rotational controller - break; - } - m_rotationalController.enableContinuousInput(0, Math.PI * 2); // Enable continuous input for rotation - addRequirements(swerveDrive); // Declare that this command requires the swerve drive subsystem - } - - // Called when the command is initially scheduled - @Override - public void initialize() { - initialYaw = swerveDrive.getPose().getRotation(); - Logger.recordOutput("IntakeNOteCommand/initialYaw", initialYaw); - Logger.recordOutput("IntakeNoteCommand/end", false); - - double currentSpeed = Math.sqrt(Math.pow(swerveDrive.getMeasuredFeildRelativeSpeeds().vxMetersPerSecond,2) + - Math.pow(swerveDrive.getMeasuredFeildRelativeSpeeds().vyMetersPerSecond,2)); - - drivingSpeedMps = RebelUtil.constrain(currentSpeed, 1, 1.6); - Logger.recordOutput("IntakeNoteCommand/drivingSpeedMps", drivingSpeedMps); - } - - // Called repeatedly while the command is scheduled - @Override - public void execute() { - Rotation2d robotYaw = swerveDrive.getPose().getRotation(); // Get current robot yaw - Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); - intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); // Rotate intake translation by robot's yaw - intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); // Add robot's translation - - Translation3d intakeTranslation3d = new Translation3d( - intakeTranslation2d.getX(), - intakeTranslation2d.getY(), - Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); // Create a 3D translation for the intake - - Translation2d noteTranslation2d = noteDetector.getNoteFieldRelativePose(); // Get position of detected note - - if (noteDetector.hasTargets()) { // Check if any targets are detected - ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); // Initialize desired speeds object - - // Calculate desired translational speeds using PID controller - desiredSpeeds.vxMetersPerSecond = - m_translationalController.calculate(intakeTranslation3d.getX(), noteTranslation2d.getX()); - desiredSpeeds.vyMetersPerSecond = - m_translationalController.calculate(intakeTranslation3d.getY(), noteTranslation2d.getY()); - - double scaler = Math.sqrt(Math.pow(drivingSpeedMps,2)/(Math.pow(desiredSpeeds.vxMetersPerSecond,2) + Math.pow(desiredSpeeds.vyMetersPerSecond,2))); - desiredSpeeds.vxMetersPerSecond *= scaler; - desiredSpeeds.vyMetersPerSecond *= scaler; - - double desiredRotation = Math.atan2( - swerveDrive.getPose().getY() - noteTranslation2d.getY(), - swerveDrive.getPose().getX() - noteTranslation2d.getX()); // Calculate desired rotation towards the note - - initialYaw = new Rotation2d(desiredRotation); // Update initial yaw to desired rotation - - desiredSpeeds.omegaRadiansPerSecond = - m_rotationalController.calculate( - robotYaw.getRadians(), desiredRotation); // Calculate rotational speed using PID controller - - Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", desiredRotation); // Log desired rotation in radians - Logger.recordOutput("IntakeNoteCommand/desiredSpeeds", desiredSpeeds); // Log desired speeds - - swerveDrive.driveFieldRelative(desiredSpeeds); // Command swerve drive to move with calculated speeds - } - else { - ChassisSpeeds desiredSpeeds = new ChassisSpeeds(0, 0, 0); // Stop if no targets are detected - - if (over) { // If over flag is true, rotate back by -30 degrees from initial yaw - double goal = initialYaw.getRadians() - Math.toRadians(30); - m_rotationalController.setGoal(goal); // Set goal for rotational controller - - if (m_rotationalController.atGoal()) { - over = false; // Toggle over flag if goal is reached - } - else { - desiredSpeeds.omegaRadiansPerSecond = - m_rotationalController.calculate(robotYaw.getRadians()); // Calculate rotational speed towards goal - Logger.recordOutput("IntakeNoteCommand/desiredRotationRad",goal); // Log goal rotation in radians - } - } - else { // If over flag is false, rotate forward by +30 degrees from initial yaw - double goal = initialYaw.getRadians() + Math.toRadians(30); - m_rotationalController.setGoal(goal); // Set goal for rotational controller - - if (m_rotationalController.atGoal()) { - over = true; // Toggle over flag if goal is reached - } - else { - desiredSpeeds.omegaRadiansPerSecond = - m_rotationalController.calculate(robotYaw.getRadians()); // Calculate rotational speed towards goal - Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", goal); // Log goal rotation in radians - } - } - - swerveDrive.driveFieldRelative(desiredSpeeds); // Command swerve drive to move with calculated speeds while rotating - } - - } - - @Override - public boolean isFinished() { - return intake.inIntake(); // Command finishes when the intake system indicates it has successfully ingested a note - } - - @Override - public void end(boolean interrupted) { - Logger.recordOutput("IntakeNoteCommand/driveInterrupted", interrupted); // Log whether command was interrupted or finished normally - Logger.recordOutput("IntakeNoteCommand/end", true); // Log command end status - swerveDrive.driveFieldRelative(new ChassisSpeeds(0, 0, 0)); // Stop all movement of the swerve drive at command end - } -} +package frc.robot.commands.drivetrain; + +import org.littletonrobotics.junction.Logger; + +import com.pathplanner.lib.util.GeometryUtil; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.lib.util.RebelUtil; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import frc.robot.subsystems.drivetrain.vision.NoteDetector; +import frc.robot.subsystems.intakeComp.Intake; + +// Command for driving the robot towards a detected note +public class DriveToNote extends Command { + private final Intake intake; // Reference to the intake subsystem + private final NoteDetector noteDetector; // Reference to the note detector subsystem + private final SwerveDrive swerveDrive; // Reference to the swerve drive subsystem + + // PID controllers for translational and rotational control + private final PIDController m_translationalController; + private final ProfiledPIDController m_rotationalController; + + private Rotation2d initialYaw = new Rotation2d(); + private boolean over = false; + private double drivingSpeedMps = 1; + private boolean initalReset = false; + private Translation2d intialNotePose = null; + + public static boolean isRunning = false; + + // Constructor for the DriveToNote command + public DriveToNote(SwerveDrive swerveDrive, Intake intakeSubsystem, NoteDetector noteDetector) { + this.intake = intakeSubsystem; // Initialize intake reference + this.noteDetector = noteDetector; // Initialize note detector reference + this.swerveDrive = swerveDrive; // Initialize swerve drive reference + + // Configure PID controllers based on current mode (SIM or REAL) + switch (Constants.currentMode) { + case SIM: + m_translationalController = new PIDController(1.2, 0, 0); // Simulated translational controller gains + m_rotationalController = new ProfiledPIDController(2, 0, 0, + new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, + Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); // Simulated rotational controller + break; + + default: + m_translationalController = new PIDController(1.6, 0, 0); + m_rotationalController = new ProfiledPIDController(2, 0, 0.1, + new Constraints(Constants.Auton.MAX_ANGULAR_VELO_RPS * 2 * Math.PI, + Constants.Auton.MAX_ANGULAR_ACCEL_RPS_SQUARED * 2 * Math.PI)); // Real rotational controller + + m_rotationalController.setTolerance(Math.toRadians(5)); // Set tolerance for rotational controller + break; + } + m_rotationalController.enableContinuousInput(0, Math.PI * 2); // Enable continuous input for rotation + // addRequirements(swerveDrive); // Declare that this command requires the swerve drive subsystem + } + + // Called when the command is initially scheduled + @Override + public void initialize() { + intialNotePose = null; + initalReset = false; + initialYaw = swerveDrive.getPose().getRotation(); + Logger.recordOutput("IntakeNoteCommand/initialYaw", initialYaw); + Logger.recordOutput("IntakeNoteCommand/end", false); + + double currentSpeed = Math.sqrt(Math.pow(swerveDrive.getMeasuredFeildRelativeSpeeds().vxMetersPerSecond,2) + + Math.pow(swerveDrive.getMeasuredFeildRelativeSpeeds().vyMetersPerSecond,2)); + + drivingSpeedMps = RebelUtil.constrain(currentSpeed, 1, 1.6); + Logger.recordOutput("IntakeNoteCommand/drivingSpeedMps", drivingSpeedMps); + } + + // Called repeatedly while the command is scheduled + @Override + public void execute() { + Rotation2d robotYaw = swerveDrive.getPose().getRotation(); // Get current robot yaw + Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); + intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); // Rotate intake translation by robot's yaw + intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); // Add robot's translation + + Translation3d intakeTranslation3d = new Translation3d( + intakeTranslation2d.getX(), + intakeTranslation2d.getY(), + Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); // Create a 3D translation for the intake + + Translation2d noteTranslation2d = noteDetector.getNoteFieldRelativePose(); // Get position of detected note + if (noteDetector.hasTargets() && intialNotePose == null) { + intialNotePose = noteTranslation2d; + } + + if (intialNotePose != null && noteDetector.getNoteFieldRelativePose().getDistance(intialNotePose) <= 0.6 && + (noteDetector.hasTargets() || (noteDetector.getNoteRobotRelativePose().getX() <= 1.1 && noteDetector.getNoteRobotRelativePose().getX() > .6))) { // Check if any targets are detected + + isRunning = true; + + initalReset = false; + Logger.recordOutput("distNoteFromInital", noteDetector.getNoteFieldRelativePose().getDistance(intialNotePose)); + //penis + + ChassisSpeeds desiredSpeeds = new ChassisSpeeds(0,0,0); // Initialize desired speeds object + + // Calculate desired translational speeds using PID controller + desiredSpeeds.vxMetersPerSecond = + m_translationalController.calculate(intakeTranslation3d.getX(), noteTranslation2d.getX()); + desiredSpeeds.vyMetersPerSecond = + m_translationalController.calculate(intakeTranslation3d.getY(), noteTranslation2d.getY()); + + desiredSpeeds = RebelUtil.scaleSpeeds(drivingSpeedMps, desiredSpeeds); + + double desiredRotation = Math.atan2( + swerveDrive.getPose().getY() - noteTranslation2d.getY(), + swerveDrive.getPose().getX() - noteTranslation2d.getX()); // Calculate desired rotation towards the note + + initialYaw = new Rotation2d(desiredRotation); // Update initial yaw to desired rotation + + desiredSpeeds.omegaRadiansPerSecond = + m_rotationalController.calculate( + robotYaw.getRadians(), initialYaw.getRadians()); // Calculate rotational speed using PID controller + + Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", desiredRotation); // Log desired rotation in radians + Logger.recordOutput("IntakeNoteCommand/desiredSpeeds", desiredSpeeds); // Log desired speeds + + swerveDrive.driveFieldRelative(desiredSpeeds); // Command swerve drive to move with calculated speeds + } + else if (DriverStation.isAutonomous()) { + isRunning = true; + + if (!initalReset) { + initialYaw = swerveDrive.getPose().getRotation(); + initalReset = true; + } + ChassisSpeeds desiredSpeeds = new ChassisSpeeds(0, 0, 0); // Stop if no targets are detected + + if (over) { // If over flag is true, rotate back by -30 degrees from initial yaw + double goal = initialYaw.getRadians() - Math.toRadians(30); + m_rotationalController.setGoal(goal); // Set goal for rotational controller + + if (m_rotationalController.atGoal()) { + over = false; // Toggle over flag if goal is reached + } + else { + desiredSpeeds.omegaRadiansPerSecond = + m_rotationalController.calculate(robotYaw.getRadians()); // Calculate rotational speed towards goal + Logger.recordOutput("IntakeNoteCommand/desiredRotationRad",goal); // Log goal rotation in radians + } + swerveDrive.driveFieldRelative(desiredSpeeds); // Command swerve drive to move with calculated speeds + } + else { // If over flag is false, rotate forward by +30 degrees from initial yaw + double goal = initialYaw.getRadians() + Math.toRadians(30); + m_rotationalController.setGoal(goal); // Set goal for rotational controller + + if (m_rotationalController.atGoal()) { + over = true; // Toggle over flag if goal is reached + } + desiredSpeeds.omegaRadiansPerSecond = + m_rotationalController.calculate(robotYaw.getRadians()); // Calculate rotational speed towards goal + Logger.recordOutput("IntakeNoteCommand/desiredRotationRad", goal); // Log goal rotation in radians + swerveDrive.driveFieldRelative(desiredSpeeds); // Command swerve drive to move with calculated speeds while rotating + } + + } + else { + isRunning = false; + } + + } + + @Override + public boolean isFinished() { + return intake.inIntake(); // Command finishes when the intake system indicates it has successfully ingested a note + } + + @Override + public void end(boolean interrupted) { + Logger.recordOutput("IntakeNoteCommand/driveInterrupted", interrupted); // Log whether command was interrupted or finished normally + Logger.recordOutput("IntakeNoteCommand/end", true); // Log command end status + // swerveDrive.driveFieldRelative(new ChassisSpeeds(0, 0, 0)); // Stop all movement of the swerve drive at command end + isRunning = false; + if (!interrupted) { + swerveDrive.driveFieldRelative(new ChassisSpeeds(0,0,0)); + } + + } +} diff --git a/src/main/java/frc/robot/commands/drivetrain/StopDrive.java b/src/main/java/frc/robot/commands/drivetrain/StopDrive.java index 5859951..5784071 100644 --- a/src/main/java/frc/robot/commands/drivetrain/StopDrive.java +++ b/src/main/java/frc/robot/commands/drivetrain/StopDrive.java @@ -1,41 +1,41 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.drivetrain; - -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class StopDrive extends Command { - -private final SwerveDrive swerveDrive; - - public StopDrive(SwerveDrive swerveDrive) { - this.swerveDrive = swerveDrive; - - addRequirements(swerveDrive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - swerveDrive.driveFieldRelative(new ChassisSpeeds(0,0,0)); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.drivetrain; + +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class StopDrive extends Command { + +private final SwerveDrive swerveDrive; + + public StopDrive(SwerveDrive swerveDrive) { + this.swerveDrive = swerveDrive; + + addRequirements(swerveDrive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + swerveDrive.driveFieldRelative(new ChassisSpeeds(0,0,0)); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorControlRaw.java b/src/main/java/frc/robot/commands/elevator/ElevatorControlRaw.java index 23587bd..95a9530 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorControlRaw.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorControlRaw.java @@ -1,35 +1,35 @@ -package frc.robot.commands.elevator; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.lib.input.XboxController; - - -public class ElevatorControlRaw extends Command{ - Elevator ElevatorSubsystem = Elevator.getInstance(); - XboxController controller; - /** - * This is really for testing, and a raw controller for the operator to use whenever they MUST, not recommend for use when other commands are working - * @param Elevator - * @param m_controller - */ - public ElevatorControlRaw(XboxController m_controller){ - controller = m_controller; - } - - @Override - public void execute(){ - double height = this.ElevatorSubsystem.getShooterHeightMeters(); - if (controller.getLeftY() > 0.04 || controller.getLeftY() < 0.04) { - height += controller.getLeftY()*0.54; - } - ElevatorSubsystem.setHeightMeters(height); - } - - @Override - //This is a default command, it should never finish theoretically - public boolean isFinished(){ - return false; - } - - -} +package frc.robot.commands.elevator; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.lib.input.XboxController; + + +public class ElevatorControlRaw extends Command{ + Elevator ElevatorSubsystem = Elevator.getInstance(); + XboxController controller; + /** + * This is really for testing, and a raw controller for the operator to use whenever they MUST, not recommend for use when other commands are working + * @param Elevator + * @param m_controller + */ + public ElevatorControlRaw(XboxController m_controller){ + controller = m_controller; + } + + @Override + public void execute(){ + double height = this.ElevatorSubsystem.getShooterHeightMeters(); + if (controller.getLeftY() > 0.04 || controller.getLeftY() < 0.04) { + height += controller.getLeftY()*0.54; + } + ElevatorSubsystem.setHeightMeters(height); + } + + @Override + //This is a default command, it should never finish theoretically + public boolean isFinished(){ + return false; + } + + +} diff --git a/src/main/java/frc/robot/commands/elevator/MoveElevatorAMP.java b/src/main/java/frc/robot/commands/elevator/MoveElevatorAMP.java index 98ab6a1..dcde8e5 100644 --- a/src/main/java/frc/robot/commands/elevator/MoveElevatorAMP.java +++ b/src/main/java/frc/robot/commands/elevator/MoveElevatorAMP.java @@ -1,26 +1,26 @@ -package frc.robot.commands.elevator; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.Elevator; - -public class MoveElevatorAMP extends Command { - private Elevator elevatorSubsystem = Elevator.getInstance(); - - public MoveElevatorAMP() { - } - - @Override - public void initialize() { - elevatorSubsystem.setHeightMeters(0.50); // after one increment of "y" - } - - @Override - public void end(boolean isInterrupted){ - //Add end if need be - } - - @Override - public boolean isFinished() { - return elevatorSubsystem.reachedSetpoint(); - } -} +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.Elevator; + +public class MoveElevatorAMP extends Command { + private Elevator elevatorSubsystem = Elevator.getInstance(); + + public MoveElevatorAMP() { + } + + @Override + public void initialize() { + elevatorSubsystem.setHeightMeters(0.50); // after one increment of "y" + } + + @Override + public void end(boolean isInterrupted){ + //Add end if need be + } + + @Override + public boolean isFinished() { + return elevatorSubsystem.reachedSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/elevator/MoveElevatorToggle.java b/src/main/java/frc/robot/commands/elevator/MoveElevatorToggle.java index 96501ac..008236e 100644 --- a/src/main/java/frc/robot/commands/elevator/MoveElevatorToggle.java +++ b/src/main/java/frc/robot/commands/elevator/MoveElevatorToggle.java @@ -1,32 +1,32 @@ -package frc.robot.commands.elevator; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.Elevator; - -public class MoveElevatorToggle extends Command { - private Elevator elevatorSubsystem = Elevator.getInstance(); - private double currHeight; - - public MoveElevatorToggle() { - } - - @Override - public void initialize() { - currHeight = elevatorSubsystem.getShooterHeightMeters(); - if(currHeight < 0.4) - elevatorSubsystem.setHeightMeters(0.5); - else - elevatorSubsystem.setHeightMeters(0); - } - - @Override - public void end(boolean isInterrupted){ - //Add end if need be - } - - @Override - public boolean isFinished() { - return elevatorSubsystem.reachedSetpoint(); - } -} - +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.Elevator; + +public class MoveElevatorToggle extends Command { + private Elevator elevatorSubsystem = Elevator.getInstance(); + private double currHeight; + + public MoveElevatorToggle() { + } + + @Override + public void initialize() { + currHeight = elevatorSubsystem.getShooterHeightMeters(); + if(currHeight < 0.4) + elevatorSubsystem.setHeightMeters(0.5); + else + elevatorSubsystem.setHeightMeters(0); + } + + @Override + public void end(boolean isInterrupted){ + //Add end if need be + } + + @Override + public boolean isFinished() { + return elevatorSubsystem.reachedSetpoint(); + } +} + diff --git a/src/main/java/frc/robot/commands/elevator/MoveElevatorTurtle.java b/src/main/java/frc/robot/commands/elevator/MoveElevatorTurtle.java index 239df4b..0a26331 100644 --- a/src/main/java/frc/robot/commands/elevator/MoveElevatorTurtle.java +++ b/src/main/java/frc/robot/commands/elevator/MoveElevatorTurtle.java @@ -1,26 +1,26 @@ -package frc.robot.commands.elevator; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.Elevator; - -public class MoveElevatorTurtle extends Command { - private Elevator elevatorSubsystem = Elevator.getInstance(); - - public MoveElevatorTurtle() { - } - - @Override - public void execute() { - elevatorSubsystem.setHeightMeters(0.00); - } - - @Override - public void end(boolean isInterrupted){ - //Add end if need be - } - - @Override - public boolean isFinished() { - return elevatorSubsystem.reachedSetpoint(); - } -} +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.Elevator; + +public class MoveElevatorTurtle extends Command { + private Elevator elevatorSubsystem = Elevator.getInstance(); + + public MoveElevatorTurtle() { + } + + @Override + public void execute() { + elevatorSubsystem.setHeightMeters(0.00); + } + + @Override + public void end(boolean isInterrupted){ + //Add end if need be + } + + @Override + public boolean isFinished() { + return elevatorSubsystem.reachedSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/intake/InIntake.java b/src/main/java/frc/robot/commands/intake/InIntake.java index 2b1170a..ed85d53 100644 --- a/src/main/java/frc/robot/commands/intake/InIntake.java +++ b/src/main/java/frc/robot/commands/intake/InIntake.java @@ -1,36 +1,36 @@ -package frc.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in wpilib. -import frc.robot.subsystems.intakeComp.Intake; // Reference to the intake subsystem. - -public class InIntake extends Command { - - private Intake intakeNeo = Intake.getInstance(); // Singleton instance of the Intake subsystem. - private boolean isIn = false; // Flag to track if the object is fully in. - - // Constructor for InIntake command. - public InIntake() { - } - - // Executes the command logic. - @Override - public void execute() { - // Call the intake method to bring an object in and update the flag. - isIn = intakeNeo.inIntake(); - } - - // Called when the command ends or is interrupted. - @Override - public void end(boolean isInterrupted) { - // If interrupted, set the flag to true to indicate the object is fully in. - if (isInterrupted) { - isIn = true; - } - } - - // Returns true when the intake operation is complete. - @Override - public boolean isFinished() { - return isIn; // Command finishes when the object is in. - } -} +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in wpilib. +import frc.robot.subsystems.intakeComp.Intake; // Reference to the intake subsystem. + +public class InIntake extends Command { + + private Intake intakeNeo = Intake.getInstance(); // Singleton instance of the Intake subsystem. + private boolean isIn = false; // Flag to track if the object is fully in. + + // Constructor for InIntake command. + public InIntake() { + } + + // Executes the command logic. + @Override + public void execute() { + // Call the intake method to bring an object in and update the flag. + isIn = intakeNeo.inIntake(); + } + + // Called when the command ends or is interrupted. + @Override + public void end(boolean isInterrupted) { + // If interrupted, set the flag to true to indicate the object is fully in. + if (isInterrupted) { + isIn = true; + } + } + + // Returns true when the intake operation is complete. + @Override + public boolean isFinished() { + return isIn; // Command finishes when the object is in. + } +} diff --git a/src/main/java/frc/robot/commands/intake/IntakeToggle.java b/src/main/java/frc/robot/commands/intake/IntakeToggle.java index fa31ad9..9eae94b 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeToggle.java +++ b/src/main/java/frc/robot/commands/intake/IntakeToggle.java @@ -1,56 +1,56 @@ -package frc.robot.commands.intake; - -import frc.robot.lib.input.XboxController; // Library for Xbox controller input. -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in wpilib. -import frc.robot.subsystems.intakeComp.Intake; // Reference to the intake subsystem. -import frc.robot.subsystems.pivotComp.Pivot; // Reference to the pivot subsystem. - -public class IntakeToggle extends Command { - - private int tapped; // Counter for button taps. - private final Intake intakeSubsystem = Intake.getInstance(); // Singleton instance of the Intake subsystem. - private final Pivot pivotSubsystem = Pivot.getInstance(); // Singleton instance of the Pivot subsystem. - private XboxController m_controller; // Reference to the Xbox controller. - - // Constructor that initializes the controller. - public IntakeToggle(XboxController controller) { - this.m_controller = controller; // Set the controller for input. - this.tapped = 0; // Initialize tapped counter. - } - - // Executes the command logic. - @Override - public void execute() { - // Check if the Y button is pressed on the controller. - if (this.m_controller.getYButton().getAsBoolean()) { - this.tapped++; // Increment the tapped counter. - - // Handle different cases based on the number of taps. - switch (tapped) { - case 1: // Roll intake - // Set intake speed based on pivot angle. - if (pivotSubsystem.getDegAngle() < 45) { - intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 20)); // Fast intake speed. - } else { - intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 4)); // Slower intake speed. - } - break; - - case 2: // Stop intake - intakeSubsystem.setVelocityRadSec(0); // Stop the intake. - break; - - case 3: // Reverse intake - intakeSubsystem.setVelocityRadSec(-1); // Reverse intake. - this.tapped = 0; // Reset tapped counter after reversing. - break; - } - } - } - - // Determines if the command should end. - @Override - public boolean isFinished() { - return true; // This command finishes immediately after execution. - } -} +package frc.robot.commands.intake; + +import frc.robot.lib.input.XboxController; // Library for Xbox controller input. +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in wpilib. +import frc.robot.subsystems.intakeComp.Intake; // Reference to the intake subsystem. +import frc.robot.subsystems.pivotComp.Pivot; // Reference to the pivot subsystem. + +public class IntakeToggle extends Command { + + private int tapped; // Counter for button taps. + private final Intake intakeSubsystem = Intake.getInstance(); // Singleton instance of the Intake subsystem. + private final Pivot pivotSubsystem = Pivot.getInstance(); // Singleton instance of the Pivot subsystem. + private XboxController m_controller; // Reference to the Xbox controller. + + // Constructor that initializes the controller. + public IntakeToggle(XboxController controller) { + this.m_controller = controller; // Set the controller for input. + this.tapped = 0; // Initialize tapped counter. + } + + // Executes the command logic. + @Override + public void execute() { + // Check if the Y button is pressed on the controller. + if (this.m_controller.getYButton().getAsBoolean()) { + this.tapped++; // Increment the tapped counter. + + // Handle different cases based on the number of taps. + switch (tapped) { + case 1: // Roll intake + // Set intake speed based on pivot angle. + if (pivotSubsystem.getDegAngle() < 45) { + intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 20)); // Fast intake speed. + } else { + intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 4)); // Slower intake speed. + } + break; + + case 2: // Stop intake + intakeSubsystem.setVelocityRadSec(0); // Stop the intake. + break; + + case 3: // Reverse intake + intakeSubsystem.setVelocityRadSec(-1); // Reverse intake. + this.tapped = 0; // Reset tapped counter after reversing. + break; + } + } + } + + // Determines if the command should end. + @Override + public boolean isFinished() { + return true; // This command finishes immediately after execution. + } +} diff --git a/src/main/java/frc/robot/commands/intake/OutIntake.java b/src/main/java/frc/robot/commands/intake/OutIntake.java index cdddd43..eadd11d 100644 --- a/src/main/java/frc/robot/commands/intake/OutIntake.java +++ b/src/main/java/frc/robot/commands/intake/OutIntake.java @@ -1,36 +1,36 @@ -package frc.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. -import frc.robot.subsystems.intakeComp.Intake; // Reference to the intake subsystem. - -public class OutIntake extends Command { - - private Intake intakeNeo = Intake.getInstance(); // Singleton instance of the Intake subsystem. - private boolean isIn = false; // Flag to track whether the object is in the intake. - - // Constructor for OutIntake command. - public OutIntake() { - } - - // Executes the command logic. - @Override - public void execute() { - // Update the isIn flag based on the intake status. - isIn = intakeNeo.inIntake(); - // Debugging statement (commented out). - // System.out.println("Out intake : " + !isIn); - } - - // Called when the command ends or is interrupted. - @Override - public void end(boolean isInterrupted) { - // No specific action on end; placeholder for future cleanup if necessary. - return; - } - - // Returns true when the object has been successfully ejected from the intake. - @Override - public boolean isFinished() { - return !isIn; // Command finishes when the object is no longer in the intake. - } -} +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. +import frc.robot.subsystems.intakeComp.Intake; // Reference to the intake subsystem. + +public class OutIntake extends Command { + + private Intake intakeNeo = Intake.getInstance(); // Singleton instance of the Intake subsystem. + private boolean isIn = false; // Flag to track whether the object is in the intake. + + // Constructor for OutIntake command. + public OutIntake() { + } + + // Executes the command logic. + @Override + public void execute() { + // Update the isIn flag based on the intake status. + isIn = intakeNeo.inIntake(); + // Debugging statement (commented out). + // System.out.println("Out intake : " + !isIn); + } + + // Called when the command ends or is interrupted. + @Override + public void end(boolean isInterrupted) { + // No specific action on end; placeholder for future cleanup if necessary. + return; + } + + // Returns true when the object has been successfully ejected from the intake. + @Override + public boolean isFinished() { + return !isIn; // Command finishes when the object is no longer in the intake. + } +} diff --git a/src/main/java/frc/robot/commands/intake/RollIntakeEject.java b/src/main/java/frc/robot/commands/intake/RollIntakeEject.java index 50f9e88..e42fea6 100644 --- a/src/main/java/frc/robot/commands/intake/RollIntakeEject.java +++ b/src/main/java/frc/robot/commands/intake/RollIntakeEject.java @@ -1,33 +1,33 @@ -package frc.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. -import frc.robot.subsystems.intakeComp.Intake; // This is the intake system. - -public class RollIntakeEject extends Command { - - private final Intake intakeSubsystem = Intake.getInstance(); // Get the intake system. - - // This constructor sets up the command. - public RollIntakeEject() { - } - - // This runs when the command starts. - @Override - public void initialize() { - // Set the intake to spin backwards to push out an object. - intakeSubsystem.setVelocityRadSec(Math.toRadians(-180)); // Speed for ejecting. - } - - // This runs when the command ends or is stopped. - @Override - public void end(boolean isInterrupted) { - // Optionally stop the intake when the command ends (currently not used). - // intakeSubsystem.setVelocityRadSec(0); // Stop spinning. - } - - // This checks if the command is done. - @Override - public boolean isFinished() { - return true; // The command is done immediately. - } -} +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. +import frc.robot.subsystems.intakeComp.Intake; // This is the intake system. + +public class RollIntakeEject extends Command { + + private final Intake intakeSubsystem = Intake.getInstance(); // Get the intake system. + + // This constructor sets up the command. + public RollIntakeEject() { + } + + // This runs when the command starts. + @Override + public void initialize() { + // Set the intake to spin backwards to push out an object. + intakeSubsystem.setVelocityRadSec(Math.toRadians(-180)); // Speed for ejecting. + } + + // This runs when the command ends or is stopped. + @Override + public void end(boolean isInterrupted) { + // Optionally stop the intake when the command ends (currently not used). + // intakeSubsystem.setVelocityRadSec(0); // Stop spinning. + } + + // This checks if the command is done. + @Override + public boolean isFinished() { + return true; // The command is done immediately. + } +} diff --git a/src/main/java/frc/robot/commands/intake/RollIntakeIn.java b/src/main/java/frc/robot/commands/intake/RollIntakeIn.java index c9230e3..b286019 100644 --- a/src/main/java/frc/robot/commands/intake/RollIntakeIn.java +++ b/src/main/java/frc/robot/commands/intake/RollIntakeIn.java @@ -1,43 +1,43 @@ -package frc.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. -import frc.robot.subsystems.intakeComp.Intake; // This is the intake system. -import frc.robot.subsystems.pivotComp.Pivot; // This is the pivot system. - -public class RollIntakeIn extends Command { - - private final Intake intakeSubsystem = Intake.getInstance(); // Get the intake system. - private final Pivot pivotSubsystem = Pivot.getInstance(); // Get the pivot system. - - // This constructor sets up the command. - public RollIntakeIn() { - } - - // This runs when the command starts. - @Override - public void initialize() { - // Check the angle of the pivot. - if (pivotSubsystem.getDegAngle() < 45) { - // If the angle is less than 45 degrees, spin the intake fast. - intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 10)); // Fast speed for intake. - } else { - // If the angle is 45 degrees or more, spin the intake at a slower speed. - intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 8)); // Slower speed for intake. - } - // intakeSubsystem.setVelocityRadSec(0); //Use radians directly - - } - - // This runs when the command ends or is stopped. - @Override - public void end(boolean isInterrupted) { - // Optionally stop the intake when the command ends (currently not used). - // intakeSubsystem.setVelocityRadSec(0); // Stop spinning. - } - - // This checks if the command is done. - @Override - public boolean isFinished() { - return true; // The command is done immediately. - } -} +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. +import frc.robot.subsystems.intakeComp.Intake; // This is the intake system. +import frc.robot.subsystems.pivotComp.Pivot; // This is the pivot system. + +public class RollIntakeIn extends Command { + + private final Intake intakeSubsystem = Intake.getInstance(); // Get the intake system. + private final Pivot pivotSubsystem = Pivot.getInstance(); // Get the pivot system. + + // This constructor sets up the command. + public RollIntakeIn() { + } + + // This runs when the command starts. + @Override + public void initialize() { + // Check the angle of the pivot. + if (pivotSubsystem.getDegAngle() < 45) { + // If the angle is less than 45 degrees, spin the intake fast. + intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 10)); // Fast speed for intake. + } else { + // If the angle is 45 degrees or more, spin the intake at a slower speed. + intakeSubsystem.setVelocityRadSec(Math.toRadians(360 * 8)); // Slower speed for intake. + } + // intakeSubsystem.setVelocityRadSec(0); //Use radians directly + + } + + // This runs when the command ends or is stopped. + @Override + public void end(boolean isInterrupted) { + // Optionally stop the intake when the command ends (currently not used). + // intakeSubsystem.setVelocityRadSec(0); // Stop spinning. + } + + // This checks if the command is done. + @Override + public boolean isFinished() { + return true; // The command is done immediately. + } +} diff --git a/src/main/java/frc/robot/commands/intake/RollIntakeInSlow.java b/src/main/java/frc/robot/commands/intake/RollIntakeInSlow.java index 7f44679..c9c5ab0 100644 --- a/src/main/java/frc/robot/commands/intake/RollIntakeInSlow.java +++ b/src/main/java/frc/robot/commands/intake/RollIntakeInSlow.java @@ -1,40 +1,40 @@ -package frc.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. -import frc.robot.subsystems.intakeComp.Intake; // This is the intake system. - -public class RollIntakeInSlow extends Command { - - private final Intake intakeSubsystem = Intake.getInstance(); // Get the intake system. - private double velo; // Variable to hold the intake speed. - - // Default constructor sets the speed to 150. - public RollIntakeInSlow() { - velo = 150; // Default speed for slow intake. - } - - // This constructor allows setting a custom speed. - public RollIntakeInSlow(double a) { - velo = a; // Set the intake speed to the value provided. - } - - // This runs when the command starts. - @Override - public void initialize() { - // Set the intake to spin at the specified slow speed. - intakeSubsystem.setVelocityRadSec(Math.toRadians(velo)); // Convert speed to radians. - } - - // This runs when the command ends or is stopped. - @Override - public void end(boolean isInterrupted) { - // Optionally stop the intake when the command ends (currently not active). - // intakeSubsystem.setVelocityRadSec(0); // Stop spinning. - } - - // This checks if the command is done. - @Override - public boolean isFinished() { - return true; // The command is done immediately. - } -} +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. +import frc.robot.subsystems.intakeComp.Intake; // This is the intake system. + +public class RollIntakeInSlow extends Command { + + private final Intake intakeSubsystem = Intake.getInstance(); // Get the intake system. + private double velo; // Variable to hold the intake speed. + + // Default constructor sets the speed to 150. + public RollIntakeInSlow() { + velo = 150; // Default speed for slow intake. + } + + // This constructor allows setting a custom speed. + public RollIntakeInSlow(double a) { + velo = a; // Set the intake speed to the value provided. + } + + // This runs when the command starts. + @Override + public void initialize() { + // Set the intake to spin at the specified slow speed. + intakeSubsystem.setVelocityRadSec(Math.toRadians(velo)); // Convert speed to radians. + } + + // This runs when the command ends or is stopped. + @Override + public void end(boolean isInterrupted) { + // Optionally stop the intake when the command ends (currently not active). + // intakeSubsystem.setVelocityRadSec(0); // Stop spinning. + } + + // This checks if the command is done. + @Override + public boolean isFinished() { + return true; // The command is done immediately. + } +} diff --git a/src/main/java/frc/robot/commands/intake/RollIntakeOut.java b/src/main/java/frc/robot/commands/intake/RollIntakeOut.java index 1716d70..caed6af 100644 --- a/src/main/java/frc/robot/commands/intake/RollIntakeOut.java +++ b/src/main/java/frc/robot/commands/intake/RollIntakeOut.java @@ -1,29 +1,29 @@ - -package frc.robot.commands.intake; - - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.intakeComp.Intake; - -public class RollIntakeOut extends Command { - private final Intake intakeSubsystem = Intake.getInstance(); - - public RollIntakeOut() { - } - - @Override - public void initialize() { - intakeSubsystem.setVelocityRadSec(Math.toRadians(-150)); //Use radians directly. - } - - @Override - public void end(boolean isInterrupted){ - //ta inkeSubsystem.setVelocityRadSec(0); - } - - @Override - public boolean isFinished() { - return true; - - } -} + +package frc.robot.commands.intake; + + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intakeComp.Intake; + +public class RollIntakeOut extends Command { + private final Intake intakeSubsystem = Intake.getInstance(); + + public RollIntakeOut() { + } + + @Override + public void initialize() { + intakeSubsystem.setVelocityRadSec(Math.toRadians(-150)); //Use radians directly. + } + + @Override + public void end(boolean isInterrupted){ + //ta inkeSubsystem.setVelocityRadSec(0); + } + + @Override + public boolean isFinished() { + return true; + + } +} diff --git a/src/main/java/frc/robot/commands/intake/StopIntake.java b/src/main/java/frc/robot/commands/intake/StopIntake.java index 608e40b..d5c82c1 100644 --- a/src/main/java/frc/robot/commands/intake/StopIntake.java +++ b/src/main/java/frc/robot/commands/intake/StopIntake.java @@ -1,26 +1,26 @@ -package frc.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.intakeComp.Intake; - -public class StopIntake extends Command { - private final Intake intakeSubsystem = Intake.getInstance(); - - public StopIntake() { - } - - @Override - public void initialize() { - intakeSubsystem.setVelocityRadSec(0); //Use radians directly. - } - - @Override - public void end(boolean isInterrupted){ - //intakeSubsystem.setVelocityRadSec(0); - } - - @Override - public boolean isFinished() { - return true; - } -} +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intakeComp.Intake; + +public class StopIntake extends Command { + private final Intake intakeSubsystem = Intake.getInstance(); + + public StopIntake() { + } + + @Override + public void initialize() { + intakeSubsystem.setVelocityRadSec(0); //Use radians directly. + } + + @Override + public void end(boolean isInterrupted){ + //intakeSubsystem.setVelocityRadSec(0); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/pivot/PivotController.java b/src/main/java/frc/robot/commands/pivot/PivotController.java index 2f58d62..f1283d4 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotController.java +++ b/src/main/java/frc/robot/commands/pivot/PivotController.java @@ -1,34 +1,34 @@ -package frc.robot.commands.pivot; - -import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj.XboxController; -import frc.robot.subsystems.pivotComp.Pivot; -import frc.robot.lib.input.XboxController; - -public class PivotController extends Command { - Pivot pivotSubsystem = Pivot.getInstance(); - XboxController controller; - - public PivotController(XboxController controller) { - this.controller = controller; - } - - @Override - public void execute() { - // pivotSubsystem.setVelocityControlMode(true); - // pivotSubsystem.setVelocitySetPoint(0.1 * controller.getRightY()); - pivotSubsystem.setVoltage(12 * controller.getRightY()); - } - - @Override - public boolean isFinished() { - return false; - } - - public void end(boolean interrupted){ - System.out.println("CALLED END"); - pivotSubsystem.setVelocitySetPoint(0); - return; - } -} - +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj.XboxController; +import frc.robot.subsystems.pivotComp.Pivot; +import frc.robot.lib.input.XboxController; + +public class PivotController extends Command { + Pivot pivotSubsystem = Pivot.getInstance(); + XboxController controller; + + public PivotController(XboxController controller) { + this.controller = controller; + } + + @Override + public void execute() { + // pivotSubsystem.setVelocityControlMode(true); + // pivotSubsystem.setVelocitySetPoint(0.1 * controller.getRightY()); + pivotSubsystem.setVoltage(12 * controller.getRightY()); + } + + @Override + public boolean isFinished() { + return false; + } + + public void end(boolean interrupted){ + System.out.println("CALLED END"); + pivotSubsystem.setVelocitySetPoint(0); + return; + } +} + diff --git a/src/main/java/frc/robot/commands/pivot/PivotMidway.java b/src/main/java/frc/robot/commands/pivot/PivotMidway.java index 58657fc..64706b0 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotMidway.java +++ b/src/main/java/frc/robot/commands/pivot/PivotMidway.java @@ -1,21 +1,21 @@ -package frc.robot.commands.pivot; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pivotComp.Pivot; - -public class PivotMidway extends Command { - private final Pivot pivotSubsystem = Pivot.getInstance(); - - public PivotMidway() { - } - - @Override - public void initialize() { - pivotSubsystem.setDegAngle(40); - } - - @Override - public boolean isFinished() { - return pivotSubsystem.reachedSetpoint(); - } -} +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pivotComp.Pivot; + +public class PivotMidway extends Command { + private final Pivot pivotSubsystem = Pivot.getInstance(); + + public PivotMidway() { + } + + @Override + public void initialize() { + pivotSubsystem.setDegAngle(40); + } + + @Override + public boolean isFinished() { + return pivotSubsystem.reachedSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/pivot/PivotToTorus.java b/src/main/java/frc/robot/commands/pivot/PivotToTorus.java index 401d5f4..1c3a989 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToTorus.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToTorus.java @@ -1,23 +1,23 @@ -package frc.robot.commands.pivot; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pivotComp.Pivot; - -public class PivotToTorus extends Command { - private final Pivot pivotSubsystem = Pivot.getInstance(); - - public PivotToTorus() { - } - - @Override - public void initialize() { - pivotSubsystem.setDegAngle(89); - // pivotSubsystem.setDegAngle(0.8); - - } - - @Override - public boolean isFinished() { - return pivotSubsystem.reachedSetpoint(); - } -} +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pivotComp.Pivot; + +public class PivotToTorus extends Command { + private final Pivot pivotSubsystem = Pivot.getInstance(); + + public PivotToTorus() { + } + + @Override + public void initialize() { + pivotSubsystem.setDegAngle(89); + // pivotSubsystem.setDegAngle(0.8); + + } + + @Override + public boolean isFinished() { + return pivotSubsystem.reachedSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/pivot/PivotTurtle.java b/src/main/java/frc/robot/commands/pivot/PivotTurtle.java index 32575a9..c982685 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotTurtle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotTurtle.java @@ -1,21 +1,21 @@ -package frc.robot.commands.pivot; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pivotComp.Pivot; - -public class PivotTurtle extends Command { - private final Pivot pivotSubsystem = Pivot.getInstance(); - - public PivotTurtle() { - } - - @Override - public void initialize() { - pivotSubsystem.setDegAngle(-0.009); //0 - } - - @Override - public boolean isFinished() { - return pivotSubsystem.reachedSetpoint(); - } -} +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pivotComp.Pivot; + +public class PivotTurtle extends Command { + private final Pivot pivotSubsystem = Pivot.getInstance(); + + public PivotTurtle() { + } + + @Override + public void initialize() { + pivotSubsystem.setDegAngle(-0.009); //0 + } + + @Override + public boolean isFinished() { + return pivotSubsystem.reachedSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/ShooterStop.java b/src/main/java/frc/robot/commands/shooter/ShooterStop.java index 002773a..aa4c7ed 100644 --- a/src/main/java/frc/robot/commands/shooter/ShooterStop.java +++ b/src/main/java/frc/robot/commands/shooter/ShooterStop.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.shooter; - -// import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.CommandScheduler; -// import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel; - -// public class ShooterStop extends Command { -// private Flywheel flywheelSubsystem; -// public ShooterStop(Flywheel flywheel) { -// flywheelSubsystem = flywheel; - -// addRequirements(flywheel); -// } - -// @Override -// public void initialize() { -// flywheelSubsystem.setRPM(0); -// } - -// @Override -// public boolean isFinished() { -// return flywheelSubsystem.reachedSetpoint(); -// } - - -// } +// package frc.robot.commands.shooter; + +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.CommandScheduler; +// import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel; + +// public class ShooterStop extends Command { +// private Flywheel flywheelSubsystem; +// public ShooterStop(Flywheel flywheel) { +// flywheelSubsystem = flywheel; + +// addRequirements(flywheel); +// } + +// @Override +// public void initialize() { +// flywheelSubsystem.setRPM(0); +// } + +// @Override +// public boolean isFinished() { +// return flywheelSubsystem.reachedSetpoint(); +// } + + +// } diff --git a/src/main/java/frc/robot/commands/shooter/ShooterWindup.java b/src/main/java/frc/robot/commands/shooter/ShooterWindup.java index 21799ad..876db02 100644 --- a/src/main/java/frc/robot/commands/shooter/ShooterWindup.java +++ b/src/main/java/frc/robot/commands/shooter/ShooterWindup.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.shooter; - -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel; - -// public class ShooterWindup extends Command { -// private Flywheel flywheelSubsystem; -// public ShooterWindup(Flywheel flywheel) { -// flywheelSubsystem = flywheel; - -// addRequirements(flywheel); -// } - -// @Override -// public void initialize() { -// flywheelSubsystem.setRPM(170); -// } - -// @Override -// public boolean isFinished() { -// return flywheelSubsystem.reachedSetpoint(); -// } - -// @Override -// public void end(boolean interrupted) {} -// } +// package frc.robot.commands.shooter; + +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel; + +// public class ShooterWindup extends Command { +// private Flywheel flywheelSubsystem; +// public ShooterWindup(Flywheel flywheel) { +// flywheelSubsystem = flywheel; + +// addRequirements(flywheel); +// } + +// @Override +// public void initialize() { +// flywheelSubsystem.setRPM(170); +// } + +// @Override +// public boolean isFinished() { +// return flywheelSubsystem.reachedSetpoint(); +// } + +// @Override +// public void end(boolean interrupted) {} +// } diff --git a/src/main/java/frc/robot/commands/shooterComp/AlignWithTargetPoint.java b/src/main/java/frc/robot/commands/shooterComp/AlignWithTargetPoint.java index ea5dfb5..cac5100 100644 --- a/src/main/java/frc/robot/commands/shooterComp/AlignWithTargetPoint.java +++ b/src/main/java/frc/robot/commands/shooterComp/AlignWithTargetPoint.java @@ -1,47 +1,47 @@ -// package frc.robot.commands.shooter; - -// import edu.wpi.first.math.geometry.Pose3d; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.geometry.Rotation3d; - -// import frc.robot.subsystems.shooter.Shooter; -// import frc.robot.subsystems.swerve.SwerveSubsystem; -// import frc.robot.subsystems.limelight.PoseLimelight; - -// import edu.wpi.first.wpilibj2.command.Command; - -// import frc.robot.Utils.RebelUtil; - -// public class AlignWithTargetPoint extends Command { -// private final SwerveSubsystem swerveSubsystem; -// private final PoseLimelight llsubsystem; -// private final Pose3d shooterPose; - -// public AlignWithTargetPoint(SwerveSubsystem swerveSubsystem, PoseLimelight llsubsystem) { -// this.swerveSubsystem = swerveSubsystem; -// this.llsubsystem = llsubsystem; -// this.shooterPose = new Pose3d(-0.301828,0,0.577596,new Rotation3d(0,0,0)); // first three zeroes are real measurments, no euler angles (rot. at tip) -// addRequirements(swerveSubsystem, llsubsystem); // F_g = mg moment -// } - -// @Override -// public void execute() { -// Pose2d currPose = this.swerveSubsystem.getPose(); // assume that everything in x,y,z?? -// Pose2d initialPose = new Pose2d(currPose.getX()-(15/39.4), currPose.getY(), currPose.getRotation()); // adjusted for the 15 in offset :skull: -// Pose3d targetPointPose = this.llsubsystem.getValidShotPoint().relativeTo(new Pose3d(initialPose)); - -// Pose2d computedPose = RebelUtil.calculateAlignedPose(llsubsystem, initialPose, this.shooterPose, targetPointPose); -// RebelUtil.driveRobotToPose(computedPose); -// calcFlywheelSpeed(new Pose3d(computedPose), targetPointPose); -// } - -// private static void calcFlywheelSpeed(Pose3d initial, Pose3d desired) { -// RebelUtil.flywheelSpeed = Math.sqrt((8/3)*9.8*Math.abs(initial.getZ()-desired.getZ())); -// } - -// @Override -// public boolean isFinished() { -// return true; -// } - +// package frc.robot.commands.shooter; + +// import edu.wpi.first.math.geometry.Pose3d; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation3d; + +// import frc.robot.subsystems.shooter.Shooter; +// import frc.robot.subsystems.swerve.SwerveSubsystem; +// import frc.robot.subsystems.limelight.PoseLimelight; + +// import edu.wpi.first.wpilibj2.command.Command; + +// import frc.robot.Utils.RebelUtil; + +// public class AlignWithTargetPoint extends Command { +// private final SwerveSubsystem swerveSubsystem; +// private final PoseLimelight llsubsystem; +// private final Pose3d shooterPose; + +// public AlignWithTargetPoint(SwerveSubsystem swerveSubsystem, PoseLimelight llsubsystem) { +// this.swerveSubsystem = swerveSubsystem; +// this.llsubsystem = llsubsystem; +// this.shooterPose = new Pose3d(-0.301828,0,0.577596,new Rotation3d(0,0,0)); // first three zeroes are real measurments, no euler angles (rot. at tip) +// addRequirements(swerveSubsystem, llsubsystem); // F_g = mg moment +// } + +// @Override +// public void execute() { +// Pose2d currPose = this.swerveSubsystem.getPose(); // assume that everything in x,y,z?? +// Pose2d initialPose = new Pose2d(currPose.getX()-(15/39.4), currPose.getY(), currPose.getRotation()); // adjusted for the 15 in offset :skull: +// Pose3d targetPointPose = this.llsubsystem.getValidShotPoint().relativeTo(new Pose3d(initialPose)); + +// Pose2d computedPose = RebelUtil.calculateAlignedPose(llsubsystem, initialPose, this.shooterPose, targetPointPose); +// RebelUtil.driveRobotToPose(computedPose); +// calcFlywheelSpeed(new Pose3d(computedPose), targetPointPose); +// } + +// private static void calcFlywheelSpeed(Pose3d initial, Pose3d desired) { +// RebelUtil.flywheelSpeed = Math.sqrt((8/3)*9.8*Math.abs(initial.getZ()-desired.getZ())); +// } + +// @Override +// public boolean isFinished() { +// return true; +// } + // } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooterComp/InShooter.java b/src/main/java/frc/robot/commands/shooterComp/InShooter.java index 2bd5766..09550de 100644 --- a/src/main/java/frc/robot/commands/shooterComp/InShooter.java +++ b/src/main/java/frc/robot/commands/shooterComp/InShooter.java @@ -1,32 +1,32 @@ -package frc.robot.commands.shooterComp; - -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. -import frc.robot.subsystems.shooterComp.Shooter; // This is the shooter subsystem. - -public class InShooter extends Command { - - private boolean isIn = false; // Flag to check if the item is in the shooter. - - // Constructor - public InShooter() { - } - - // This runs repeatedly while the command is scheduled. - @Override - public void execute() { - isIn = Shooter.getInstance().inShooter(); // Update the status based on the shooter's state. - } - - // This runs when the command ends or is interrupted. - @Override - public void end(boolean isInterrupted) { - // No specific action on end for now; can be expanded as needed. - return; - } - - // This checks if the command is finished. - @Override - public boolean isFinished() { - return isIn; // Command finishes when the item is in the shooter. - } -} +package frc.robot.commands.shooterComp; + +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. +import frc.robot.subsystems.shooterComp.Shooter; // This is the shooter subsystem. + +public class InShooter extends Command { + + private boolean isIn = false; // Flag to check if the item is in the shooter. + + // Constructor + public InShooter() { + } + + // This runs repeatedly while the command is scheduled. + @Override + public void execute() { + isIn = Shooter.getInstance().inShooter(); // Update the status based on the shooter's state. + } + + // This runs when the command ends or is interrupted. + @Override + public void end(boolean isInterrupted) { + // No specific action on end for now; can be expanded as needed. + return; + } + + // This checks if the command is finished. + @Override + public boolean isFinished() { + return isIn; // Command finishes when the item is in the shooter. + } +} diff --git a/src/main/java/frc/robot/commands/shooterComp/ShooterAmp.java b/src/main/java/frc/robot/commands/shooterComp/ShooterAmp.java new file mode 100644 index 0000000..31aa032 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooterComp/ShooterAmp.java @@ -0,0 +1,23 @@ +package frc.robot.commands.shooterComp; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.shooterComp.Shooter; + +public class ShooterAmp extends Command { + + private double topVelo = 10; + private double bottomVelo = 60; + private final Shooter shooterSubsystem = Shooter.getInstance(); + + @Override + public void initialize(){ + shooterSubsystem.setVelocityRadSec(0, true, bottomVelo, topVelo); + } + + @Override + public boolean isFinished(){ + return true; + } + +} diff --git a/src/main/java/frc/robot/commands/shooterComp/ShooterHold.java b/src/main/java/frc/robot/commands/shooterComp/ShooterHold.java index 943703d..9aef416 100644 --- a/src/main/java/frc/robot/commands/shooterComp/ShooterHold.java +++ b/src/main/java/frc/robot/commands/shooterComp/ShooterHold.java @@ -1,26 +1,26 @@ -package frc.robot.commands.shooterComp; - -import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. -import frc.robot.subsystems.shooterComp.Shooter; // This is the shooter subsystem. - -public class ShooterHold extends Command { - - private final Shooter shooterSubsystem = Shooter.getInstance(); // Get the shooter subsystem. - - public ShooterHold() { - // Constructor with no parameters. - } - - // This runs when the command is initialized. - @Override - public void initialize() { - // Set the shooter's velocity to 6.5 rad/sec. - shooterSubsystem.setVelocityRadSec(6.5, false, 0, 0); // Set velocity for holding. - } - - // This checks if the command is finished. - @Override - public boolean isFinished() { - return true; // The command completes immediately. - } -} +package frc.robot.commands.shooterComp; + +import edu.wpi.first.wpilibj2.command.Command; // Base class for commands in WPILib. +import frc.robot.subsystems.shooterComp.Shooter; // This is the shooter subsystem. + +public class ShooterHold extends Command { + + private final Shooter shooterSubsystem = Shooter.getInstance(); // Get the shooter subsystem. + + public ShooterHold() { + // Constructor with no parameters. + } + + // This runs when the command is initialized. + @Override + public void initialize() { + // Set the shooter's velocity to 6.5 rad/sec. + shooterSubsystem.setVelocityRadSec(6.5, false, 0, 0); // Set velocity for holding. + } + + // This checks if the command is finished. + @Override + public boolean isFinished() { + return true; // The command completes immediately. + } +} diff --git a/src/main/java/frc/robot/commands/shooterComp/ShooterStop.java b/src/main/java/frc/robot/commands/shooterComp/ShooterStop.java index 45e02e2..9f40068 100644 --- a/src/main/java/frc/robot/commands/shooterComp/ShooterStop.java +++ b/src/main/java/frc/robot/commands/shooterComp/ShooterStop.java @@ -1,27 +1,27 @@ -package frc.robot.commands.shooterComp; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.shooterComp.Shooter; - -public class ShooterStop extends Command { - // private double velocitySetPoint = 0; - private final Shooter shooterSubsystem = Shooter.getInstance(); - - public ShooterStop(){ - } - public ShooterStop(SequentialCommandGroup s){ - s.cancel(); - } - - @Override - public void initialize(){ - shooterSubsystem.setVelocityRadSec(0,false, 0, 0); - } - - @Override - public boolean isFinished(){ - return true; - } - -} +package frc.robot.commands.shooterComp; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.shooterComp.Shooter; + +public class ShooterStop extends Command { + // private double velocitySetPoint = 0; + private final Shooter shooterSubsystem = Shooter.getInstance(); + + public ShooterStop(){ + } + public ShooterStop(SequentialCommandGroup s){ + s.cancel(); + } + + @Override + public void initialize(){ + shooterSubsystem.setVelocityRadSec(0,false, 0, 0); + } + + @Override + public boolean isFinished(){ + return true; + } + +} diff --git a/src/main/java/frc/robot/commands/shooterComp/ShooterToggle.java b/src/main/java/frc/robot/commands/shooterComp/ShooterToggle.java index dfc9c21..e4b091f 100644 --- a/src/main/java/frc/robot/commands/shooterComp/ShooterToggle.java +++ b/src/main/java/frc/robot/commands/shooterComp/ShooterToggle.java @@ -1,59 +1,59 @@ -package frc.robot.commands.shooterComp; - -import frc.robot.lib.input.XboxController; // Xbox controller for input. -import edu.wpi.first.wpilibj2.command.Command; // Base command class. -import frc.robot.subsystems.shooterComp.Shooter; // Shooter subsystem reference. - -public class ShooterToggle extends Command { - - private final double stopVelocitySetPoint = 0; // Velocity to stop the shooter. - private final double windupVelocitySetpoint = 5; // Velocity for winding up. - private final double windupReverseVelocitySetpoint = -2; // Velocity for reverse windup. - private final double holdVelocitySetpoint = 0; // Hold velocity. - - private int tapped; // Counter for button taps. - - private final Shooter shooterSubsystem = Shooter.getInstance(); // Singleton instance of the shooter subsystem. - private XboxController m_controller; // Controller instance. - - public ShooterToggle(XboxController controller) { - this.m_controller = controller; // Initialize the controller. - this.tapped = 0; // Initialize tap counter. - } - - @Override - public void execute() { - if (this.m_controller.getLeftBumper().getAsBoolean()) { // Check if the left bumper is pressed. - this.tapped++; // Increment tap counter. - double desiredSpeed = 0; // Variable to hold the desired speed. - - // Determine the desired speed based on the number of taps. - switch (tapped) { - case 1: - desiredSpeed = this.windupVelocitySetpoint; // Set for windup. - break; - - case 2: - desiredSpeed = this.stopVelocitySetPoint; // Set to stop the shooter. - break; - - case 3: - desiredSpeed = this.windupReverseVelocitySetpoint; // Set for reverse windup. - break; - - case 4: - desiredSpeed = this.holdVelocitySetpoint; // Set for holding position. - this.tapped = 0; // Reset counter after the fourth tap. - break; - } - - // Apply the desired speed to the shooter. - shooterSubsystem.setVelocityRadSec(desiredSpeed, false, 0, 0); - } - } - - @Override - public boolean isFinished() { - return true; // Command finishes immediately after execution. - } -} +package frc.robot.commands.shooterComp; + +import frc.robot.lib.input.XboxController; // Xbox controller for input. +import edu.wpi.first.wpilibj2.command.Command; // Base command class. +import frc.robot.subsystems.shooterComp.Shooter; // Shooter subsystem reference. + +public class ShooterToggle extends Command { + + private final double stopVelocitySetPoint = 0; // Velocity to stop the shooter. + private final double windupVelocitySetpoint = 5; // Velocity for winding up. + private final double windupReverseVelocitySetpoint = -2; // Velocity for reverse windup. + private final double holdVelocitySetpoint = 0; // Hold velocity. + + private int tapped; // Counter for button taps. + + private final Shooter shooterSubsystem = Shooter.getInstance(); // Singleton instance of the shooter subsystem. + private XboxController m_controller; // Controller instance. + + public ShooterToggle(XboxController controller) { + this.m_controller = controller; // Initialize the controller. + this.tapped = 0; // Initialize tap counter. + } + + @Override + public void execute() { + if (this.m_controller.getLeftBumper().getAsBoolean()) { // Check if the left bumper is pressed. + this.tapped++; // Increment tap counter. + double desiredSpeed = 0; // Variable to hold the desired speed. + + // Determine the desired speed based on the number of taps. + switch (tapped) { + case 1: + desiredSpeed = this.windupVelocitySetpoint; // Set for windup. + break; + + case 2: + desiredSpeed = this.stopVelocitySetPoint; // Set to stop the shooter. + break; + + case 3: + desiredSpeed = this.windupReverseVelocitySetpoint; // Set for reverse windup. + break; + + case 4: + desiredSpeed = this.holdVelocitySetpoint; // Set for holding position. + this.tapped = 0; // Reset counter after the fourth tap. + break; + } + + // Apply the desired speed to the shooter. + shooterSubsystem.setVelocityRadSec(desiredSpeed, false, 0, 0); + } + } + + @Override + public boolean isFinished() { + return true; // Command finishes immediately after execution. + } +} diff --git a/src/main/java/frc/robot/commands/shooterComp/ShooterWindReverse.java b/src/main/java/frc/robot/commands/shooterComp/ShooterWindReverse.java index 7c8a4c3..c3c9492 100644 --- a/src/main/java/frc/robot/commands/shooterComp/ShooterWindReverse.java +++ b/src/main/java/frc/robot/commands/shooterComp/ShooterWindReverse.java @@ -1,23 +1,23 @@ -package frc.robot.commands.shooterComp; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.shooterComp.Shooter; - -public class ShooterWindReverse extends Command { - private double velocitySetPoint = -24; - private final Shooter shooterSubsystem = Shooter.getInstance(); - - public ShooterWindReverse(){ - } - - @Override - public void initialize(){ - shooterSubsystem.setVelocityRadSec(velocitySetPoint, false, 0, 0); - } - - @Override - public boolean isFinished(){ - return true; - } - -} +package frc.robot.commands.shooterComp; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooterComp.Shooter; + +public class ShooterWindReverse extends Command { + private double velocitySetPoint = -24; + private final Shooter shooterSubsystem = Shooter.getInstance(); + + public ShooterWindReverse(){ + } + + @Override + public void initialize(){ + shooterSubsystem.setVelocityRadSec(velocitySetPoint, false, 0, 0); + } + + @Override + public boolean isFinished(){ + return true; + } + +} diff --git a/src/main/java/frc/robot/commands/shooterComp/ShooterWindup.java b/src/main/java/frc/robot/commands/shooterComp/ShooterWindup.java index 59474cc..e41d4de 100644 --- a/src/main/java/frc/robot/commands/shooterComp/ShooterWindup.java +++ b/src/main/java/frc/robot/commands/shooterComp/ShooterWindup.java @@ -1,32 +1,32 @@ -package frc.robot.commands.shooterComp; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.shooterComp.Shooter; - -public class ShooterWindup extends Command { - - private double velocitySetPoint = 60; - private final Shooter shooterSubsystem = Shooter.getInstance(); - - public ShooterWindup(double setpoint){ - this.velocitySetPoint = setpoint; - } - - public ShooterWindup() { - } - public ShooterWindup(SequentialCommandGroup s){ - s.cancel(); - } - - @Override - public void initialize(){ - shooterSubsystem.setVelocityRadSec(velocitySetPoint, false, 0, 0); - } - - @Override - public boolean isFinished(){ - return true; - } - -} +package frc.robot.commands.shooterComp; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.shooterComp.Shooter; + +public class ShooterWindup extends Command { + + private double velocitySetPoint = 60; + private final Shooter shooterSubsystem = Shooter.getInstance(); + + public ShooterWindup(double setpoint){ + this.velocitySetPoint = setpoint; + } + + public ShooterWindup() { + } + public ShooterWindup(SequentialCommandGroup s){ + s.cancel(); + } + + @Override + public void initialize(){ + shooterSubsystem.setVelocityRadSec(velocitySetPoint, false, 0, 0); + } + + @Override + public boolean isFinished(){ + return true; + } + +} diff --git a/src/main/java/frc/robot/commands/shooterComp/ShooterWindupLob.java b/src/main/java/frc/robot/commands/shooterComp/ShooterWindupLob.java index 1e4165b..d6fd279 100644 --- a/src/main/java/frc/robot/commands/shooterComp/ShooterWindupLob.java +++ b/src/main/java/frc/robot/commands/shooterComp/ShooterWindupLob.java @@ -1,32 +1,32 @@ -package frc.robot.commands.shooterComp; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.shooterComp.Shooter; - -public class ShooterWindupLob extends Command { - - private double velocitySetPoint = 48; - private final Shooter shooterSubsystem = Shooter.getInstance(); - - public ShooterWindupLob(double setpoint){ - this.velocitySetPoint = setpoint; - } - - public ShooterWindupLob() { - } - public ShooterWindupLob(SequentialCommandGroup s){ - s.cancel(); - } - - @Override - public void initialize(){ - shooterSubsystem.setVelocityRadSec(velocitySetPoint, false, 0, 0); - } - - @Override - public boolean isFinished(){ - return true; - } - -} +package frc.robot.commands.shooterComp; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.shooterComp.Shooter; + +public class ShooterWindupLob extends Command { + + private double velocitySetPoint = 48; + private final Shooter shooterSubsystem = Shooter.getInstance(); + + public ShooterWindupLob(double setpoint){ + this.velocitySetPoint = setpoint; + } + + public ShooterWindupLob() { + } + public ShooterWindupLob(SequentialCommandGroup s){ + s.cancel(); + } + + @Override + public void initialize(){ + shooterSubsystem.setVelocityRadSec(velocitySetPoint, false, 0, 0); + } + + @Override + public boolean isFinished(){ + return true; + } + +} diff --git a/src/main/java/frc/robot/lib/input/Controller.java b/src/main/java/frc/robot/lib/input/Controller.java index 177ab50..8a9df0a 100644 --- a/src/main/java/frc/robot/lib/input/Controller.java +++ b/src/main/java/frc/robot/lib/input/Controller.java @@ -1,8 +1,8 @@ -package frc.robot.lib.input; - -/** - * Controller interface - */ -public interface Controller { - void mapController(); +package frc.robot.lib.input; + +/** + * Controller interface + */ +public interface Controller { + void mapController(); } \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/input/ControllerConstants.java b/src/main/java/frc/robot/lib/input/ControllerConstants.java index 98da229..1ab2df8 100644 --- a/src/main/java/frc/robot/lib/input/ControllerConstants.java +++ b/src/main/java/frc/robot/lib/input/ControllerConstants.java @@ -1,29 +1,29 @@ -package frc.robot.lib.input; - -/** - * Constants used for controllers - */ -public class ControllerConstants { - public class XboxConstants { - public static final int JOYSTICK_LEFT_X = 0; - public static final int JOYSTICK_LEFT_Y = 1; - public static final int JOYSTICK_LEFT_TRIGGER = 2; - public static final int JOYSTICK_RIGHT_TRIGGER = 3; - public static final int JOYSTICK_RIGHT_X = 4; - public static final int JOYSTICK_RIGHT_Y = 5; - - public static final int BUTTON_A = 1; - public static final int BUTTON_B = 2; - public static final int BUTTON_X = 3; - public static final int BUTTON_Y = 4; - public static final int BUMPER_LEFT = 5; - public static final int BUMPER_RIGHT = 6; - public static final int BUTTON_LEFT_MIDDLE = 7; - public static final int BUTTON_RIGHT_MIDDLE = 8; - public static final int BUTTON_LEFT_STICK = 9; - public static final int BUTTON_RIGHT_STICK = 10; - - public static final double AXIS_BUTTON_THRESHOLD = .5; - public static final double JOYSTICK_BUTTON_THRESHOLD = .1; - } +package frc.robot.lib.input; + +/** + * Constants used for controllers + */ +public class ControllerConstants { + public class XboxConstants { + public static final int JOYSTICK_LEFT_X = 0; + public static final int JOYSTICK_LEFT_Y = 1; + public static final int JOYSTICK_LEFT_TRIGGER = 2; + public static final int JOYSTICK_RIGHT_TRIGGER = 3; + public static final int JOYSTICK_RIGHT_X = 4; + public static final int JOYSTICK_RIGHT_Y = 5; + + public static final int BUTTON_A = 1; + public static final int BUTTON_B = 2; + public static final int BUTTON_X = 3; + public static final int BUTTON_Y = 4; + public static final int BUMPER_LEFT = 5; + public static final int BUMPER_RIGHT = 6; + public static final int BUTTON_LEFT_MIDDLE = 7; + public static final int BUTTON_RIGHT_MIDDLE = 8; + public static final int BUTTON_LEFT_STICK = 9; + public static final int BUTTON_RIGHT_STICK = 10; + + public static final double AXIS_BUTTON_THRESHOLD = .5; + public static final double JOYSTICK_BUTTON_THRESHOLD = .1; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/input/XboxController.java b/src/main/java/frc/robot/lib/input/XboxController.java index ab1f917..54357d9 100644 --- a/src/main/java/frc/robot/lib/input/XboxController.java +++ b/src/main/java/frc/robot/lib/input/XboxController.java @@ -1,223 +1,223 @@ -package frc.robot.lib.input; - -import static frc.robot.lib.input.ControllerConstants.XboxConstants.*; - -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; - -/** - * Implementation of an Xbox controller - */ -public class XboxController implements Controller { - private Joystick joystick; // Joystick object representing the Xbox controller - - // Trigger objects for each button on the Xbox controller - private Trigger aButton, bButton, xButton, yButton, leftMiddleButton, rightMiddleButton, - leftBumper, rightBumper, leftStick, rightStick, - leftTriggerButton, rightTriggerButton, rightStickYButton, - upDpad, downDpad, leftDpad, rightDpad; - - // Constructor that initializes the joystick with a given instance - public XboxController(Joystick joystick) { - this.joystick = joystick; - this.mapController(); // Map the buttons to their respective triggers - } - - // Constructor that initializes the joystick with a given port number - public XboxController(int port) { - this(new Joystick(port)); // Create a new Joystick instance using the specified port - } - - @Override - public void mapController() { - // Map each button on the joystick to its corresponding trigger - this.aButton = new JoystickButton(joystick, BUTTON_A); - this.bButton = new JoystickButton(joystick, BUTTON_B); - this.xButton = new JoystickButton(joystick, BUTTON_X); - this.yButton = new JoystickButton(joystick, BUTTON_Y); - this.leftMiddleButton = new JoystickButton(joystick, BUTTON_LEFT_MIDDLE); - this.rightMiddleButton = new JoystickButton(joystick, BUTTON_RIGHT_MIDDLE); - this.leftBumper = new JoystickButton(joystick, BUMPER_LEFT); - this.rightBumper = new JoystickButton(joystick, BUMPER_RIGHT); - this.leftStick = new JoystickButton(joystick, BUTTON_LEFT_STICK); - this.rightStick = new JoystickButton(joystick, BUTTON_RIGHT_STICK); - this.upDpad = new POVButton(joystick, 0); // Up D-pad button - this.rightDpad = new POVButton(joystick, 90); // Right D-pad button - this.downDpad = new POVButton(joystick, 180); // Down D-pad button - this.leftDpad = new POVButton(joystick, 270); // Left D-pad button - } - - /** - * @return the value of the left trigger - */ - public double getLeftTrigger() { - return joystick.getRawAxis(JOYSTICK_LEFT_TRIGGER); // Get raw axis value for left trigger - } - - /** - * @return the value of the right trigger - */ - public double getRightTrigger() { - return joystick.getRawAxis(JOYSTICK_RIGHT_TRIGGER); // Get raw axis value for right trigger - } - - /** - * @return the value of the left joystick x-axis - */ - public double getLeftX() { - return joystick.getRawAxis(JOYSTICK_LEFT_X); // Get raw axis value for left joystick X-axis - } - - /** - * @return the value of the left joystick y-axis (up is positive and down is negative) - */ - public double getLeftY() { - return joystick.getRawAxis(JOYSTICK_LEFT_Y); // Get raw axis value for left joystick Y-axis - } - - /** - * @return the value of the right joystick x-axis - */ - public double getRightX() { - return joystick.getRawAxis(JOYSTICK_RIGHT_X); // Get raw axis value for right joystick X-axis - } - - /** - * @return the value of the right joystick y-axis (up is positive and down is negative) - */ - public double getRightY() { - return joystick.getRawAxis(JOYSTICK_RIGHT_Y); // Get raw axis value for right joystick Y-axis - } - - /** - * @return the A button trigger - */ - public Trigger getAButton() { - return aButton; // Return A button trigger - } - - /** - * @return the B button trigger - */ - public Trigger getBButton() { - return bButton; // Return B button trigger - } - - /** - * @return the X button trigger - */ - public Trigger getXButton() { - return xButton; // Return X button trigger - } - - /** - * @return the Y button trigger - */ - public Trigger getYButton() { - return yButton; // Return Y button trigger - } - - /** - * @return the left middle button (start button? view button?) - */ - public Trigger getLeftMiddleButton() { - return leftMiddleButton; // Return left middle button trigger - } - - /** - * @return the right middle button (select button? menu button?) - */ - public Trigger getRightMiddleButton() { - return rightMiddleButton; // Return right middle button trigger - } - - /** - * @return the left bumper trigger - */ - public Trigger getLeftBumper() { - return leftBumper; // Return left bumper trigger - } - - /** - * @return the right bumper trigger - */ - public Trigger getRightBumper() { - return rightBumper; // Return right bumper trigger - } - - /** - * @return the left stick as a trigger - */ - public Trigger getLeftStick() { - return leftStick; // Return left stick trigger - } - - /** - * @return the right stick as a trigger - */ - public Trigger getRightStick() { - return rightStick; // Return right stick trigger - } - - /** - * @return the right trigger as a button (not implemented in original code) - */ - public Trigger getRightTriggerButton() { - return rightTriggerButton; // Return right trigger as a button (if implemented) - } - - /** - * @return the left trigger as a button (not implemented in original code) - */ - public Trigger getLeftTriggerButton() { - return leftTriggerButton; // Return left trigger as a button (if implemented) - } - - /** - * @return the right stick's y-axis as a button (not implemented in original code) - */ - public Trigger getRightStickYButton() { - return rightStickYButton; // Return right stick's Y-axis as a button (if implemented) - } - - /** - * @return the up D-pad button trigger - */ - public Trigger getUpDpad() { - return upDpad; // Return up D-pad button trigger - } - - /** - * @return the down D-pad button trigger - */ - public Trigger getDownDpad() { - return downDpad; // Return down D-pad button trigger - } - - /** - * @return the left D-pad button trigger - */ - public Trigger getLeftDpad() { - return leftDpad; // Return left D-pad button trigger - } - - /** - * @return the right D-pad button trigger - */ - public Trigger getRightDpad() { - return rightDpad; // Return right D-pad button trigger - } - - /** - * Sets the rumble amount on the controller. - * - * @param type Which rumble value to set (left or right motor) - * @param value The normalized value (0 to 1) to set the rumble to. - */ - public void setRumble(RumbleType type, double value) { - joystick.setRumble(type, value); // Set rumble effect on specified motor with given intensity. - } -} +package frc.robot.lib.input; + +import static frc.robot.lib.input.ControllerConstants.XboxConstants.*; + +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; + +/** + * Implementation of an Xbox controller + */ +public class XboxController implements Controller { + private Joystick joystick; // Joystick object representing the Xbox controller + + // Trigger objects for each button on the Xbox controller + private Trigger aButton, bButton, xButton, yButton, leftMiddleButton, rightMiddleButton, + leftBumper, rightBumper, leftStick, rightStick, + leftTriggerButton, rightTriggerButton, rightStickYButton, + upDpad, downDpad, leftDpad, rightDpad; + + // Constructor that initializes the joystick with a given instance + public XboxController(Joystick joystick) { + this.joystick = joystick; + this.mapController(); // Map the buttons to their respective triggers + } + + // Constructor that initializes the joystick with a given port number + public XboxController(int port) { + this(new Joystick(port)); // Create a new Joystick instance using the specified port + } + + @Override + public void mapController() { + // Map each button on the joystick to its corresponding trigger + this.aButton = new JoystickButton(joystick, BUTTON_A); + this.bButton = new JoystickButton(joystick, BUTTON_B); + this.xButton = new JoystickButton(joystick, BUTTON_X); + this.yButton = new JoystickButton(joystick, BUTTON_Y); + this.leftMiddleButton = new JoystickButton(joystick, BUTTON_LEFT_MIDDLE); + this.rightMiddleButton = new JoystickButton(joystick, BUTTON_RIGHT_MIDDLE); + this.leftBumper = new JoystickButton(joystick, BUMPER_LEFT); + this.rightBumper = new JoystickButton(joystick, BUMPER_RIGHT); + this.leftStick = new JoystickButton(joystick, BUTTON_LEFT_STICK); + this.rightStick = new JoystickButton(joystick, BUTTON_RIGHT_STICK); + this.upDpad = new POVButton(joystick, 0); // Up D-pad button + this.rightDpad = new POVButton(joystick, 90); // Right D-pad button + this.downDpad = new POVButton(joystick, 180); // Down D-pad button + this.leftDpad = new POVButton(joystick, 270); // Left D-pad button + } + + /** + * @return the value of the left trigger + */ + public double getLeftTrigger() { + return joystick.getRawAxis(JOYSTICK_LEFT_TRIGGER); // Get raw axis value for left trigger + } + + /** + * @return the value of the right trigger + */ + public double getRightTrigger() { + return joystick.getRawAxis(JOYSTICK_RIGHT_TRIGGER); // Get raw axis value for right trigger + } + + /** + * @return the value of the left joystick x-axis + */ + public double getLeftX() { + return joystick.getRawAxis(JOYSTICK_LEFT_X); // Get raw axis value for left joystick X-axis + } + + /** + * @return the value of the left joystick y-axis (up is positive and down is negative) + */ + public double getLeftY() { + return joystick.getRawAxis(JOYSTICK_LEFT_Y); // Get raw axis value for left joystick Y-axis + } + + /** + * @return the value of the right joystick x-axis + */ + public double getRightX() { + return joystick.getRawAxis(JOYSTICK_RIGHT_X); // Get raw axis value for right joystick X-axis + } + + /** + * @return the value of the right joystick y-axis (up is positive and down is negative) + */ + public double getRightY() { + return joystick.getRawAxis(JOYSTICK_RIGHT_Y); // Get raw axis value for right joystick Y-axis + } + + /** + * @return the A button trigger + */ + public Trigger getAButton() { + return aButton; // Return A button trigger + } + + /** + * @return the B button trigger + */ + public Trigger getBButton() { + return bButton; // Return B button trigger + } + + /** + * @return the X button trigger + */ + public Trigger getXButton() { + return xButton; // Return X button trigger + } + + /** + * @return the Y button trigger + */ + public Trigger getYButton() { + return yButton; // Return Y button trigger + } + + /** + * @return the left middle button (start button? view button?) + */ + public Trigger getLeftMiddleButton() { + return leftMiddleButton; // Return left middle button trigger + } + + /** + * @return the right middle button (select button? menu button?) + */ + public Trigger getRightMiddleButton() { + return rightMiddleButton; // Return right middle button trigger + } + + /** + * @return the left bumper trigger + */ + public Trigger getLeftBumper() { + return leftBumper; // Return left bumper trigger + } + + /** + * @return the right bumper trigger + */ + public Trigger getRightBumper() { + return rightBumper; // Return right bumper trigger + } + + /** + * @return the left stick as a trigger + */ + public Trigger getLeftStick() { + return leftStick; // Return left stick trigger + } + + /** + * @return the right stick as a trigger + */ + public Trigger getRightStick() { + return rightStick; // Return right stick trigger + } + + /** + * @return the right trigger as a button (not implemented in original code) + */ + public Trigger getRightTriggerButton() { + return rightTriggerButton; // Return right trigger as a button (if implemented) + } + + /** + * @return the left trigger as a button (not implemented in original code) + */ + public Trigger getLeftTriggerButton() { + return leftTriggerButton; // Return left trigger as a button (if implemented) + } + + /** + * @return the right stick's y-axis as a button (not implemented in original code) + */ + public Trigger getRightStickYButton() { + return rightStickYButton; // Return right stick's Y-axis as a button (if implemented) + } + + /** + * @return the up D-pad button trigger + */ + public Trigger getUpDpad() { + return upDpad; // Return up D-pad button trigger + } + + /** + * @return the down D-pad button trigger + */ + public Trigger getDownDpad() { + return downDpad; // Return down D-pad button trigger + } + + /** + * @return the left D-pad button trigger + */ + public Trigger getLeftDpad() { + return leftDpad; // Return left D-pad button trigger + } + + /** + * @return the right D-pad button trigger + */ + public Trigger getRightDpad() { + return rightDpad; // Return right D-pad button trigger + } + + /** + * Sets the rumble amount on the controller. + * + * @param type Which rumble value to set (left or right motor) + * @param value The normalized value (0 to 1) to set the rumble to. + */ + public void setRumble(RumbleType type, double value) { + joystick.setRumble(type, value); // Set rumble effect on specified motor with given intensity. + } +} diff --git a/src/main/java/frc/robot/lib/util/RebelUtil.java b/src/main/java/frc/robot/lib/util/RebelUtil.java index 700d1f6..21fdcd8 100644 --- a/src/main/java/frc/robot/lib/util/RebelUtil.java +++ b/src/main/java/frc/robot/lib/util/RebelUtil.java @@ -1,61 +1,72 @@ -package frc.robot.lib.util; - -public class RebelUtil { - public static final double EPSILON = 1e-12; - /** - * Constrains a number to be within a minimum and maximum - * - * @param toConstrain The number you're constraining - * @param min The minimum value allowable for this number - * @param max The maximum value allowable for this number - * - * @return the constrained value (will be between min and max) - */ - public static double constrain(double toConstrain, double min, double max) { - if (toConstrain > max) { - return max; - } - if (toConstrain < min) { - return min; - } - return toConstrain; - } - - public static double linearDeadband(double value, double deadband) { - if (Math.abs(value) > deadband) { - if (value > 0.0) { - return (value - deadband) / (1.0 - deadband); - } - else { - return (value + deadband) / (1.0 - deadband); - } - } - else { - return 0.0; - } - } - - public static double cubicDeadband(double value, double deadband, double weight) { - value = linearDeadband(value, deadband); - return weight * Math.pow(value, 3) + (1 - weight) * value; - } - - /** - * Checks if a number is "close enough" for equality to another number. - * - * @param a The first number you want to check - * @param b The second number you want to check - * @param epsilon Allowed difference - * - * @return true/false for whether equality is met - * - * Second version uses default EPSILON - */ - public static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); - } - - public static boolean epsilonEquals(double a, double b) { - return epsilonEquals(a, b, EPSILON); - } +package frc.robot.lib.util; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class RebelUtil { + public static final double EPSILON = 1e-12; + /** + * Constrains a number to be within a minimum and maximum + * + * @param toConstrain The number you're constraining + * @param min The minimum value allowable for this number + * @param max The maximum value allowable for this number + * + * @return the constrained value (will be between min and max) + */ + public static double constrain(double toConstrain, double min, double max) { + if (toConstrain > max) { + return max; + } + if (toConstrain < min) { + return min; + } + return toConstrain; + } + + public static double linearDeadband(double value, double deadband) { + if (Math.abs(value) > deadband) { + if (value > 0.0) { + return (value - deadband) / (1.0 - deadband); + } + else { + return (value + deadband) / (1.0 - deadband); + } + } + else { + return 0.0; + } + } + + public static double cubicDeadband(double value, double deadband, double weight) { + value = linearDeadband(value, deadband); + return weight * Math.pow(value, 3) + (1 - weight) * value; + } + + /** + * Checks if a number is "close enough" for equality to another number. + * + * @param a The first number you want to check + * @param b The second number you want to check + * @param epsilon Allowed difference + * + * @return true/false for whether equality is met + * + * Second version uses default EPSILON + */ + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, EPSILON); + } + + public static ChassisSpeeds scaleSpeeds(double maxTranslationalSpeedMps, ChassisSpeeds speeds) { + ChassisSpeeds corrected = new ChassisSpeeds(); + double scaler = Math.sqrt(Math.pow(maxTranslationalSpeedMps,2)/(Math.pow(speeds.vxMetersPerSecond,2) + Math.pow(speeds.vyMetersPerSecond,2))); + corrected.vxMetersPerSecond = speeds.vxMetersPerSecond * scaler; + corrected.vyMetersPerSecond = speeds.vyMetersPerSecond * scaler; + corrected.omegaRadiansPerSecond = speeds.omegaRadiansPerSecond; + return corrected; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java index 6b375da..c1ca433 100644 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java @@ -1,47 +1,47 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ExampleSubsystem extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + public ExampleSubsystem() {} + + /** + * Example command factory method. + * + * @return a command + */ + public Command exampleMethodCommand() { + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return runOnce( + () -> { + /* one-time action goes here */ + }); + } + + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + public boolean exampleCondition() { + // Query some boolean state, such as a digital sensor. + return false; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java index b33cbba..c3e9ef7 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java @@ -1,177 +1,177 @@ -package frc.robot.subsystems.drivetrain.swerve; - -import java.util.ArrayList; - -import org.littletonrobotics.junction.Logger; -import frc.robot.Constants; -import frc.robot.Constants.DrivetrainConstants; -import frc.robot.lib.util.RebelUtil; - -public class DriveFFController { - ArrayList points = new ArrayList(); - public DriveFFController() { - // drive meters per second, radians per second, mps error - switch (Constants.currentMode) { - case SIM: - points.add(new double[] {0, 0, 0}); - points.add(new double[] {2, 0, 0}); - points.add(new double[] {4, 0, 0}); - points.add(new double[] {0, Math.PI, 0}); - points.add(new double[] {0, 2 * Math.PI, 0}); - points.add(new double[] {4, 2 * Math.PI, 1.237}); - points.add(new double[] {2, 2 * Math.PI, 0.582}); - points.add(new double[] {4, Math.PI, 0.435}); - points.add(new double[] {2, Math.PI, 0.412}); - break; - - default: - points.add(new double[] {0, 0, 0}); - points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, 0, 0}); - points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 0, 0}); - points.add(new double[] {0, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0}); - points.add(new double[] {0, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0}); - points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0.635}); - points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0.33}); - points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0.4}); - points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0.15}); - break; - } - } - public double calculate(double imps, double irps) { - - double mps = RebelUtil.constrain(Math.abs(imps), 0, 4); - double rps = RebelUtil.constrain(Math.abs(irps), 0, 2 * Math.PI); - // for (int i = 0; i < points.size(); i++) { - // if (points.get(i)[0] == mps && points.get(i)[1] == rps) { - // return points.get(i)[2]; - // } - // } - - double[] inputPoint = new double[] {mps, rps, 0}; - - double[][] p = new double[4][3]; - for (int i = 0; i < 4; i++) { - p[i] = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, -1}; - } - - ArrayList temp = new ArrayList(); - temp.addAll(points); - - for (int n = 0; n < 4; n++) { - - boolean good = false; - - while (!good) { - int index = 0; - double minDist = Integer.MAX_VALUE; - good = true; - for(int i = 0; i < temp.size(); i++) { - if (distance(temp.get(i), inputPoint) < minDist) { - minDist = distance(temp.get(i), inputPoint); - index = i; - } - } - - for (int m = 0; m < n; m++) { - if (distance(p[m], temp.get(index)) > Math.sqrt(2)) { - good = false; - break; - } - } - if (good) { - p[n] = temp.get(index); - } - - temp.remove(index); - } - } - - - - double[] tr, tl, br, bl; - tr = new double[] {-Integer.MAX_VALUE, -Integer.MAX_VALUE, 0}; - tl = new double[] {Integer.MAX_VALUE, -Integer.MAX_VALUE, 0}; - br = new double[] {-Integer.MAX_VALUE, Integer.MAX_VALUE, 0}; - bl = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, 0}; - - boolean trT = false; - boolean tlT = false; - boolean brT = false; - boolean blT = false; - - - for (int i = 0; i < 4; i++) { - if (p[i][0] >= inputPoint[0] && p[i][1] >= inputPoint[1] && !trT) { - tr = p[i]; - trT = true; - } - else if (p[i][0] <= inputPoint[0] && p[i][1] >= inputPoint[1] && !tlT) { - tl = p[i]; - tlT = true; - } - else if (p[i][0] >= inputPoint[0] && p[i][1] <= inputPoint[1] && !brT) { - br = p[i]; - brT = true; - } - else if (p[i][0] <= inputPoint[0] && p[i][1] <= inputPoint[1] && !blT) { - bl = p[i]; - blT = true; - } - } - - double r1 = Math.abs((tr[0] - inputPoint[0])/(tr[0] - tl[0]) * tl[2] + (inputPoint[0] - tl[0])/(tr[0] - tl[0]) * tr[2]); - double r2 = Math.abs((br[0] - inputPoint[0])/(br[0] - bl[0]) * bl[2] + (inputPoint[0] - bl[0])/(br[0] - bl[0]) * br[2]); - double pf = Math.abs((inputPoint[1] - br[1])/(tr[1] - br[1]) * r1 + (tr[1] - inputPoint[1])/(tr[1] - bl[1]) * r2); - - Logger.recordOutput("SwerveDrive/driveFF/r1", r1); - Logger.recordOutput("SwerveDrive/driveFF/r2", r2); - Logger.recordOutput("SwerveDrive/driveFF/pf", pf); - - - double multiplier = 1; - if (imps >= 0 && irps <= 0) { - multiplier = -1; - } - else if (imps <= 0 && irps >= 0) { - multiplier = -1; - } - - Logger.recordOutput("SwerveDrive/driveFF/multiplier", multiplier); - - if (!Double.isNaN(pf) && Double.isFinite(pf)) { - return (pf * multiplier); - } - - if (r1 == 0 && !Double.isNaN(r2) && Double.isFinite(r2)) { - return (r2 * multiplier); - } - - if (r2 == 0 && !Double.isNaN(r1) && Double.isFinite(r1)) { - return (r1 * multiplier); - } - - return 0; - - } - - private double distance(double[] a, double[] b) { - double r; - switch (Constants.currentMode) { - case SIM: - r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 2)); - break; - - default: - r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 2)); - break; - } - - return r; - } - - public static void main(String[] args) { - DriveFFController driveFFController = new DriveFFController(); - System.out.println(driveFFController.calculate(-1, -3.23)); - } - +package frc.robot.subsystems.drivetrain.swerve; + +import java.util.ArrayList; + +import org.littletonrobotics.junction.Logger; +import frc.robot.Constants; +import frc.robot.Constants.DrivetrainConstants; +import frc.robot.lib.util.RebelUtil; + +public class DriveFFController { + ArrayList points = new ArrayList(); + public DriveFFController() { + // drive meters per second, radians per second, mps error + switch (Constants.currentMode) { + case SIM: + points.add(new double[] {0, 0, 0}); + points.add(new double[] {2, 0, 0}); + points.add(new double[] {4, 0, 0}); + points.add(new double[] {0, Math.PI, 0}); + points.add(new double[] {0, 2 * Math.PI, 0}); + points.add(new double[] {4, 2 * Math.PI, 1.237}); + points.add(new double[] {2, 2 * Math.PI, 0.582}); + points.add(new double[] {4, Math.PI, 0.435}); + points.add(new double[] {2, Math.PI, 0.412}); + break; + + default: + points.add(new double[] {0, 0, 0}); + points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, 0, 0}); + points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 0, 0}); + points.add(new double[] {0, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0}); + points.add(new double[] {0, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0}); + points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 1.8}); // 1.6 + points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0.8}); // .6 + points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0.25});// 0.2 + points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0.2}); // .2 + break; + } + } + public double calculate(double imps, double irps) { + + double mps = RebelUtil.constrain(Math.abs(imps), 0, 4); + double rps = RebelUtil.constrain(Math.abs(irps), 0, 2 * Math.PI); + // for (int i = 0; i < points.size(); i++) { + // if (points.get(i)[0] == mps && points.get(i)[1] == rps) { + // return points.get(i)[2]; + // } + // } + + double[] inputPoint = new double[] {mps, rps, 0}; + + double[][] p = new double[4][3]; + for (int i = 0; i < 4; i++) { + p[i] = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, -1}; + } + + ArrayList temp = new ArrayList(); + temp.addAll(points); + + for (int n = 0; n < 4; n++) { + + boolean good = false; + + while (!good) { + int index = 0; + double minDist = Integer.MAX_VALUE; + good = true; + for(int i = 0; i < temp.size(); i++) { + if (distance(temp.get(i), inputPoint) < minDist) { + minDist = distance(temp.get(i), inputPoint); + index = i; + } + } + + for (int m = 0; m < n; m++) { + if (distance(p[m], temp.get(index)) > Math.sqrt(2)) { + good = false; + break; + } + } + if (good) { + p[n] = temp.get(index); + } + + temp.remove(index); + } + } + + + + double[] tr, tl, br, bl; + tr = new double[] {-Integer.MAX_VALUE, -Integer.MAX_VALUE, 0}; + tl = new double[] {Integer.MAX_VALUE, -Integer.MAX_VALUE, 0}; + br = new double[] {-Integer.MAX_VALUE, Integer.MAX_VALUE, 0}; + bl = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, 0}; + + boolean trT = false; + boolean tlT = false; + boolean brT = false; + boolean blT = false; + + + for (int i = 0; i < 4; i++) { + if (p[i][0] >= inputPoint[0] && p[i][1] >= inputPoint[1] && !trT) { + tr = p[i]; + trT = true; + } + else if (p[i][0] <= inputPoint[0] && p[i][1] >= inputPoint[1] && !tlT) { + tl = p[i]; + tlT = true; + } + else if (p[i][0] >= inputPoint[0] && p[i][1] <= inputPoint[1] && !brT) { + br = p[i]; + brT = true; + } + else if (p[i][0] <= inputPoint[0] && p[i][1] <= inputPoint[1] && !blT) { + bl = p[i]; + blT = true; + } + } + + double r1 = Math.abs((tr[0] - inputPoint[0])/(tr[0] - tl[0]) * tl[2] + (inputPoint[0] - tl[0])/(tr[0] - tl[0]) * tr[2]); + double r2 = Math.abs((br[0] - inputPoint[0])/(br[0] - bl[0]) * bl[2] + (inputPoint[0] - bl[0])/(br[0] - bl[0]) * br[2]); + double pf = Math.abs((inputPoint[1] - br[1])/(tr[1] - br[1]) * r1 + (tr[1] - inputPoint[1])/(tr[1] - bl[1]) * r2); + + Logger.recordOutput("SwerveDrive/driveFF/r1", r1); + Logger.recordOutput("SwerveDrive/driveFF/r2", r2); + Logger.recordOutput("SwerveDrive/driveFF/pf", pf); + + + double multiplier = 1; + if (imps >= 0 && irps <= 0) { + multiplier = -1; + } + else if (imps <= 0 && irps >= 0) { + multiplier = -1; + } + + Logger.recordOutput("SwerveDrive/driveFF/multiplier", multiplier); + + if (!Double.isNaN(pf) && Double.isFinite(pf)) { + return (pf * multiplier); + } + + if (r1 == 0 && !Double.isNaN(r2) && Double.isFinite(r2)) { + return (r2 * multiplier); + } + + if (r2 == 0 && !Double.isNaN(r1) && Double.isFinite(r1)) { + return (r1 * multiplier); + } + + return 0; + + } + + private double distance(double[] a, double[] b) { + double r; + switch (Constants.currentMode) { + case SIM: + r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 2)); + break; + + default: + r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 2)); + break; + } + + return r; + } + + public static void main(String[] args) { + DriveFFController driveFFController = new DriveFFController(); + System.out.println(driveFFController.calculate(-1, -3.23)); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIO.java index 8053a03..939a6c2 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIO.java @@ -1,20 +1,20 @@ -package frc.robot.subsystems.drivetrain.swerve; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; - -public interface GyroIO { - @AutoLog - public static class GyroIOInputs { - public Rotation2d yaw = new Rotation2d(); - public boolean isConnected = false; - } - - public default void updateInputs(GyroIOInputs inputs) {}; - public default void setOffset(Rotation3d offset) {}; - public default void zero() {}; - public default void reset(Rotation3d inital) {}; - -} +package frc.robot.subsystems.drivetrain.swerve; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public Rotation2d yaw = new Rotation2d(); + public boolean isConnected = false; + } + + public default void updateInputs(GyroIOInputs inputs) {}; + public default void setOffset(Rotation3d offset) {}; + public default void zero() {}; + public default void reset(Rotation3d inital) {}; + +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIONavX.java index 01185e4..47b1b51 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/GyroIONavX.java @@ -1,45 +1,45 @@ -package frc.robot.subsystems.drivetrain.swerve; - -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.wpilibj.SerialPort.Port; - -public class GyroIONavX implements GyroIO { - private AHRS gyro; - private Rotation3d offset = new Rotation3d(); - - public GyroIONavX() { - try { - gyro = new AHRS(Port.kMXP); - } - catch (RuntimeException ex) { - System.err.println("Error initilizing Gyro"); - } - } - - @Override - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.isConnected = gyro.isConnected(); - if (inputs.isConnected) { - inputs.yaw = new Rotation2d(-(Math.toRadians(gyro.getYaw()) - offset.getZ())); - } - } - - @Override - public void zero() { - gyro.zeroYaw(); - } - - @Override - public void reset(Rotation3d inital) { - offset = gyro.getRotation3d().plus(inital); - } - -} +package frc.robot.subsystems.drivetrain.swerve; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj.SerialPort.Port; + +public class GyroIONavX implements GyroIO { + private AHRS gyro; + private Rotation3d offset = new Rotation3d(); + + public GyroIONavX() { + try { + gyro = new AHRS(Port.kMXP); + } + catch (RuntimeException ex) { + System.err.println("Error initilizing Gyro"); + } + } + + @Override + public void setOffset(Rotation3d offset) { + this.offset = offset; + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.isConnected = gyro.isConnected(); + if (inputs.isConnected) { + inputs.yaw = new Rotation2d(-(Math.toRadians(gyro.getYaw()) - offset.getZ())); + } + } + + @Override + public void zero() { + gyro.zeroYaw(); + } + + @Override + public void reset(Rotation3d inital) { + offset = gyro.getRotation3d().plus(inital); + } + +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIO.java index 01d551b..5dfe67b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIO.java @@ -1,33 +1,33 @@ -package frc.robot.subsystems.drivetrain.swerve; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.kinematics.SwerveModuleState; - -public interface ModuleIO { - @AutoLog - public static class ModuleIOInputs { - public double driveVelocityMps = 0; - public double drivePositionMeters = 0; - - public double angleVelocityRadPerSec = 0; - public double anglePositionRad = 0; - - public double absEncoderRad = 0; - - public double driveCurrentAmps = 0; - public double angleCurrentAmps = 0; - - public double driveTempC = 0; - public double angleTempC = 0; - - public double driveVoltage = 0; - public double angleVoltage = 0; - } - - public default void updateInputs(ModuleIOInputs inputs) {} - public default void setState(SwerveModuleState state) {} - public default void resetPosition() {} - public default void setDriveVoltage(double voltage) {} - public default void queueSynchronizeEncoders() {} -} +package frc.robot.subsystems.drivetrain.swerve; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public double driveVelocityMps = 0; + public double drivePositionMeters = 0; + + public double angleVelocityRadPerSec = 0; + public double anglePositionRad = 0; + + public double absEncoderRad = 0; + + public double driveCurrentAmps = 0; + public double angleCurrentAmps = 0; + + public double driveTempC = 0; + public double angleTempC = 0; + + public double driveVoltage = 0; + public double angleVoltage = 0; + } + + public default void updateInputs(ModuleIOInputs inputs) {} + public default void setState(SwerveModuleState state) {} + public default void resetPosition() {} + public default void setDriveVoltage(double voltage) {} + public default void queueSynchronizeEncoders() {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOSim.java index 262fe83..5c5fcc2 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOSim.java @@ -1,107 +1,107 @@ -package frc.robot.subsystems.drivetrain.swerve; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import frc.robot.Constants; -import frc.robot.lib.util.RebelUtil; - -public class ModuleIOSim implements ModuleIO { - private DCMotor m_gearBoxAngle = DCMotor.getFalcon500Foc(1); - private DCMotor m_gearBoxDrive = DCMotor.getFalcon500Foc(1); - - private FlywheelSim m_angleSim = new FlywheelSim(m_gearBoxAngle, 1, 0.0001); - private FlywheelSim m_driveSim = new FlywheelSim(m_gearBoxDrive, 1, 0.003); - - private static final PIDController m_angleFeedbackController = new PIDController(0.06, 0.0, 0.0002); - private static final PIDController m_driveFeedbackController = new PIDController(0.001, 0, 0); - - private static final SimpleMotorFeedforward m_angleFeedforward = new SimpleMotorFeedforward(0, 0.0000); - private static final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(0, 0.00365, 0.00); - - private double m_angleVoltage = 0; - private double m_driveVoltage = 0; - - private double anglePositionRad = 0; - private double drivePositionsMeters = 0; - - private double prevTime = Timer.getFPGATimestamp(); - - public ModuleIOSim() { - m_angleFeedbackController.setTolerance(Math.toRadians(3)); - m_driveFeedbackController.setTolerance(0.01); - - m_angleFeedbackController.enableContinuousInput(0, 2 * Math.PI); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - m_angleSim.update(Timer.getFPGATimestamp() - prevTime); - m_driveSim.update(Timer.getFPGATimestamp() - prevTime); - - inputs.angleVelocityRadPerSec = m_angleSim.getAngularVelocityRPM(); - inputs.driveVelocityMps = m_driveSim.getAngularVelocityRPM() * Constants.DrivetrainConstants.kDRIVE_WHEEL_RADIUS_METERS * 2 * Math.PI; - - anglePositionRad += inputs.angleVelocityRadPerSec * (Timer.getFPGATimestamp() - prevTime); - drivePositionsMeters += inputs.driveVelocityMps * (Timer.getFPGATimestamp() - prevTime); - - anglePositionRad = anglePositionRad % (2 * Math.PI); - - inputs.anglePositionRad = anglePositionRad; - inputs.drivePositionMeters = drivePositionsMeters; - - inputs.angleCurrentAmps = m_angleSim.getCurrentDrawAmps(); - inputs.driveCurrentAmps = m_driveSim.getCurrentDrawAmps(); - - inputs.angleTempC = 0; - inputs.driveTempC = 0; - - inputs.angleVoltage = m_angleVoltage; - inputs.driveVoltage = m_driveVoltage; - - prevTime = Timer.getFPGATimestamp(); - } - - @Override - public void setState(SwerveModuleState state) { - if (DriverStation.isTest()) {return;} - - double speed = state.speedMetersPerSecond; - m_driveVoltage = m_driveFeedforward.calculate(speed, Math.signum(speed - m_driveSim.getAngularVelocityRPM())) + - m_driveFeedbackController.calculate(m_driveSim.getAngularVelocityRPM(), speed); - - m_driveVoltage = RebelUtil.constrain( - m_driveVoltage, -Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE, - Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE); - - m_driveSim.setInputVoltage(m_driveVoltage); - - double angle = state.angle.getRadians(); - m_angleVoltage = m_angleFeedforward.calculate(0, 0) + - m_angleFeedbackController.calculate(anglePositionRad, angle); - - m_angleVoltage = RebelUtil.constrain( - m_angleVoltage, -Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE, - Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE); - - m_angleSim.setInputVoltage(m_angleVoltage); - } - - @Override - public void setDriveVoltage(double voltage) { - m_driveVoltage = RebelUtil.constrain( - voltage, -Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE, - Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE); - - m_driveSim.setInputVoltage(m_driveVoltage); - } - - @Override - public void resetPosition() { - drivePositionsMeters = 0; - } -} +package frc.robot.subsystems.drivetrain.swerve; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants; +import frc.robot.lib.util.RebelUtil; + +public class ModuleIOSim implements ModuleIO { + private DCMotor m_gearBoxAngle = DCMotor.getFalcon500Foc(1); + private DCMotor m_gearBoxDrive = DCMotor.getFalcon500Foc(1); + + private FlywheelSim m_angleSim = new FlywheelSim(m_gearBoxAngle, 1, 0.0001); + private FlywheelSim m_driveSim = new FlywheelSim(m_gearBoxDrive, 1, 0.003); + + private static final PIDController m_angleFeedbackController = new PIDController(0.06, 0.0, 0.0002); + private static final PIDController m_driveFeedbackController = new PIDController(0.001, 0, 0); + + private static final SimpleMotorFeedforward m_angleFeedforward = new SimpleMotorFeedforward(0, 0.0000); + private static final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(0, 0.00365, 0.00); + + private double m_angleVoltage = 0; + private double m_driveVoltage = 0; + + private double anglePositionRad = 0; + private double drivePositionsMeters = 0; + + private double prevTime = Timer.getFPGATimestamp(); + + public ModuleIOSim() { + m_angleFeedbackController.setTolerance(Math.toRadians(3)); + m_driveFeedbackController.setTolerance(0.01); + + m_angleFeedbackController.enableContinuousInput(0, 2 * Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + m_angleSim.update(Timer.getFPGATimestamp() - prevTime); + m_driveSim.update(Timer.getFPGATimestamp() - prevTime); + + inputs.angleVelocityRadPerSec = m_angleSim.getAngularVelocityRPM(); + inputs.driveVelocityMps = m_driveSim.getAngularVelocityRPM() * Constants.DrivetrainConstants.kDRIVE_WHEEL_RADIUS_METERS * 2 * Math.PI; + + anglePositionRad += inputs.angleVelocityRadPerSec * (Timer.getFPGATimestamp() - prevTime); + drivePositionsMeters += inputs.driveVelocityMps * (Timer.getFPGATimestamp() - prevTime); + + anglePositionRad = anglePositionRad % (2 * Math.PI); + + inputs.anglePositionRad = anglePositionRad; + inputs.drivePositionMeters = drivePositionsMeters; + + inputs.angleCurrentAmps = m_angleSim.getCurrentDrawAmps(); + inputs.driveCurrentAmps = m_driveSim.getCurrentDrawAmps(); + + inputs.angleTempC = 0; + inputs.driveTempC = 0; + + inputs.angleVoltage = m_angleVoltage; + inputs.driveVoltage = m_driveVoltage; + + prevTime = Timer.getFPGATimestamp(); + } + + @Override + public void setState(SwerveModuleState state) { + if (DriverStation.isTest()) {return;} + + double speed = state.speedMetersPerSecond; + m_driveVoltage = m_driveFeedforward.calculate(speed, Math.signum(speed - m_driveSim.getAngularVelocityRPM())) + + m_driveFeedbackController.calculate(m_driveSim.getAngularVelocityRPM(), speed); + + m_driveVoltage = RebelUtil.constrain( + m_driveVoltage, -Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE, + Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE); + + m_driveSim.setInputVoltage(m_driveVoltage); + + double angle = state.angle.getRadians(); + m_angleVoltage = m_angleFeedforward.calculate(0, 0) + + m_angleFeedbackController.calculate(anglePositionRad, angle); + + m_angleVoltage = RebelUtil.constrain( + m_angleVoltage, -Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE, + Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE); + + m_angleSim.setInputVoltage(m_angleVoltage); + } + + @Override + public void setDriveVoltage(double voltage) { + m_driveVoltage = RebelUtil.constrain( + voltage, -Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE, + Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE); + + m_driveSim.setInputVoltage(m_driveVoltage); + } + + @Override + public void resetPosition() { + drivePositionsMeters = 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.java index 50e55aa..a82080e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/ModuleIOTalon.java @@ -1,175 +1,199 @@ -package frc.robot.subsystems.drivetrain.swerve; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; -import com.ctre.phoenix6.signals.MagnetHealthValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.Constants; -import frc.robot.lib.util.RebelUtil; - -public class ModuleIOTalon implements ModuleIO { - - private TalonFX m_angle; - private TalonFX m_drive; - - private final PIDController m_angleFeedbackController = new PIDController(6, 0.0, 0.01); - private final PIDController m_driveFeedbackController = new PIDController(2, 0, 0); - - private final SimpleMotorFeedforward m_angleFeedforward = new SimpleMotorFeedforward(.23, 0.0000); // .23 - private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(0.22, 2.16); // 0.22 2.16 0 - - private double m_angleVoltage = 0; - - private double m_driveVoltage = 0; - - private double anglePositionRad = 0; - private double drivePositionsMeters = 0; - - private double driveVelocityMps = 0; - - private CANcoder angleEncoder; - private boolean syncQueued = false; - - public ModuleIOTalon(int id) { - CANcoderConfiguration endocerConfig = new CANcoderConfiguration(); - endocerConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; - endocerConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - - switch (id) { - case 0: - angleEncoder = new CANcoder(10, "drivetrain"); - endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kFRONT_LEFT_ANGLE_OFFSET_DEG / 360; - - m_angle = new TalonFX(5, "drivetrain"); - m_drive = new TalonFX(4, "drivetrain"); - - m_angle.setInverted(true); - m_drive.setInverted(false); - break; - case 1: - angleEncoder = new CANcoder(9, "drivetrain"); - endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kFRONT_RIGHT_ANGLE_OFFSET_DEG / 360; - - m_angle = new TalonFX(3, "drivetrain"); - m_drive = new TalonFX(2, "drivetrain"); - - m_angle.setInverted(true); - m_drive.setInverted(false); - break; - case 2: - angleEncoder = new CANcoder(11, "drivetrain"); - endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kBACK_LEFT_ANGLE_OFFSET_DEG / 360; - - m_angle = new TalonFX(7, "drivetrain"); - m_drive = new TalonFX(6, "drivetrain"); - - m_angle.setInverted(true); - m_drive.setInverted(false); - break; - - case 3: - angleEncoder = new CANcoder(8, "drivetrain"); - endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kBACK_RIGHT_ANGLE_OFFSET_DEG / 360; - - m_angle = new TalonFX(0, "drivetrain"); - m_drive = new TalonFX(1, "drivetrain"); - - m_angle.setInverted(true); - m_drive.setInverted(false); - break; - } - - endocerConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; - angleEncoder.getConfigurator().apply(endocerConfig); - - m_angle.setNeutralMode(NeutralModeValue.Brake); - m_drive.setNeutralMode(NeutralModeValue.Brake); - - - - m_angleFeedbackController.setTolerance(Math.toRadians(2)); - m_driveFeedbackController.setTolerance(0.1); - - m_angleFeedbackController.enableContinuousInput(0, 2 * Math.PI); - } - - @Override - public void queueSynchronizeEncoders() { - if (angleEncoder != null) {syncQueued = true;} - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - - inputs.angleVelocityRadPerSec = m_angle.getVelocity().getValueAsDouble() * 2 * Math.PI; - inputs.driveVelocityMps = m_drive.getVelocity().getValueAsDouble() * 2 * Math.PI * Constants.DrivetrainConstants.kDRIVE_WHEEL_RADIUS_METERS * Constants.DrivetrainConstants.kDRIVE_MOTOR_TO_OUTPUT_SHAFT_RATIO; - - driveVelocityMps = inputs.driveVelocityMps; - - anglePositionRad = m_angle.getPosition().getValueAsDouble() * 2 * Math.PI * Constants.DrivetrainConstants.kANGLE_MOTOR_TO_OUTPUT_SHAFT_RATIO; - drivePositionsMeters = m_drive.getPosition().getValueAsDouble() * 2 * Math.PI * Constants.DrivetrainConstants.kDRIVE_WHEEL_RADIUS_METERS * Constants.DrivetrainConstants.kDRIVE_MOTOR_TO_OUTPUT_SHAFT_RATIO; - - inputs.absEncoderRad = (angleEncoder.getPosition().getValueAsDouble() * Math.PI * 2); - - anglePositionRad = inputs.absEncoderRad; - - - inputs.anglePositionRad = anglePositionRad; - inputs.drivePositionMeters = drivePositionsMeters; - - inputs.angleCurrentAmps = m_angle.getSupplyCurrent().getValueAsDouble(); - inputs.driveCurrentAmps = m_drive.getSupplyCurrent().getValueAsDouble(); - - inputs.angleTempC = m_angle.getDeviceTemp().getValueAsDouble(); - inputs.driveTempC = m_drive.getDeviceTemp().getValueAsDouble(); - - inputs.angleVoltage = m_angleVoltage; - inputs.driveVoltage = m_driveVoltage; - } - - @Override - public void setState(SwerveModuleState state) { - if (DriverStation.isTest()) {return;} - - double speed = state.speedMetersPerSecond; - m_driveVoltage = m_driveFeedbackController.calculate(driveVelocityMps, speed); - m_driveVoltage += m_driveFeedforward.calculate(speed, Math.signum(m_driveVoltage)); - - m_driveVoltage = RebelUtil.constrain( - m_driveVoltage, -Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE, - Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE); - - m_drive.setVoltage(m_driveVoltage); - - if (angleEncoder != null && angleEncoder.getMagnetHealth().getValue() == MagnetHealthValue.Magnet_Green && syncQueued) { - m_angle.setPosition(angleEncoder.getAbsolutePosition().getValueAsDouble()); - syncQueued = false; - } - - double angle = state.angle.getRadians(); - - m_angleVoltage = m_angleFeedbackController.calculate(anglePositionRad, angle); - m_angleVoltage += m_angleFeedforward.calculate(angle - anglePositionRad, Math.signum(m_angleVoltage)); // TODO: this is scuffed - - - m_angleVoltage = RebelUtil.constrain( - m_angleVoltage, -Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE, - Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE); - - m_angle.setVoltage(m_angleVoltage); - } - - @Override - public void resetPosition() { - drivePositionsMeters = 0; - m_drive.setPosition(0); - } -} \ No newline at end of file +package frc.robot.subsystems.drivetrain.swerve; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.MagnetHealthValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.lib.util.RebelUtil; + +public class ModuleIOTalon implements ModuleIO { + + private TalonFX m_angle; + private TalonFX m_drive; + + private final PIDController m_angleFeedbackController = new PIDController(4.5, 1, 0.043, .315); // 6 0 .01 //3, 0.0, 0.02 + private final PIDController m_driveFeedbackController = new PIDController(0, 0, 0); // 2 0 0 + + private final SimpleMotorFeedforward m_angleFeedforward = new SimpleMotorFeedforward(0.23, 0.06); // .23 + private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(0.22, 2.2, 0.09); // 0.22 2.16 0 + + private double m_angleVoltage = 0; + + private double m_driveVoltage = 0; + + private double anglePositionRad = 0; + private double drivePositionsMeters = 0; + + private double driveVelocityMps = 0; + + private CANcoder angleEncoder; + private boolean syncQueued = false; + private double prevTime = 0; + private double prevVelocity = 0; + + public ModuleIOTalon(int id) { + CANcoderConfiguration endocerConfig = new CANcoderConfiguration(); + endocerConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; + endocerConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + + switch (id) { + case 0: + angleEncoder = new CANcoder(10, "drivetrain"); + endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kFRONT_LEFT_ANGLE_OFFSET_DEG / 360; + + m_angle = new TalonFX(5, "drivetrain"); + m_drive = new TalonFX(4, "drivetrain"); + + m_angle.setInverted(true); + m_drive.setInverted(false); + break; + case 1: + angleEncoder = new CANcoder(9, "drivetrain"); + endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kFRONT_RIGHT_ANGLE_OFFSET_DEG / 360; + + m_angle = new TalonFX(3, "drivetrain"); + m_drive = new TalonFX(2, "drivetrain"); + + m_angle.setInverted(true); + m_drive.setInverted(false); + break; + case 2: + angleEncoder = new CANcoder(11, "drivetrain"); + endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kBACK_LEFT_ANGLE_OFFSET_DEG / 360; + + m_angle = new TalonFX(7, "drivetrain"); + m_drive = new TalonFX(6, "drivetrain"); + + m_angle.setInverted(true); + m_drive.setInverted(false); + break; + + case 3: + angleEncoder = new CANcoder(8, "drivetrain"); + endocerConfig.MagnetSensor.MagnetOffset = Constants.DrivetrainConstants.kBACK_RIGHT_ANGLE_OFFSET_DEG / 360; + + m_angle = new TalonFX(0, "drivetrain"); + m_drive = new TalonFX(1, "drivetrain"); + + m_angle.setInverted(true); + m_drive.setInverted(false); + break; + } + + endocerConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; + angleEncoder.getConfigurator().apply(endocerConfig); + + m_angle.setNeutralMode(NeutralModeValue.Brake); + m_drive.setNeutralMode(NeutralModeValue.Brake); + + m_angleFeedbackController.setTolerance(Math.toRadians(4)); + m_angleFeedbackController.setIZone(Math.toRadians(35)); + + // m_angleFeedbackController.setIntegratorRange(0); + m_driveFeedbackController.setTolerance(0.1); + + m_angleFeedbackController.enableContinuousInput(0, 2 * Math.PI); + } + + @Override + public void queueSynchronizeEncoders() { + if (angleEncoder != null) {syncQueued = true;} + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + + inputs.angleVelocityRadPerSec = m_angle.getVelocity().getValueAsDouble() * 2 * Math.PI; + inputs.driveVelocityMps = m_drive.getVelocity().getValueAsDouble() * 2 * Math.PI * Constants.DrivetrainConstants.kDRIVE_WHEEL_RADIUS_METERS * Constants.DrivetrainConstants.kDRIVE_MOTOR_TO_OUTPUT_SHAFT_RATIO; + + driveVelocityMps = inputs.driveVelocityMps; + + anglePositionRad = m_angle.getPosition().getValueAsDouble() * 2 * Math.PI * Constants.DrivetrainConstants.kANGLE_MOTOR_TO_OUTPUT_SHAFT_RATIO; + drivePositionsMeters = m_drive.getPosition().getValueAsDouble() * 2 * Math.PI * Constants.DrivetrainConstants.kDRIVE_WHEEL_RADIUS_METERS * Constants.DrivetrainConstants.kDRIVE_MOTOR_TO_OUTPUT_SHAFT_RATIO; + + inputs.absEncoderRad = (angleEncoder.getPosition().getValueAsDouble() * Math.PI * 2) % (Math.PI * 2); + + anglePositionRad = inputs.absEncoderRad; + + + inputs.anglePositionRad = anglePositionRad; + inputs.drivePositionMeters = drivePositionsMeters; + + inputs.angleCurrentAmps = m_angle.getSupplyCurrent().getValueAsDouble(); + inputs.driveCurrentAmps = m_drive.getSupplyCurrent().getValueAsDouble(); + + inputs.angleTempC = m_angle.getDeviceTemp().getValueAsDouble(); + inputs.driveTempC = m_drive.getDeviceTemp().getValueAsDouble(); + + inputs.angleVoltage = m_angleVoltage; + inputs.driveVoltage = m_driveVoltage; + } + + @Override + public void setState(SwerveModuleState state) { + if (DriverStation.isTest()) {return;} + + double speed = state.speedMetersPerSecond; + m_driveVoltage = m_driveFeedbackController.calculate(driveVelocityMps, speed); + m_driveVoltage += m_driveFeedforward.calculate( + driveVelocityMps, + state.speedMetersPerSecond, + (Timer.getFPGATimestamp() - prevTime)); + + + m_driveVoltage = RebelUtil.constrain( + m_driveVoltage, -Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE, + Constants.DrivetrainConstants.kMAX_DRIVE_VOLTAGE); + + m_drive.setVoltage(m_driveVoltage); + + if (angleEncoder != null && angleEncoder.getMagnetHealth().getValue() == MagnetHealthValue.Magnet_Green && syncQueued) { + m_angle.setPosition(angleEncoder.getAbsolutePosition().getValueAsDouble()); + syncQueued = false; + } + + double desiredAngle = state.angle.getRadians(); + + m_angleVoltage = m_angleFeedbackController.calculate(anglePositionRad, desiredAngle); + + double deltaTheta = desiredAngle - anglePositionRad; + if (deltaTheta < -Math.PI) { + deltaTheta = Math.PI * 2 + deltaTheta; + } + else if (deltaTheta > Math.PI) { + deltaTheta = Math.PI * 2 - deltaTheta; + } + Logger.recordOutput("SwerveDrive/deltaTheta", deltaTheta); + double velocityRadSec = (deltaTheta) / (Timer.getFPGATimestamp() - prevTime); + Logger.recordOutput("SwerveDrive/velocityRadSec", velocityRadSec); + m_angleVoltage += m_angleFeedforward.calculate(velocityRadSec, Math.signum(m_angleVoltage)); // TODO: this is scuffed + + m_angleVoltage = RebelUtil.constrain( + m_angleVoltage, -Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE, + Constants.DrivetrainConstants.kMAX_ANGLE_VOLTAGE); + + m_angle.setVoltage(m_angleVoltage); + + prevTime = Timer.getFPGATimestamp(); + } + + @Override + public void resetPosition() { + drivePositionsMeters = 0; + m_drive.setPosition(0); + } +} + diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java index 576c5b4..a71f7be 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java @@ -1,433 +1,438 @@ -package frc.robot.subsystems.drivetrain.swerve; - -import java.util.LinkedList; -import java.util.Queue; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import java.util.function.Consumer; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -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.units.Measure; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.Voltage; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; -import frc.robot.Constants; -import frc.robot.Constants.DrivetrainConstants; -import frc.robot.lib.util.RebelUtil; - -public class SwerveDrive extends SubsystemBase { - - private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( - Constants.DrivetrainConstants.kFRONT_LEFT_POSITION_METERS, - Constants.DrivetrainConstants.kFRONT_RIGHT_POSITION_METERS, - Constants.DrivetrainConstants.kBACK_LEFT_POSITION_METERS, - Constants.DrivetrainConstants.kBACK_RIGHT_POSITION_METERS); - - private ModuleIO[] modules; - private ModuleIOInputsAutoLogged[] moduleInputs = { - new ModuleIOInputsAutoLogged(), - new ModuleIOInputsAutoLogged(), - new ModuleIOInputsAutoLogged(), - new ModuleIOInputsAutoLogged() - }; - - private ChassisSpeeds desiredRobotRelativeSpeeds = new ChassisSpeeds(0,0,0); - private ChassisSpeeds desiredFieldRelativeSpeeds = new ChassisSpeeds(0,0,0); - - private final GyroIO gyroIO; - private GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - - private int syncCount = 0; - - private Rotation2d yaw = new Rotation2d(0); - - private SwerveDrivePoseEstimator m_poseEstimator; - - private final Notifier odometryThread; - private final Lock odometryLock = new ReentrantLock(); - - private ChassisSpeeds measuredRobotRelativeSpeeds = new ChassisSpeeds(0,0,0); - private ChassisSpeeds measuredFieldRelativeSpeeds = new ChassisSpeeds(0,0,0); - - private PIDController m_angleFeedbackController = new PIDController(1, 0, 0);; - private DriveFFController driveFFController = new DriveFFController(); - - private PIDController m_translationalFeedbackController = new PIDController(1, 0, 0); - - private SwerveModuleState[] measuredModuleStates = { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - - private SwerveModuleState[] previousDesiredStates = { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - - private double prevTime = Timer.getFPGATimestamp(); - - private final Config m_sysDriveConfig; - private final SysIdRoutine m_sysDriveIdRoutine; - - private Queue> poseQueue = new LinkedList>(); - - private final SlewRateLimiter vxSlewRateLimiter = - new SlewRateLimiter( - Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED, - Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED, - 0); - private final SlewRateLimiter vySlewRateLimiter = - new SlewRateLimiter( - Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED, - Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED, - 0); - private final SlewRateLimiter omegaSlewRateLimiter = - new SlewRateLimiter( - Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED, - Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_DECELERATION_RADIANS_PER_SECOND_SQUARED, - 0); - private final SlewRateLimiter[] moduleDriveSlewRateLimiters = new SlewRateLimiter[4]; - - public SwerveDrive() { - switch (Constants.currentMode) { - case SIM: - modules = new ModuleIO[] { - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim() - }; - gyroIO = new GyroIO() {}; - - m_sysDriveConfig = new Config(Units.Volts.per(Units.Second).of(1), Units.Volts.of(7), Units.Second.of(10)); - - break; - default: - modules = new ModuleIO[] { - new ModuleIOTalon(0), - new ModuleIOTalon(1), - new ModuleIOTalon(2), - new ModuleIOTalon(3) - }; - gyroIO = new GyroIONavX(); - - m_sysDriveConfig = new Config(Units.Volts.per(Units.Second).of(1), Units.Volts.of(7), Units.Second.of(10)); - - break; - } - - Consumer> driveConsumer = volts -> { - for (int i = 0; i < 4; i++) { - modules[i].setDriveVoltage(volts.baseUnitMagnitude()); - }}; - Consumer logConsumer = log -> { - for (int i = 0; i < 4; i++) { - log.motor("module" + i). - voltage(Units.Volts.of(moduleInputs[i].driveVoltage)). - linearVelocity(Units.MetersPerSecond.of(moduleInputs[i].driveVelocityMps)). - linearPosition(Units.Meters.of(moduleInputs[i].drivePositionMeters)); - }}; - Mechanism m_driveMechanism = new Mechanism(driveConsumer, logConsumer, this); - m_sysDriveIdRoutine = new SysIdRoutine(m_sysDriveConfig, m_driveMechanism); - - m_poseEstimator = new SwerveDrivePoseEstimator( - m_kinematics, - new Rotation2d(), - new SwerveModulePosition[] { - new SwerveModulePosition(0, new Rotation2d()), - new SwerveModulePosition(0, new Rotation2d()), - new SwerveModulePosition(0, new Rotation2d()), - new SwerveModulePosition(0, new Rotation2d()) - }, - new Pose2d()); - - odometryThread = new Notifier(this::updateOdometry); - odometryThread.startPeriodic(0.02); - - for (int i = 0; i < 4; i++) { - moduleDriveSlewRateLimiters[i] = - new SlewRateLimiter( - Constants.DrivetrainConstants.kMAX_MODULE_DRIVE_ACCELERATION_METERS_PER_SECOND, - Constants.DrivetrainConstants.kMAX_MODULE_DRIVE_DECELERATION_METERS_PER_SECOND, - 0); - } - - } - - private void updateInputs() { - gyroIO.updateInputs(gyroInputs); - Logger.processInputs("SwerveDrive/Gyro", gyroInputs); - - for (int i = 0; i < 4; i++) { - modules[i].updateInputs(moduleInputs[i]); - Logger.processInputs("SwerveDrive/Module" + i, moduleInputs[i]); - - measuredModuleStates[i].angle = new Rotation2d(moduleInputs[i].anglePositionRad); - measuredModuleStates[i].speedMetersPerSecond = moduleInputs[i].driveVelocityMps; - } - - Logger.recordOutput("SwerveDrive/measuredModuleStates", measuredModuleStates); - - if (gyroInputs.isConnected) { - yaw = gyroInputs.yaw; - } - else { - yaw = new Rotation2d(yaw.getRadians() + - m_kinematics.toChassisSpeeds( - measuredModuleStates[0], - measuredModuleStates[1], - measuredModuleStates[2], - measuredModuleStates[3]).omegaRadiansPerSecond * (Timer.getFPGATimestamp() - prevTime)); // TODO: discrepancy between prevTime being set and used in the calculation, causing a fixed offset - - yaw = new Rotation2d(yaw.getRadians() % (2 * Math.PI)); - - } - - prevTime = Timer.getFPGATimestamp(); - } - - @Override - public void periodic() { - odometryLock.lock(); - updateOdometry(); - odometryLock.unlock(); - - Pair pair = new Pair(m_poseEstimator.getEstimatedPosition(), Double.valueOf(Timer.getFPGATimestamp())); - poseQueue.add(pair); - if (Timer.getFPGATimestamp() - poseQueue.peek().getSecond().doubleValue() > 1) { - poseQueue.poll(); - } - - m_poseEstimator.update(yaw, new SwerveModulePosition[] { - new SwerveModulePosition(moduleInputs[0].drivePositionMeters, new Rotation2d(moduleInputs[0].anglePositionRad)), - new SwerveModulePosition(moduleInputs[1].drivePositionMeters, new Rotation2d(moduleInputs[1].anglePositionRad)), - new SwerveModulePosition(moduleInputs[2].drivePositionMeters, new Rotation2d(moduleInputs[2].anglePositionRad)), - new SwerveModulePosition(moduleInputs[3].drivePositionMeters, new Rotation2d(moduleInputs[3].anglePositionRad)) - }); - - measuredRobotRelativeSpeeds = m_kinematics.toChassisSpeeds( - measuredModuleStates[0], - measuredModuleStates[1], - measuredModuleStates[2], - measuredModuleStates[3]); - - measuredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(measuredRobotRelativeSpeeds, getPose().getRotation()); - - Logger.recordOutput("SwerveDrive/estimYaw", yaw.getRadians()); - Logger.recordOutput("SwerveDrive/estimatedPose", new double[] { - m_poseEstimator.getEstimatedPosition().getTranslation().getX(), - m_poseEstimator.getEstimatedPosition().getTranslation().getY(), - m_poseEstimator.getEstimatedPosition().getRotation().getRadians() - }); - - desiredFieldRelativeSpeeds.vxMetersPerSecond = - RebelUtil.constrain( - desiredFieldRelativeSpeeds.vxMetersPerSecond, - -DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, - DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND); - desiredFieldRelativeSpeeds.vyMetersPerSecond = - RebelUtil.constrain( - desiredFieldRelativeSpeeds.vyMetersPerSecond, - -DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, - DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND); - desiredFieldRelativeSpeeds.omegaRadiansPerSecond = - RebelUtil.constrain( - desiredFieldRelativeSpeeds.omegaRadiansPerSecond, - -DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, - DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND); - - desiredFieldRelativeSpeeds.vxMetersPerSecond = vxSlewRateLimiter.calculate(desiredFieldRelativeSpeeds.vxMetersPerSecond); - desiredFieldRelativeSpeeds.vyMetersPerSecond = vySlewRateLimiter.calculate(desiredFieldRelativeSpeeds.vyMetersPerSecond); - desiredFieldRelativeSpeeds.omegaRadiansPerSecond = omegaSlewRateLimiter.calculate(desiredFieldRelativeSpeeds.omegaRadiansPerSecond); - - desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredFieldRelativeSpeeds, getPose().getRotation()); - Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); - Logger.recordOutput("SwerveDrive/measuredRobotRelativeSpeeds", measuredRobotRelativeSpeeds); - Logger.recordOutput("SwerveDrive/desiredFieldRelativeSpeeds", desiredFieldRelativeSpeeds); - Logger.recordOutput("SwerveDrive/measuredFieldRelativeSpeeds", measuredFieldRelativeSpeeds); - - - - double estimatedVyDriftMetersPerSecond = driveFFController.calculate(desiredFieldRelativeSpeeds.vxMetersPerSecond, desiredFieldRelativeSpeeds.omegaRadiansPerSecond); - double estimatedVxDriftMetersPerSecond = driveFFController.calculate(desiredFieldRelativeSpeeds.vyMetersPerSecond, desiredFieldRelativeSpeeds.omegaRadiansPerSecond); - Logger.recordOutput("SwerveDrive/estimatedVxDriftMetersPerSecond", estimatedVxDriftMetersPerSecond); - Logger.recordOutput("SwerveDrive/estimatedVyDriftMetersPerSecond", estimatedVyDriftMetersPerSecond); - - ChassisSpeeds correctedSpeeds = desiredFieldRelativeSpeeds; - correctedSpeeds.vxMetersPerSecond = correctedSpeeds.vxMetersPerSecond + estimatedVxDriftMetersPerSecond /* + - m_translationalFeedbackController.calculate(measuredFieldRelativeSpeeds.vxMetersPerSecond, - desiredFieldRelativeSpeeds.vxMetersPerSecond)*/; - correctedSpeeds.vyMetersPerSecond = correctedSpeeds.vyMetersPerSecond - estimatedVyDriftMetersPerSecond; - correctedSpeeds.omegaRadiansPerSecond = correctedSpeeds.omegaRadiansPerSecond /* + m_angleFeedbackController.calculate(measuredFieldRelativeSpeeds.omegaRadiansPerSecond, desiredFieldRelativeSpeeds.omegaRadiansPerSecond)*/; - - correctedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(correctedSpeeds, getPose().getRotation()); - - Logger.recordOutput("SwerveDrive/correctedSpeeds", correctedSpeeds); - - if (Math.abs(desiredRobotRelativeSpeeds.vxMetersPerSecond) <= 0.01 && - Math.abs(desiredRobotRelativeSpeeds.vyMetersPerSecond) <= 0.01 && - Math.abs(desiredRobotRelativeSpeeds.omegaRadiansPerSecond) <= 0.1) { - correctedSpeeds = desiredRobotRelativeSpeeds; - } - - SwerveModuleState[] desiredModuleStates = m_kinematics.toSwerveModuleStates(correctedSpeeds); - - - for (int i = 0; i < 4; i++) { - Logger.recordOutput("SwerveDrive/unoptimizedDesiredModuleStates", desiredModuleStates); - - desiredModuleStates[i] = SwerveModuleState.optimize(desiredModuleStates[i], measuredModuleStates[i].angle); - desiredModuleStates[i].speedMetersPerSecond = - RebelUtil.constrain( - desiredModuleStates[i].speedMetersPerSecond, - -DrivetrainConstants.kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND, - DrivetrainConstants.kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND); - desiredModuleStates[i].speedMetersPerSecond = moduleDriveSlewRateLimiters[i].calculate(desiredModuleStates[i].speedMetersPerSecond); - modules[i].setState(desiredModuleStates[i]); - } - - Logger.recordOutput("SwerveDrive/desiredModuleStates", desiredModuleStates); - } - - public void driveFieldRelative(ChassisSpeeds speeds) { - desiredFieldRelativeSpeeds = speeds; - } - - public void driveRobotRelative(ChassisSpeeds speeds) { - desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, getPose().getRotation()); - } - - public void resetPose(Pose2d pose) { - odometryLock.lock(); - SwerveModulePosition[] positions = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - modules[i].resetPosition(); - positions[i] = new SwerveModulePosition(0.0, new Rotation2d(moduleInputs[i].anglePositionRad)); - } - - // var alliance = DriverStation.getAlliance(); - // if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - // pose = GeometryUtil.flipFieldPose(pose); - // } - - yaw = pose.getRotation(); - Logger.recordOutput("INtialYAW", yaw.getDegrees()); - m_poseEstimator.resetPosition(gyroInputs.yaw, positions, pose); - odometryLock.unlock(); - } - - public void zeroGyro() { - gyroIO.zero(); - gyroIO.setOffset(new Rotation3d()); - } - - public ChassisSpeeds getMeasuredRobotRelativeSpeeds() { - return measuredRobotRelativeSpeeds; - } - - public ChassisSpeeds getMeasuredFeildRelativeSpeeds() { - return measuredFieldRelativeSpeeds; - } - - public Pose2d getPose() { - odometryLock.lock(); - Pose2d estimate = m_poseEstimator.getEstimatedPosition(); - odometryLock.unlock(); - return estimate; - } - - public Pose2d getPoseAtTimestamp(double time) { - double lowestError = Double.MAX_VALUE; - Pose2d pose = poseQueue.peek().getFirst(); - Logger.recordOutput("SwerveDrive/queueLength", poseQueue.size()); - for (Pair pair : poseQueue) { - double currentError = Math.abs(time - pair.getSecond().doubleValue()); - if (currentError < lowestError) { - lowestError = time - pair.getSecond().doubleValue(); - pose = pair.getFirst(); - } - else { - break; - } - } - - return pose; - } - - public SwerveModulePosition[] getSwerveModulePositions() { - return new SwerveModulePosition[] { - new SwerveModulePosition(moduleInputs[0].drivePositionMeters, new Rotation2d(moduleInputs[0].anglePositionRad)), - new SwerveModulePosition(moduleInputs[1].drivePositionMeters, new Rotation2d(moduleInputs[1].anglePositionRad)), - new SwerveModulePosition(moduleInputs[2].drivePositionMeters, new Rotation2d(moduleInputs[2].anglePositionRad)), - new SwerveModulePosition(moduleInputs[3].drivePositionMeters, new Rotation2d(moduleInputs[3].anglePositionRad)) - }; - } - - public void synchronizeModuleEncoders() { - for (ModuleIO m : modules) {m.queueSynchronizeEncoders();} - } - - private void updateOdometry() { - odometryLock.lock(); - try { - updateInputs(); - m_poseEstimator.update(yaw, getSwerveModulePositions()); - - double velocitySum = 0.0; - - for (ModuleIOInputsAutoLogged m : moduleInputs) { - velocitySum += Math.abs(m.driveVelocityMps); - } - - if (velocitySum < 0.01 && ++syncCount > 5) { - synchronizeModuleEncoders(); - syncCount = 0; - } - } - - catch (Exception e) { - odometryLock.unlock(); - throw e; - } - - odometryLock.unlock(); - } - - public void resetOdometry() { - odometryLock.lock(); - m_poseEstimator.resetPosition(gyroInputs.yaw, getSwerveModulePositions(), getPose()); - odometryLock.unlock(); - m_kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, gyroInputs.yaw)); // TODO: this is yaw in radians right? - } - - public Command sysIDriveQuasistatic(SysIdRoutine.Direction direction) { - return m_sysDriveIdRoutine.quasistatic(direction); - } - - public Command sysIdDriveDynamic(SysIdRoutine.Direction direction) { - return m_sysDriveIdRoutine.dynamic(direction); - } +package frc.robot.subsystems.drivetrain.swerve; + +import java.util.LinkedList; +import java.util.Queue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +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.units.Measure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.Constants; +import frc.robot.Constants.DrivetrainConstants; +import frc.robot.lib.util.RebelUtil; + +public class SwerveDrive extends SubsystemBase { + + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( + Constants.DrivetrainConstants.kFRONT_LEFT_POSITION_METERS, + Constants.DrivetrainConstants.kFRONT_RIGHT_POSITION_METERS, + Constants.DrivetrainConstants.kBACK_LEFT_POSITION_METERS, + Constants.DrivetrainConstants.kBACK_RIGHT_POSITION_METERS); + + private ModuleIO[] modules; + private ModuleIOInputsAutoLogged[] moduleInputs = { + new ModuleIOInputsAutoLogged(), + new ModuleIOInputsAutoLogged(), + new ModuleIOInputsAutoLogged(), + new ModuleIOInputsAutoLogged() + }; + + private ChassisSpeeds desiredRobotRelativeSpeeds = new ChassisSpeeds(0,0,0); + private ChassisSpeeds desiredFieldRelativeSpeeds = new ChassisSpeeds(0,0,0); + + private final GyroIO gyroIO; + private GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + + private int syncCount = 0; + + private Rotation2d yaw = new Rotation2d(0); + + private SwerveDrivePoseEstimator m_poseEstimator; + + private final Notifier odometryThread; + private final Lock odometryLock = new ReentrantLock(); + + private ChassisSpeeds measuredRobotRelativeSpeeds = new ChassisSpeeds(0,0,0); + private ChassisSpeeds measuredFieldRelativeSpeeds = new ChassisSpeeds(0,0,0); + + private PIDController m_angleFeedbackController = new PIDController(1, 0, 0);; + private DriveFFController driveFFController = new DriveFFController(); + + private PIDController m_translationalFeedbackController = new PIDController(1, 0, 0); + + private SwerveModuleState[] measuredModuleStates = { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + + private SwerveModuleState[] previousDesiredStates = { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + + private double prevTime = Timer.getFPGATimestamp(); + + private final Config m_sysDriveConfig; + private final SysIdRoutine m_sysDriveIdRoutine; + + private Queue> poseQueue = new LinkedList>(); + + private final SlewRateLimiter vxSlewRateLimiter = + new SlewRateLimiter( + Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED, + -Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED, + 0); + private final SlewRateLimiter vySlewRateLimiter = + new SlewRateLimiter( + Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED, + -Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED, + 0); + private final SlewRateLimiter omegaSlewRateLimiter = + new SlewRateLimiter( + Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED, + -Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_DECELERATION_RADIANS_PER_SECOND_SQUARED, + 0); + private final SlewRateLimiter[] moduleDriveSlewRateLimiters = new SlewRateLimiter[4]; + + private double prevDiscretizationTime = 0; + + public SwerveDrive() { + switch (Constants.currentMode) { + case SIM: + modules = new ModuleIO[] { + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim() + }; + gyroIO = new GyroIO() {}; + + m_sysDriveConfig = new Config(Units.Volts.per(Units.Second).of(1), Units.Volts.of(7), Units.Second.of(10)); + + break; + default: + modules = new ModuleIO[] { + new ModuleIOTalon(0), + new ModuleIOTalon(1), + new ModuleIOTalon(2), + new ModuleIOTalon(3) + }; + gyroIO = new GyroIONavX(); + + m_sysDriveConfig = new Config(Units.Volts.per(Units.Second).of(1), Units.Volts.of(7), Units.Second.of(10)); + + break; + } + + Consumer> driveConsumer = volts -> { + for (int i = 0; i < 4; i++) { + modules[i].setDriveVoltage(volts.baseUnitMagnitude()); + }}; + Consumer logConsumer = log -> { + for (int i = 0; i < 4; i++) { + log.motor("module" + i). + voltage(Units.Volts.of(moduleInputs[i].driveVoltage)). + linearVelocity(Units.MetersPerSecond.of(moduleInputs[i].driveVelocityMps)). + linearPosition(Units.Meters.of(moduleInputs[i].drivePositionMeters)); + }}; + Mechanism m_driveMechanism = new Mechanism(driveConsumer, logConsumer, this); + m_sysDriveIdRoutine = new SysIdRoutine(m_sysDriveConfig, m_driveMechanism); + + m_poseEstimator = new SwerveDrivePoseEstimator( + m_kinematics, + new Rotation2d(), + new SwerveModulePosition[] { + new SwerveModulePosition(0, new Rotation2d()), + new SwerveModulePosition(0, new Rotation2d()), + new SwerveModulePosition(0, new Rotation2d()), + new SwerveModulePosition(0, new Rotation2d()) + }, + new Pose2d()); + + odometryThread = new Notifier(this::updateOdometry); + odometryThread.startPeriodic(0.02); + + for (int i = 0; i < 4; i++) { + moduleDriveSlewRateLimiters[i] = + new SlewRateLimiter( + Constants.DrivetrainConstants.kMAX_MODULE_DRIVE_ACCELERATION_METERS_PER_SECOND, + -Constants.DrivetrainConstants.kMAX_MODULE_DRIVE_DECELERATION_METERS_PER_SECOND, + 0); + } + + } + + private void updateInputs() { + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("SwerveDrive/Gyro", gyroInputs); + + for (int i = 0; i < 4; i++) { + modules[i].updateInputs(moduleInputs[i]); + Logger.processInputs("SwerveDrive/Module" + i, moduleInputs[i]); + + measuredModuleStates[i].angle = new Rotation2d(moduleInputs[i].anglePositionRad); + measuredModuleStates[i].speedMetersPerSecond = moduleInputs[i].driveVelocityMps; + } + + Logger.recordOutput("SwerveDrive/measuredModuleStates", measuredModuleStates); + + if (gyroInputs.isConnected) { + yaw = gyroInputs.yaw; + } + else { + yaw = new Rotation2d(yaw.getRadians() + + m_kinematics.toChassisSpeeds( + measuredModuleStates[0], + measuredModuleStates[1], + measuredModuleStates[2], + measuredModuleStates[3]).omegaRadiansPerSecond * (Timer.getFPGATimestamp() - prevTime)); // TODO: discrepancy between prevTime being set and used in the calculation, causing a fixed offset + + yaw = new Rotation2d(yaw.getRadians() % (2 * Math.PI)); + + } + + prevTime = Timer.getFPGATimestamp(); + } + + @Override + public void periodic() { + odometryLock.lock(); + updateOdometry(); + odometryLock.unlock(); + + Pair pair = new Pair(m_poseEstimator.getEstimatedPosition(), Double.valueOf(Timer.getFPGATimestamp())); + poseQueue.add(pair); + if (Timer.getFPGATimestamp() - poseQueue.peek().getSecond().doubleValue() > 1) { + poseQueue.poll(); + } + + m_poseEstimator.update(yaw, new SwerveModulePosition[] { + new SwerveModulePosition(moduleInputs[0].drivePositionMeters, new Rotation2d(moduleInputs[0].anglePositionRad)), + new SwerveModulePosition(moduleInputs[1].drivePositionMeters, new Rotation2d(moduleInputs[1].anglePositionRad)), + new SwerveModulePosition(moduleInputs[2].drivePositionMeters, new Rotation2d(moduleInputs[2].anglePositionRad)), + new SwerveModulePosition(moduleInputs[3].drivePositionMeters, new Rotation2d(moduleInputs[3].anglePositionRad)) + }); + + measuredRobotRelativeSpeeds = m_kinematics.toChassisSpeeds( + measuredModuleStates[0], + measuredModuleStates[1], + measuredModuleStates[2], + measuredModuleStates[3]); + + measuredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(measuredRobotRelativeSpeeds, getPose().getRotation()); + + Logger.recordOutput("SwerveDrive/estimYaw", yaw.getRadians()); + Logger.recordOutput("SwerveDrive/estimatedPose", new double[] { + m_poseEstimator.getEstimatedPosition().getTranslation().getX(), + m_poseEstimator.getEstimatedPosition().getTranslation().getY(), + m_poseEstimator.getEstimatedPosition().getRotation().getRadians() + }); + + Logger.recordOutput("SwerveDrive/desiredFieldRelativeSpeeds", desiredFieldRelativeSpeeds); + + ChassisSpeeds scaledSpeeds = + RebelUtil.scaleSpeeds( + DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, + desiredFieldRelativeSpeeds); + if (Math.abs(scaledSpeeds.vxMetersPerSecond) < Math.abs(desiredFieldRelativeSpeeds.vxMetersPerSecond) || + Math.abs(scaledSpeeds.vyMetersPerSecond) < Math.abs(desiredFieldRelativeSpeeds.vyMetersPerSecond)) { + desiredFieldRelativeSpeeds = scaledSpeeds; + } + + Logger.recordOutput("SwerveDrive/scaledFieldRelativeSpeeds", scaledSpeeds); + + desiredFieldRelativeSpeeds.omegaRadiansPerSecond = + RebelUtil.constrain( + desiredFieldRelativeSpeeds.omegaRadiansPerSecond, + -DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, + DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND); + + desiredFieldRelativeSpeeds.vxMetersPerSecond = Math.signum(desiredFieldRelativeSpeeds.vxMetersPerSecond) * vxSlewRateLimiter.calculate(Math.abs(desiredFieldRelativeSpeeds.vxMetersPerSecond)); + desiredFieldRelativeSpeeds.vyMetersPerSecond = Math.signum(desiredFieldRelativeSpeeds.vyMetersPerSecond) * vySlewRateLimiter.calculate(Math.abs(desiredFieldRelativeSpeeds.vyMetersPerSecond)); + desiredFieldRelativeSpeeds.omegaRadiansPerSecond = Math.signum(desiredFieldRelativeSpeeds.omegaRadiansPerSecond) * omegaSlewRateLimiter.calculate(Math.abs(desiredFieldRelativeSpeeds.omegaRadiansPerSecond)); + + desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredFieldRelativeSpeeds, getPose().getRotation()); + Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); + Logger.recordOutput("SwerveDrive/measuredRobotRelativeSpeeds", measuredRobotRelativeSpeeds); + Logger.recordOutput("SwerveDrive/measuredFieldRelativeSpeeds", measuredFieldRelativeSpeeds); + + double estimatedVyDriftMetersPerSecond = driveFFController.calculate(desiredFieldRelativeSpeeds.vxMetersPerSecond, desiredFieldRelativeSpeeds.omegaRadiansPerSecond); + double estimatedVxDriftMetersPerSecond = driveFFController.calculate(desiredFieldRelativeSpeeds.vyMetersPerSecond, desiredFieldRelativeSpeeds.omegaRadiansPerSecond); + Logger.recordOutput("SwerveDrive/estimatedVxDriftMetersPerSecond", estimatedVxDriftMetersPerSecond); + Logger.recordOutput("SwerveDrive/estimatedVyDriftMetersPerSecond", estimatedVyDriftMetersPerSecond); + + ChassisSpeeds correctedSpeeds = desiredFieldRelativeSpeeds; + correctedSpeeds.vxMetersPerSecond = correctedSpeeds.vxMetersPerSecond + estimatedVxDriftMetersPerSecond /* + + m_translationalFeedbackController.calculate(measuredFieldRelativeSpeeds.vxMetersPerSecond, + desiredFieldRelativeSpeeds.vxMetersPerSecond)*/; + correctedSpeeds.vyMetersPerSecond = correctedSpeeds.vyMetersPerSecond - estimatedVyDriftMetersPerSecond; + correctedSpeeds.omegaRadiansPerSecond = correctedSpeeds.omegaRadiansPerSecond /* + m_angleFeedbackController.calculate(measuredFieldRelativeSpeeds.omegaRadiansPerSecond, desiredFieldRelativeSpeeds.omegaRadiansPerSecond)*/; + + correctedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(correctedSpeeds, getPose().getRotation()); + + Logger.recordOutput("SwerveDrive/correctedSpeeds", correctedSpeeds); + + if (Math.abs(desiredRobotRelativeSpeeds.vxMetersPerSecond) <= 0.01 && + Math.abs(desiredRobotRelativeSpeeds.vyMetersPerSecond) <= 0.01 && + Math.abs(desiredRobotRelativeSpeeds.omegaRadiansPerSecond) <= 0.1) { + correctedSpeeds = desiredRobotRelativeSpeeds; + } + + SwerveModuleState[] desiredModuleStates = m_kinematics.toSwerveModuleStates(correctedSpeeds); + double discretizationTime = Timer.getFPGATimestamp(); + ChassisSpeeds.discretize(correctedSpeeds, discretizationTime - prevDiscretizationTime); + prevDiscretizationTime = discretizationTime; + + for (int i = 0; i < 4; i++) { + desiredModuleStates[i] = SwerveModuleState.optimize(desiredModuleStates[i], measuredModuleStates[i].angle); + desiredModuleStates[i].speedMetersPerSecond = + RebelUtil.constrain( + desiredModuleStates[i].speedMetersPerSecond, + -DrivetrainConstants.kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND, + DrivetrainConstants.kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND); + desiredModuleStates[i].speedMetersPerSecond = Math.signum(desiredModuleStates[i].speedMetersPerSecond) * moduleDriveSlewRateLimiters[i].calculate(Math.abs(desiredModuleStates[i].speedMetersPerSecond)); + modules[i].setState(desiredModuleStates[i]); + + Logger.recordOutput("SwerveDrive/unoptimizedDesiredModuleStates", desiredModuleStates); + + } + + Logger.recordOutput("SwerveDrive/desiredModuleStates", desiredModuleStates); + } + + public void driveFieldRelative(ChassisSpeeds speeds) { + desiredFieldRelativeSpeeds = speeds; + } + + public void driveRobotRelative(ChassisSpeeds speeds) { + desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, getPose().getRotation()); + } + + public void resetPose(Pose2d pose) { + odometryLock.lock(); + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + modules[i].resetPosition(); + positions[i] = new SwerveModulePosition(0.0, new Rotation2d(moduleInputs[i].anglePositionRad)); + } + + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + // pose = GeometryUtil.flipFieldPose(pose); + // } + + yaw = pose.getRotation(); + Logger.recordOutput("INtialYAW", yaw.getDegrees()); + m_poseEstimator.resetPosition(gyroInputs.yaw, positions, pose); + odometryLock.unlock(); + } + + public void zeroGyro() { + gyroIO.zero(); + gyroIO.setOffset(new Rotation3d()); + } + + public ChassisSpeeds getMeasuredRobotRelativeSpeeds() { + return measuredRobotRelativeSpeeds; + } + + public ChassisSpeeds getMeasuredFeildRelativeSpeeds() { + return measuredFieldRelativeSpeeds; + } + + public Pose2d getPose() { + odometryLock.lock(); + Pose2d estimate = m_poseEstimator.getEstimatedPosition(); + odometryLock.unlock(); + return estimate; + } + + public Pose2d getPoseAtTimestamp(double time) { + double lowestError = Double.MAX_VALUE; + Pose2d pose = poseQueue.peek().getFirst(); + Logger.recordOutput("SwerveDrive/queueLength", poseQueue.size()); + for (Pair pair : poseQueue) { + double currentError = Math.abs(time - pair.getSecond().doubleValue()); + if (currentError < lowestError) { + lowestError = time - pair.getSecond().doubleValue(); + pose = pair.getFirst(); + } + else { + break; + } + } + + return pose; + } + + public SwerveModulePosition[] getSwerveModulePositions() { + return new SwerveModulePosition[] { + new SwerveModulePosition(moduleInputs[0].drivePositionMeters, new Rotation2d(moduleInputs[0].anglePositionRad)), + new SwerveModulePosition(moduleInputs[1].drivePositionMeters, new Rotation2d(moduleInputs[1].anglePositionRad)), + new SwerveModulePosition(moduleInputs[2].drivePositionMeters, new Rotation2d(moduleInputs[2].anglePositionRad)), + new SwerveModulePosition(moduleInputs[3].drivePositionMeters, new Rotation2d(moduleInputs[3].anglePositionRad)) + }; + } + + public void synchronizeModuleEncoders() { + for (ModuleIO m : modules) {m.queueSynchronizeEncoders();} + } + + private void updateOdometry() { + odometryLock.lock(); + try { + updateInputs(); + m_poseEstimator.update(yaw, getSwerveModulePositions()); + + double velocitySum = 0.0; + + for (ModuleIOInputsAutoLogged m : moduleInputs) { + velocitySum += Math.abs(m.driveVelocityMps); + } + + if (velocitySum < 0.01 && ++syncCount > 5) { + synchronizeModuleEncoders(); + syncCount = 0; + } + } + + catch (Exception e) { + odometryLock.unlock(); + throw e; + } + + odometryLock.unlock(); + } + + public void resetOdometry() { + odometryLock.lock(); + m_poseEstimator.resetPosition(gyroInputs.yaw, getSwerveModulePositions(), getPose()); + odometryLock.unlock(); + m_kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, gyroInputs.yaw)); // TODO: this is yaw in radians right? + } + + public Command sysIDriveQuasistatic(SysIdRoutine.Direction direction) { + return m_sysDriveIdRoutine.quasistatic(direction); + } + + public Command sysIdDriveDynamic(SysIdRoutine.Direction direction) { + return m_sysDriveIdRoutine.dynamic(direction); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetector.java b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetector.java index e181394..e18f148 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetector.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetector.java @@ -1,148 +1,149 @@ -package frc.robot.subsystems.drivetrain.vision; - -import java.util.LinkedList; -import java.util.Queue; -import java.util.concurrent.DelayQueue; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; - -public class NoteDetector extends SubsystemBase { - private final NoteDetectorIOInputsAutoLogged inputs = new NoteDetectorIOInputsAutoLogged(); - private NoteDetectorIO io; - private final SwerveDrive swerveDrive; - - private Translation2d prevSample = new Translation2d(); - - private boolean[] checked = new boolean[] { - false, - false, - false, - false, - false, - true, - false, - false - }; - - public NoteDetector(SwerveDrive swerveDrive) { - this.swerveDrive = swerveDrive; - switch(Constants.currentMode) { - case SIM: - io = new NoteDetectorIOSim(swerveDrive); - break; - default: - io = new NoteDetectorIOReal(); - break; - } - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("NoteDetector", inputs); - - Logger.recordOutput("NoteDetector/chekedNotes", checked); - Logger.recordOutput("NoteDetector/estimFeildRelativeNotePose", new Translation3d(getNoteFieldRelativePose().getX(), getNoteFieldRelativePose().getY(),0)); - - } - - public boolean hasTargets() { - return inputs.hasTargets; - } - - public Translation2d getNoteRobotRelativePose() { - return getNoteFieldRelativePose().minus(swerveDrive.getPose().getTranslation()).rotateBy(swerveDrive.getPose().getRotation()); - } - - public Translation2d getNoteFieldRelativePose() { - if (!inputs.hasTargets) { return prevSample; } - if (Constants.currentMode == Constants.Mode.SIM && inputs.hasTargets) { - prevSample = inputs.bestNote; - return inputs.bestNote; - } - Pose2d delayedPose = swerveDrive.getPoseAtTimestamp(Timer.getFPGATimestamp() - inputs.totalLatencySeconds); - Logger.recordOutput("NoteDetector/delayedPose", delayedPose); - - double pitch = Math.PI / 2 - (Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getY() - inputs.tyRadians); - double yaw = inputs.txRadians; - - Logger.recordOutput("NoteDetector/pitchDeg", Math.toDegrees(pitch)); - Logger.recordOutput("NoteDetector/yawDeg", Math.toDegrees(yaw)); - - double xMeters = Math.tan(pitch) * Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getZ(); - double yMeters = xMeters * Math.tan(yaw); - - Logger.recordOutput("NoteDetector/xMeters", xMeters); - Logger.recordOutput("NoteDetector/yMeters", yMeters); - - Rotation2d robotYaw = delayedPose.getRotation(); - Translation2d cameraTranslation2d = Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().toTranslation2d(); - cameraTranslation2d = cameraTranslation2d.rotateBy(robotYaw); - cameraTranslation2d = cameraTranslation2d.plus(delayedPose.getTranslation()); - - Translation3d cameraTranslation = new Translation3d( - cameraTranslation2d.getX(), - cameraTranslation2d.getY(), - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().getZ()); - Rotation3d cameraRotation = new Rotation3d( - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getX(), - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getY(), - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getZ() + robotYaw.getRadians()); - - Pose3d cameraPose = new Pose3d(cameraTranslation, cameraRotation); - - Logger.recordOutput("NoteDetector/cameraPose", cameraPose); - - - Translation2d realtiveTranslation2d = new Translation2d(xMeters, yMeters); - Logger.recordOutput("NoteDetector/estimRobotRelativeNotePose", realtiveTranslation2d); - Translation2d absoluteTranslation2d = realtiveTranslation2d. - rotateBy(new Rotation2d(cameraPose.getRotation().getZ())); - - absoluteTranslation2d = delayedPose.getTranslation().minus(absoluteTranslation2d); - prevSample = absoluteTranslation2d; - - // TODO: Fixed sim :skull: - - - return absoluteTranslation2d; - } - - public double getDrivetrainDistToNote() { - return swerveDrive.getPose().getTranslation().getDistance(getNoteFieldRelativePose()); - } - - public double intakeDistToNote() { - Rotation2d robotYaw = swerveDrive.getPose().getRotation(); - Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); - intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); - intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); - - Translation3d intakeTranslation3d = new Translation3d( - intakeTranslation2d.getX(), - intakeTranslation2d.getY(), - Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); - - return intakeTranslation3d.getDistance(new Translation3d(getNoteFieldRelativePose().getX(), getNoteFieldRelativePose().getY(), 0)); - } - - public boolean checked(int index) { - return checked[index]; - } - - public void setCheked(int index) { - checked[index] = true; - } -} +package frc.robot.subsystems.drivetrain.vision; + +import java.util.LinkedList; +import java.util.Queue; +import java.util.concurrent.DelayQueue; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; + +public class NoteDetector extends SubsystemBase { + private final NoteDetectorIOInputsAutoLogged inputs = new NoteDetectorIOInputsAutoLogged(); + private NoteDetectorIO io; + private final SwerveDrive swerveDrive; + + private Translation2d prevSample = new Translation2d(); + + private boolean[] checked = new boolean[] { + false, + false, + false, + false, + false, + true, + false, + false + }; + + public NoteDetector(SwerveDrive swerveDrive) { + this.swerveDrive = swerveDrive; + switch(Constants.currentMode) { + case SIM: + io = new NoteDetectorIOSim(swerveDrive); + break; + default: + io = new NoteDetectorIOReal(); + break; + } + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("NoteDetector", inputs); + + Logger.recordOutput("NoteDetector/chekedNotes", checked); + Logger.recordOutput("NoteDetector/estimFeildRelativeNotePose", new Translation3d(getNoteFieldRelativePose().getX(), getNoteFieldRelativePose().getY(),0)); + + } + + public boolean hasTargets() { + return inputs.hasTargets; + } + + public Translation2d getNoteRobotRelativePose() { + return getNoteFieldRelativePose().minus(swerveDrive.getPose().getTranslation()).rotateBy(swerveDrive.getPose().getRotation()); + } + + public Translation2d getNoteFieldRelativePose() { + if (!inputs.hasTargets) { return prevSample; } + if (Constants.currentMode == Constants.Mode.SIM && inputs.hasTargets) { + prevSample = inputs.bestNote; + return inputs.bestNote; + } + Pose2d delayedPose = swerveDrive.getPoseAtTimestamp(Timer.getFPGATimestamp() - inputs.totalLatencySeconds); + delayedPose = new Pose2d(new Translation2d(Math.abs(delayedPose.getX()), Math.abs(delayedPose.getY())), delayedPose.getRotation()); + + Logger.recordOutput("NoteDetector/delayedPose", delayedPose); + + double pitch = Math.PI / 2 - (Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getY() - inputs.tyRadians); + double yaw = inputs.txRadians; + + Logger.recordOutput("NoteDetector/pitchDeg", Math.toDegrees(pitch)); + Logger.recordOutput("NoteDetector/yawDeg", Math.toDegrees(yaw)); + + double xMeters = Math.tan(pitch) * Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getZ(); + double yMeters = xMeters * Math.tan(yaw); + + Logger.recordOutput("NoteDetector/xMeters", xMeters); + Logger.recordOutput("NoteDetector/yMeters", yMeters); + + Rotation2d robotYaw = delayedPose.getRotation(); + Translation2d cameraTranslation2d = Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().toTranslation2d(); + cameraTranslation2d = cameraTranslation2d.rotateBy(robotYaw); + cameraTranslation2d = cameraTranslation2d.plus(delayedPose.getTranslation()); + + Translation3d cameraTranslation = new Translation3d( + cameraTranslation2d.getX(), + cameraTranslation2d.getY(), + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().getZ()); + Rotation3d cameraRotation = new Rotation3d( + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getX(), + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getY(), + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getZ() + robotYaw.getRadians()); + + Pose3d cameraPose = new Pose3d(cameraTranslation, cameraRotation); + + Logger.recordOutput("NoteDetector/cameraPose", cameraPose); + + + Translation2d realtiveTranslation2d = new Translation2d(xMeters, yMeters); + Logger.recordOutput("NoteDetector/estimRobotRelativeNotePose", realtiveTranslation2d); + Translation2d absoluteTranslation2d = realtiveTranslation2d. + rotateBy(new Rotation2d(cameraPose.getRotation().getZ())); + + absoluteTranslation2d = delayedPose.getTranslation().minus(absoluteTranslation2d); + prevSample = absoluteTranslation2d; + + // TODO: Fixed sim :skull: + + return absoluteTranslation2d; + } + + public double getDrivetrainDistToNote() { + return swerveDrive.getPose().getTranslation().getDistance(getNoteFieldRelativePose()); + } + + public double intakeDistToNote() { + Rotation2d robotYaw = swerveDrive.getPose().getRotation(); + Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); + intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); + intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); + + Translation3d intakeTranslation3d = new Translation3d( + intakeTranslation2d.getX(), + intakeTranslation2d.getY(), + Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); + + return intakeTranslation3d.getDistance(new Translation3d(getNoteFieldRelativePose().getX(), getNoteFieldRelativePose().getY(), 0)); + } + + public boolean checked(int index) { + return checked[index]; + } + + public void setCheked(int index) { + checked[index] = true; + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIO.java b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIO.java index 5d98c8b..488f6b9 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIO.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIO.java @@ -1,20 +1,20 @@ -package frc.robot.subsystems.drivetrain.vision; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.geometry.Translation2d; - -public interface NoteDetectorIO { - @AutoLog - public static class NoteDetectorIOInputs { - public double txRadians = 0; - public double tyRadians = 0; - public boolean hasTargets = false; - public Translation2d bestNote = new Translation2d(); - public double totalLatencySeconds = 0; - } - - public default void updateInputs(NoteDetectorIOInputs inputs) {}; - public default Translation2d getPose() { return new Translation2d(); }; - -} +package frc.robot.subsystems.drivetrain.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Translation2d; + +public interface NoteDetectorIO { + @AutoLog + public static class NoteDetectorIOInputs { + public double txRadians = 0; + public double tyRadians = 0; + public boolean hasTargets = false; + public Translation2d bestNote = new Translation2d(); + public double totalLatencySeconds = 0; + } + + public default void updateInputs(NoteDetectorIOInputs inputs) {}; + public default Translation2d getPose() { return new Translation2d(); }; + +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOReal.java index 8a1f2b2..9b58a5c 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOReal.java @@ -1,19 +1,22 @@ -package frc.robot.subsystems.drivetrain.vision; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class NoteDetectorIOReal implements NoteDetectorIO { - private NetworkTable llTable; - public NoteDetectorIOReal() { - llTable = NetworkTableInstance.getDefault().getTable("limelight"); - - } - public void updateInputs(NoteDetectorIOInputs inputs) { - inputs.hasTargets = llTable.getEntry("tv").getDouble(0) == 1; - inputs.txRadians = -Math.toRadians(llTable.getEntry("tx").getDouble(0)); - inputs.tyRadians = Math.toRadians(llTable.getEntry("ty").getDouble(0)); - inputs.totalLatencySeconds = (llTable.getEntry("cl").getDouble(0) + llTable.getEntry("tl").getDouble(0))/1000; - } - +package frc.robot.subsystems.drivetrain.vision; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class NoteDetectorIOReal implements NoteDetectorIO { + private NetworkTable llTable; + private static final Debouncer debouncer = new Debouncer(0.2); + + public NoteDetectorIOReal() { + llTable = NetworkTableInstance.getDefault().getTable("limelight"); + + } + public void updateInputs(NoteDetectorIOInputs inputs) { + inputs.hasTargets = debouncer.calculate(llTable.getEntry("tv").getDouble(0) == 1); + inputs.txRadians = -Math.toRadians(llTable.getEntry("tx").getDouble(0)); + inputs.tyRadians = Math.toRadians(llTable.getEntry("ty").getDouble(0)); + inputs.totalLatencySeconds = (llTable.getEntry("cl").getDouble(0) + llTable.getEntry("tl").getDouble(0))/1000; + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOSim.java index e4e9363..c839c4e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/vision/NoteDetectorIOSim.java @@ -1,87 +1,87 @@ -package frc.robot.subsystems.drivetrain.vision; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import frc.robot.Constants; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; - -public class NoteDetectorIOSim implements NoteDetectorIO { - private final SwerveDrive swerveDrive; - private final boolean[] taken = new boolean[] {true, true, false, false, false, false, false, false}; - public NoteDetectorIOSim(SwerveDrive swerveDrive) { - this.swerveDrive = swerveDrive; - } - @Override - public void updateInputs(NoteDetectorIOInputs inputs) { - Rotation2d robotYaw = swerveDrive.getPose().getRotation(); - Translation2d cameraTranslation2d = Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().toTranslation2d(); - cameraTranslation2d = cameraTranslation2d.rotateBy(robotYaw); - cameraTranslation2d = cameraTranslation2d.plus(swerveDrive.getPose().getTranslation()); - - Translation3d cameraTranslation = new Translation3d( - cameraTranslation2d.getX(), - cameraTranslation2d.getY(), - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().getZ()); - Rotation3d cameraRotation = new Rotation3d( - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getX(), - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getY(), - Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getZ() + robotYaw.getRadians()); - - Pose3d cameraPose = new Pose3d(cameraTranslation, cameraRotation); - - Logger.recordOutput("NoteDetector/cameraPose", cameraPose); - - double minDist = Double.MAX_VALUE; - inputs.hasTargets = false; - for (int i = 0; i < Constants.FieldConstants.kNOTE_ARR.length; i++) { - if (taken[i]) { - inputs.hasTargets = false; - continue; - } - - Translation3d closestNote = Constants.FieldConstants.kNOTE_ARR[i]; - double vxAngle = ((cameraPose.getRotation().getZ() - - Math.atan2(closestNote.getY() - cameraPose.getY(), - closestNote.getX() - cameraPose.getX()))) % (2 * Math.PI); - - double x = Math.sqrt(Math.pow(closestNote.getY() - cameraPose.getY(), 2) + - Math.pow(closestNote.getX() - cameraPose.getX(), 2)); - - double vyAngle = ((Math.atan2(closestNote.getZ() - cameraPose.getZ(), x) + - cameraPose.getRotation().getY())) % (2 * Math.PI); - - - if (vxAngle <= -Math.PI) { - vxAngle += 2 * Math.PI; - } - else if (vxAngle >= 180) { - vxAngle -= 2 * Math.PI; - } - Logger.recordOutput("NoteDetector/txAngleDeg", Math.toDegrees(vxAngle)); - Logger.recordOutput("NoteDetector/tyAngleDeg", Math.toDegrees(vyAngle)); - - Logger.recordOutput("NoteDetector/dist", closestNote.getDistance(cameraPose.getTranslation())); - - if (Math.abs(vxAngle) <= Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_FOV_X_RAD/2 && - Math.abs(vyAngle) <= Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_FOV_Y_RAD/2 && - closestNote.getDistance(cameraPose.getTranslation()) <= Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_MAX_RANGE_METERS) { - inputs.hasTargets = true; - inputs.txRadians = -vxAngle; - inputs.tyRadians = vyAngle; - if (closestNote.getDistance(cameraPose.getTranslation()) < minDist) { - inputs.hasTargets = true; - inputs.bestNote = closestNote.toTranslation2d(); - minDist = closestNote.getDistance(cameraPose.getTranslation()); - } - } - - } - - - } -} +package frc.robot.subsystems.drivetrain.vision; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.Constants; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; + +public class NoteDetectorIOSim implements NoteDetectorIO { + private final SwerveDrive swerveDrive; + private final boolean[] taken = new boolean[] {true, true, false, false, false, false, false, false}; + public NoteDetectorIOSim(SwerveDrive swerveDrive) { + this.swerveDrive = swerveDrive; + } + @Override + public void updateInputs(NoteDetectorIOInputs inputs) { + Rotation2d robotYaw = swerveDrive.getPose().getRotation(); + Translation2d cameraTranslation2d = Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().toTranslation2d(); + cameraTranslation2d = cameraTranslation2d.rotateBy(robotYaw); + cameraTranslation2d = cameraTranslation2d.plus(swerveDrive.getPose().getTranslation()); + + Translation3d cameraTranslation = new Translation3d( + cameraTranslation2d.getX(), + cameraTranslation2d.getY(), + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getTranslation().getZ()); + Rotation3d cameraRotation = new Rotation3d( + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getX(), + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getY(), + Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_POSE.getRotation().getZ() + robotYaw.getRadians()); + + Pose3d cameraPose = new Pose3d(cameraTranslation, cameraRotation); + + Logger.recordOutput("NoteDetector/cameraPose", cameraPose); + + double minDist = Double.MAX_VALUE; + inputs.hasTargets = false; + for (int i = 0; i < Constants.FieldConstants.kNOTE_ARR.length; i++) { + if (taken[i]) { + inputs.hasTargets = false; + continue; + } + + Translation3d closestNote = Constants.FieldConstants.kNOTE_ARR[i]; + double vxAngle = ((cameraPose.getRotation().getZ() - + Math.atan2(closestNote.getY() - cameraPose.getY(), + closestNote.getX() - cameraPose.getX()))) % (2 * Math.PI); + + double x = Math.sqrt(Math.pow(closestNote.getY() - cameraPose.getY(), 2) + + Math.pow(closestNote.getX() - cameraPose.getX(), 2)); + + double vyAngle = ((Math.atan2(closestNote.getZ() - cameraPose.getZ(), x) + + cameraPose.getRotation().getY())) % (2 * Math.PI); + + + if (vxAngle <= -Math.PI) { + vxAngle += 2 * Math.PI; + } + else if (vxAngle >= 180) { + vxAngle -= 2 * Math.PI; + } + Logger.recordOutput("NoteDetector/txAngleDeg", Math.toDegrees(vxAngle)); + Logger.recordOutput("NoteDetector/tyAngleDeg", Math.toDegrees(vyAngle)); + + Logger.recordOutput("NoteDetector/dist", closestNote.getDistance(cameraPose.getTranslation())); + + if (Math.abs(vxAngle) <= Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_FOV_X_RAD/2 && + Math.abs(vyAngle) <= Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_FOV_Y_RAD/2 && + closestNote.getDistance(cameraPose.getTranslation()) <= Constants.VisionConstants.kNOTE_DETECTOR_CAMERA_MAX_RANGE_METERS) { + inputs.hasTargets = true; + inputs.txRadians = -vxAngle; + inputs.tyRadians = vyAngle; + if (closestNote.getDistance(cameraPose.getTranslation()) < minDist) { + inputs.hasTargets = true; + inputs.bestNote = closestNote.toTranslation2d(); + minDist = closestNote.getDistance(cameraPose.getTranslation()); + } + } + + } + + + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index ea2f982..6595108 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,87 +1,87 @@ -package frc.robot.subsystems.elevator; - -import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Elevator extends SubsystemBase{ - - private static ElevatorIO io; - private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); - PIDController positionFeedBackController; - ElevatorFeedforward positionFeedForwardController; - ElevatorFeedforward climbFeedForwardController; - - // PIDController velocityFeedBackController; - // ElevatorFeedforward velocityFeedForwardController; //Literally never gonna be used - private static final double kPID_TOLERANCE_METERS = 0.01; //this is 1cm - private static final double kCLIMB_KG = 12; - - private static Elevator instance = null; - private double goalPositionMeters = 0; - public Elevator(ElevatorIO io) { - Elevator.io = io; - positionFeedBackController = new PIDController(12, 0, 0); // 12, 2, 0 - positionFeedForwardController = new ElevatorFeedforward(0.13, 0.06, 0.2); //0.33, 0.14, 0 - climbFeedForwardController = new ElevatorFeedforward(0,0,0); - - - positionFeedBackController.setTolerance(kPID_TOLERANCE_METERS); - - io.configureController(positionFeedForwardController, positionFeedBackController, climbFeedForwardController, kCLIMB_KG); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Elevator", inputs); - io.setHeightMeters(goalPositionMeters); - // if(isRaw){ - // io.setHeightMeters(goalPositionMeters); - // } - // else if (isShooterHeight) { - // Logger.recordOutput("Elevator/desiredShooterHeight", goalPositionMeters); - // io.setHeightMeters(goalPositionMeters); - // } - // else { - // Logger.recordOutput("Elevator/desiredClimberHeight", goalPositionMeters); - // io.setHeightMeters(goalPositionMeters); - // } - Logger.recordOutput("Elevator/desiredShooterHeight", goalPositionMeters); - } - - public void setHeightMeters(double goalPositionMeters) { - this.goalPositionMeters = goalPositionMeters; - } - - public void setVoltage(double voltage){ - io.setVoltage(voltage); - return; - } - - public double getShooterHeightMeters() { - return inputs.shooterHeightMeters; - } - public double getClimberHeightMeters(){ - return inputs.climberHeightMeters; - } - - public void zeroHeight() { - io.zeroHeight(); - } - public boolean reachedSetpoint() { - return inputs.reachedSetpoint; - } - public static Elevator getInstance(){ - if(instance == null){ - return new Elevator(Elevator.io); - } - return instance; - } - //Sets and returns instance, the only reason why it returns is to just make our life easier. - public static Elevator setInstance(Elevator inst){ - instance = inst; - return inst; - } -} +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Elevator extends SubsystemBase{ + + private static ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + PIDController positionFeedBackController; + ElevatorFeedforward positionFeedForwardController; + ElevatorFeedforward climbFeedForwardController; + + // PIDController velocityFeedBackController; + // ElevatorFeedforward velocityFeedForwardController; //Literally never gonna be used + private static final double kPID_TOLERANCE_METERS = 0.01; //this is 1cm + private static final double kCLIMB_KG = 12; + + private static Elevator instance = null; + private double goalPositionMeters = 0; + public Elevator(ElevatorIO io) { + Elevator.io = io; + positionFeedBackController = new PIDController(12, 0, 0); // 12, 2, 0 + positionFeedForwardController = new ElevatorFeedforward(0.13, 0.06, 0.2); //0.33, 0.14, 0 + climbFeedForwardController = new ElevatorFeedforward(0,0,0); + + + positionFeedBackController.setTolerance(kPID_TOLERANCE_METERS); + + io.configureController(positionFeedForwardController, positionFeedBackController, climbFeedForwardController, kCLIMB_KG); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Elevator", inputs); + io.setHeightMeters(goalPositionMeters); + // if(isRaw){ + // io.setHeightMeters(goalPositionMeters); + // } + // else if (isShooterHeight) { + // Logger.recordOutput("Elevator/desiredShooterHeight", goalPositionMeters); + // io.setHeightMeters(goalPositionMeters); + // } + // else { + // Logger.recordOutput("Elevator/desiredClimberHeight", goalPositionMeters); + // io.setHeightMeters(goalPositionMeters); + // } + Logger.recordOutput("Elevator/desiredShooterHeight", goalPositionMeters); + } + + public void setHeightMeters(double goalPositionMeters) { + this.goalPositionMeters = goalPositionMeters; + } + + public void setVoltage(double voltage){ + io.setVoltage(voltage); + return; + } + + public double getShooterHeightMeters() { + return inputs.shooterHeightMeters; + } + public double getClimberHeightMeters(){ + return inputs.climberHeightMeters; + } + + public void zeroHeight() { + io.zeroHeight(); + } + public boolean reachedSetpoint() { + return inputs.reachedSetpoint; + } + public static Elevator getInstance(){ + if(instance == null){ + return new Elevator(Elevator.io); + } + return instance; + } + //Sets and returns instance, the only reason why it returns is to just make our life easier. + public static Elevator setInstance(Elevator inst){ + instance = inst; + return inst; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 24a860e..c218685 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -1,29 +1,29 @@ -package frc.robot.subsystems.elevator; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; - -public interface ElevatorIO { - @AutoLog - public static class ElevatorIOInputs { - public double shooterHeightMeters; - public double climberHeightMeters; - public double voltageOut; - public boolean reachedSetpoint; - public double goalPositionMeters; - - } - - public default void updateInputs(ElevatorIOInputs inputs) {} - - public default void setHeightMeters(double goalPositionMeters) {} - - public default void configureController(ElevatorFeedforward pff, PIDController pfb, ElevatorFeedforward cff, double kCLIMB_KG) {} - - public default void setVoltage(double voltage) {} - - public default void zeroHeight() {} - -} +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; + +public interface ElevatorIO { + @AutoLog + public static class ElevatorIOInputs { + public double shooterHeightMeters; + public double climberHeightMeters; + public double voltageOut; + public boolean reachedSetpoint; + public double goalPositionMeters; + + } + + public default void updateInputs(ElevatorIOInputs inputs) {} + + public default void setHeightMeters(double goalPositionMeters) {} + + public default void configureController(ElevatorFeedforward pff, PIDController pfb, ElevatorFeedforward cff, double kCLIMB_KG) {} + + public default void setVoltage(double voltage) {} + + public default void zeroHeight() {} + +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOFalcon.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOFalcon.java index 61bd230..d182b22 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOFalcon.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOFalcon.java @@ -1,121 +1,121 @@ -package frc.robot.subsystems.elevator; - -import org.littletonrobotics.junction.Logger; - -import com.revrobotics.CANSparkMax; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.lib.util.RebelUtil; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -public class ElevatorIOFalcon extends SubsystemBase implements ElevatorIO { - private static final double kMotorToOutputShaftRatio = 1/7.5; - private static final double kSproketDiameterMeters = 0.032; - private static final double kFIRST_STAGE_TO_SECOND = 2.054054054054054; - private static final double kSECOND_STAGE_TO_THIRD = 1; //They got rid of the third stage - - private TalonFX m_motor1 = new TalonFX(17); - private TalonFX m_motor2 = new TalonFX(18); - // private static final double kMAX_CURRENT_AMPS = 35; //Let the smart current handler in the motorControllers handle it. - private static final double kMAX_VOLTAGE = 12; - private static final double kELEVATOR_ANGLE_SIN = Math.sin(Math.toRadians(23)); - - // Should be where the second stage starts when elavator down. measured from the base of the elevator to the second stage top in meters. do not measure from belly pan. measure from base of elavator - - // Should be where the second stage starts when elavator down. measured from the base of the elevator to the second stage top in meters. do not measure from belly pan. measure from base of elavator - - private static final double kMIN_SHOOTER_HEIGHT = 0; // 0.6m offset, stop the cap - private static final double kMAX_SHOOTER_HEIGHT= 0.52; - - private static final double kMIN_CLIMBER_HEIGHT = 0; - private static final double kMAX_CLIMBER_HEIGHT= 0.7; - - private double kCLIMB_KG = 0; - private PIDController positionFeedBackController = new PIDController(0, 0, 0); - private ElevatorFeedforward positionFeedForwardController = new ElevatorFeedforward(0, 0, 0); - private ElevatorFeedforward climbElevatorFeedforwardController = new ElevatorFeedforward(0,0,0); - - private double lastShooterHeightMeters; - private double lastClimberHeightMeters; - private double goalPositionMeters; - - - public ElevatorIOFalcon() { - - m_motor1.clearStickyFault_BootDuringEnable(); - m_motor2.clearStickyFault_BootDuringEnable(); - m_motor1.setNeutralMode(NeutralModeValue.Brake); - m_motor2.setNeutralMode(NeutralModeValue.Brake); - - zeroHeight(); - - } - - @Override - public void updateInputs(ElevatorIOInputs inputs) { - inputs.shooterHeightMeters = m_motor1.getPosition().getValueAsDouble() * kMotorToOutputShaftRatio * Math.PI * kSproketDiameterMeters * kFIRST_STAGE_TO_SECOND; - - inputs.climberHeightMeters = m_motor1.getPosition().getValueAsDouble() * kMotorToOutputShaftRatio * Math.PI * - kSproketDiameterMeters * kFIRST_STAGE_TO_SECOND * kSECOND_STAGE_TO_THIRD * kELEVATOR_ANGLE_SIN; - - inputs.voltageOut = m_motor1.getDutyCycle().getValueAsDouble() * kMAX_VOLTAGE; - - inputs.reachedSetpoint = positionFeedBackController.atSetpoint(); - - inputs.goalPositionMeters = this.goalPositionMeters; - - lastClimberHeightMeters = inputs.climberHeightMeters; - lastShooterHeightMeters = inputs.shooterHeightMeters; - } - - @Override - public void setHeightMeters(double goalPositionMeters) { - positionFeedBackController.setSetpoint(goalPositionMeters); - - this.goalPositionMeters = goalPositionMeters; - double currentPositionMeters = lastShooterHeightMeters; - - double feedBackControllerVoltage = positionFeedBackController.calculate(currentPositionMeters); - double accel = feedBackControllerVoltage == 0 ? 0 : feedBackControllerVoltage < 0 ? -1: 1; //Changes direction of accel given the feedbackcontroller voltage. - double feedForwardVoltage = positionFeedForwardController.calculate(goalPositionMeters, accel); - - double voltageOut = feedForwardVoltage + feedBackControllerVoltage; - voltageOut = RebelUtil.constrain(voltageOut, -12, 12); - - if ((currentPositionMeters > kMAX_SHOOTER_HEIGHT && voltageOut > 0) || - (currentPositionMeters < kMIN_SHOOTER_HEIGHT && voltageOut < 0) || - (goalPositionMeters > kMAX_SHOOTER_HEIGHT || goalPositionMeters < kMIN_SHOOTER_HEIGHT)) { - voltageOut = 0; - } - - Logger.recordOutput("Elevator/voltageOut", voltageOut); - m_motor1.setVoltage(voltageOut); - m_motor2.setVoltage(voltageOut); - - } - - public void setVoltage(double voltage){ - Logger.recordOutput("Elevator/voltageOut", voltage); - m_motor1.setVoltage(voltage); - m_motor2.setVoltage(voltage); - } - - - @Override - public void configureController(ElevatorFeedforward pff, PIDController pfb, ElevatorFeedforward Climbff,double kCLIMB_KG) { - this.kCLIMB_KG = kCLIMB_KG; - positionFeedBackController = pfb; - positionFeedForwardController = pff; - climbElevatorFeedforwardController = Climbff; - } - - @Override - public void zeroHeight() { - m_motor1.setPosition(0.0); - m_motor2.setPosition(0.0); - } - +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.lib.util.RebelUtil; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class ElevatorIOFalcon extends SubsystemBase implements ElevatorIO { + private static final double kMotorToOutputShaftRatio = 1/7.5; + private static final double kSproketDiameterMeters = 0.032; + private static final double kFIRST_STAGE_TO_SECOND = 2.054054054054054; + private static final double kSECOND_STAGE_TO_THIRD = 1; //They got rid of the third stage + + private TalonFX m_motor1 = new TalonFX(17); + private TalonFX m_motor2 = new TalonFX(18); + // private static final double kMAX_CURRENT_AMPS = 35; //Let the smart current handler in the motorControllers handle it. + private static final double kMAX_VOLTAGE = 12; + private static final double kELEVATOR_ANGLE_SIN = Math.sin(Math.toRadians(23)); + + // Should be where the second stage starts when elavator down. measured from the base of the elevator to the second stage top in meters. do not measure from belly pan. measure from base of elavator + + // Should be where the second stage starts when elavator down. measured from the base of the elevator to the second stage top in meters. do not measure from belly pan. measure from base of elavator + + private static final double kMIN_SHOOTER_HEIGHT = 0; // 0.6m offset, stop the cap + private static final double kMAX_SHOOTER_HEIGHT= 0.52; + + private static final double kMIN_CLIMBER_HEIGHT = 0; + private static final double kMAX_CLIMBER_HEIGHT= 0.7; + + private double kCLIMB_KG = 0; + private PIDController positionFeedBackController = new PIDController(0, 0, 0); + private ElevatorFeedforward positionFeedForwardController = new ElevatorFeedforward(0, 0, 0); + private ElevatorFeedforward climbElevatorFeedforwardController = new ElevatorFeedforward(0,0,0); + + private double lastShooterHeightMeters; + private double lastClimberHeightMeters; + private double goalPositionMeters; + + + public ElevatorIOFalcon() { + + m_motor1.clearStickyFault_BootDuringEnable(); + m_motor2.clearStickyFault_BootDuringEnable(); + m_motor1.setNeutralMode(NeutralModeValue.Brake); + m_motor2.setNeutralMode(NeutralModeValue.Brake); + + zeroHeight(); + + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.shooterHeightMeters = m_motor1.getPosition().getValueAsDouble() * kMotorToOutputShaftRatio * Math.PI * kSproketDiameterMeters * kFIRST_STAGE_TO_SECOND; + + inputs.climberHeightMeters = m_motor1.getPosition().getValueAsDouble() * kMotorToOutputShaftRatio * Math.PI * + kSproketDiameterMeters * kFIRST_STAGE_TO_SECOND * kSECOND_STAGE_TO_THIRD * kELEVATOR_ANGLE_SIN; + + inputs.voltageOut = m_motor1.getDutyCycle().getValueAsDouble() * kMAX_VOLTAGE; + + inputs.reachedSetpoint = positionFeedBackController.atSetpoint(); + + inputs.goalPositionMeters = this.goalPositionMeters; + + lastClimberHeightMeters = inputs.climberHeightMeters; + lastShooterHeightMeters = inputs.shooterHeightMeters; + } + + @Override + public void setHeightMeters(double goalPositionMeters) { + positionFeedBackController.setSetpoint(goalPositionMeters); + + this.goalPositionMeters = goalPositionMeters; + double currentPositionMeters = lastShooterHeightMeters; + + double feedBackControllerVoltage = positionFeedBackController.calculate(currentPositionMeters); + double accel = feedBackControllerVoltage == 0 ? 0 : feedBackControllerVoltage < 0 ? -1: 1; //Changes direction of accel given the feedbackcontroller voltage. + double feedForwardVoltage = positionFeedForwardController.calculate(goalPositionMeters, accel); + + double voltageOut = feedForwardVoltage + feedBackControllerVoltage; + voltageOut = RebelUtil.constrain(voltageOut, -12, 12); + + if ((currentPositionMeters > kMAX_SHOOTER_HEIGHT && voltageOut > 0) || + (currentPositionMeters < kMIN_SHOOTER_HEIGHT && voltageOut < 0) || + (goalPositionMeters > kMAX_SHOOTER_HEIGHT || goalPositionMeters < kMIN_SHOOTER_HEIGHT)) { + voltageOut = 0; + } + + Logger.recordOutput("Elevator/voltageOut", voltageOut); + m_motor1.setVoltage(voltageOut); + m_motor2.setVoltage(voltageOut); + + } + + public void setVoltage(double voltage){ + Logger.recordOutput("Elevator/voltageOut", voltage); + m_motor1.setVoltage(voltage); + m_motor2.setVoltage(voltage); + } + + + @Override + public void configureController(ElevatorFeedforward pff, PIDController pfb, ElevatorFeedforward Climbff,double kCLIMB_KG) { + this.kCLIMB_KG = kCLIMB_KG; + positionFeedBackController = pfb; + positionFeedForwardController = pff; + climbElevatorFeedforwardController = Climbff; + } + + @Override + public void zeroHeight() { + m_motor1.setPosition(0.0); + m_motor2.setPosition(0.0); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index c142d89..4e85a38 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,38 +1,38 @@ -package frc.robot.subsystems.elevator; - -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ElevatorIOSim extends SubsystemBase implements ElevatorIO { - - private double shooterHeightMeters; - private double climberHeightMeters; - private static final double kSECOND_STAGE_TO_THIRD = 2; - - public void updateInputs(ElevatorIOInputs inputs) { - inputs.shooterHeightMeters = shooterHeightMeters; - inputs.climberHeightMeters = climberHeightMeters; - } - - public void setHeightMeters(double goalPositionMeters, boolean isShooterHight, boolean isClimbing) { - if (isShooterHight) { - shooterHeightMeters = goalPositionMeters; - climberHeightMeters = shooterHeightMeters * kSECOND_STAGE_TO_THIRD; - } - else { - climberHeightMeters = goalPositionMeters; - shooterHeightMeters = climberHeightMeters / kSECOND_STAGE_TO_THIRD; - } - } - - public void configureController(ElevatorFeedforward pff, PIDController pfb, ElevatorFeedforward cff, double kCLIMB_KG) {} - - public void setVoltage(double voltage) {} - - public boolean reachedSetpoint() { - return true; - } - - public void zeroHeight() {} +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ElevatorIOSim extends SubsystemBase implements ElevatorIO { + + private double shooterHeightMeters; + private double climberHeightMeters; + private static final double kSECOND_STAGE_TO_THIRD = 2; + + public void updateInputs(ElevatorIOInputs inputs) { + inputs.shooterHeightMeters = shooterHeightMeters; + inputs.climberHeightMeters = climberHeightMeters; + } + + public void setHeightMeters(double goalPositionMeters, boolean isShooterHight, boolean isClimbing) { + if (isShooterHight) { + shooterHeightMeters = goalPositionMeters; + climberHeightMeters = shooterHeightMeters * kSECOND_STAGE_TO_THIRD; + } + else { + climberHeightMeters = goalPositionMeters; + shooterHeightMeters = climberHeightMeters / kSECOND_STAGE_TO_THIRD; + } + } + + public void configureController(ElevatorFeedforward pff, PIDController pfb, ElevatorFeedforward cff, double kCLIMB_KG) {} + + public void setVoltage(double voltage) {} + + public boolean reachedSetpoint() { + return true; + } + + public void zeroHeight() {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index f72c986..f2c7cd5 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,49 +1,49 @@ -package frc.robot.subsystems.indexer; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import frc.robot.subsystems.drivetrain.vision.NoteDetector; -import frc.robot.subsystems.intakeComp.Intake; -import frc.robot.subsystems.pivotComp.Pivot; - -public class Indexer extends SubsystemBase { - private IndexerIO io; - private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); - private final SwerveDrive swerveDrive; - private Intake intake; - private final NoteDetector noteDetector; - public Indexer(SwerveDrive swerveDrive, NoteDetector noteDetector) { - this.swerveDrive = swerveDrive; - this.noteDetector = noteDetector; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Indexer", inputs); - } - - public boolean inIntake() { - return inputs.inIntake; - } - - public boolean inShooter() { - return inputs.inShooter; - } - - public void setSubsystem(Intake intake, Pivot pivot) { - this.intake = intake; - switch (Constants.currentMode) { - case SIM: - io = new IndexerIOSim(swerveDrive, intake, noteDetector, pivot); - break; - - default: - io = new IndexerIOReal(); - break; - } - } -} +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import frc.robot.subsystems.drivetrain.vision.NoteDetector; +import frc.robot.subsystems.intakeComp.Intake; +import frc.robot.subsystems.pivotComp.Pivot; + +public class Indexer extends SubsystemBase { + private IndexerIO io; + private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); + private final SwerveDrive swerveDrive; + private Intake intake; + private final NoteDetector noteDetector; + public Indexer(SwerveDrive swerveDrive, NoteDetector noteDetector) { + this.swerveDrive = swerveDrive; + this.noteDetector = noteDetector; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Indexer", inputs); + } + + public boolean inIntake() { + return inputs.inIntake; + } + + public boolean inShooter() { + return inputs.inShooter; + } + + public void setSubsystem(Intake intake, Pivot pivot) { + this.intake = intake; + switch (Constants.currentMode) { + case SIM: + io = new IndexerIOSim(swerveDrive, intake, noteDetector, pivot); + break; + + default: + io = new IndexerIOReal(); + break; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java index 57a2b93..15b56a5 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -1,13 +1,13 @@ -package frc.robot.subsystems.indexer; - -import org.littletonrobotics.junction.AutoLog; - -public interface IndexerIO { - @AutoLog - public static class IndexerIOInputs { - public boolean inIntake = false; - public boolean inShooter = false; - } - - public default void updateInputs(IndexerIOInputs inputs) {}; -} +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + @AutoLog + public static class IndexerIOInputs { + public boolean inIntake = false; + public boolean inShooter = false; + } + + public default void updateInputs(IndexerIOInputs inputs) {}; +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java index 8b4cc0a..594647b 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java @@ -1,18 +1,18 @@ -package frc.robot.subsystems.indexer; - -import edu.wpi.first.wpilibj.DigitalInput; - -public class IndexerIOReal implements IndexerIO { - - private final DigitalInput intakeBrake = new DigitalInput(7); - private final DigitalInput shooterBrake = new DigitalInput(9); - public IndexerIOReal() { - - } - - @Override - public void updateInputs(IndexerIOInputs inputs) { - inputs.inIntake = !intakeBrake.get(); - inputs.inShooter = !shooterBrake.get(); - } -} +package frc.robot.subsystems.indexer; + +import edu.wpi.first.wpilibj.DigitalInput; + +public class IndexerIOReal implements IndexerIO { + + private final DigitalInput intakeBrake = new DigitalInput(7); + private final DigitalInput shooterBrake = new DigitalInput(9); + public IndexerIOReal() { + + } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.inIntake = !intakeBrake.get(); + inputs.inShooter = !shooterBrake.get(); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java index ad2c3ee..6638109 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java @@ -1,68 +1,68 @@ -package frc.robot.subsystems.indexer; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import frc.robot.Constants; -import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; -import frc.robot.subsystems.drivetrain.vision.NoteDetector; -import frc.robot.subsystems.intakeComp.Intake; -import frc.robot.subsystems.pivotComp.Pivot; - -public class IndexerIOSim implements IndexerIO { - - private final SwerveDrive swerveDrive; - private final Intake intake; - private final NoteDetector noteDetector; - private final Pivot pivot; - private boolean contact = false; - private double initialIntakePose = 0; - - public IndexerIOSim(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector, Pivot pivot) { - this.pivot = pivot; - this.swerveDrive = swerveDrive; - this.intake = intake; - this.noteDetector = noteDetector; - } - - @Override - public void updateInputs(IndexerIOInputs inputs) { - Rotation2d robotYaw = swerveDrive.getPose().getRotation(); - Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); - intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); - intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); - - Translation3d intakeTranslation3d = new Translation3d( - intakeTranslation2d.getX(), - intakeTranslation2d.getY(), - Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); - - - Logger.recordOutput("Indexer/intakePose", new Pose3d(intakeTranslation3d, new Rotation3d())); - - double dist = Double.MAX_VALUE; - dist = noteDetector.getNoteFieldRelativePose().getDistance(intakeTranslation3d.toTranslation2d()); - - if (dist <= .15 && !contact && intake.getVelocityMps() >= .2 && pivot.getDegAngle() >= 60) { - contact = true; - initialIntakePose = intake.getPoseMeters(); - } - // if (contact && intake.getPoseMeters() < initialIntakePose) { - // contact = false; - // inputs.inIntake = false; - // } - if (contact && intake.getPoseMeters() - initialIntakePose >= Constants.FieldConstants.kNOTE_DIAMETER_METERS) { - inputs.inIntake = true; - } - - Logger.recordOutput("Indexer/contact", contact); - Logger.recordOutput("Indexer/dist", dist); - Logger.recordOutput("Indexer/initialIntakePose", initialIntakePose); - - - } -} +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.Constants; +import frc.robot.subsystems.drivetrain.swerve.SwerveDrive; +import frc.robot.subsystems.drivetrain.vision.NoteDetector; +import frc.robot.subsystems.intakeComp.Intake; +import frc.robot.subsystems.pivotComp.Pivot; + +public class IndexerIOSim implements IndexerIO { + + private final SwerveDrive swerveDrive; + private final Intake intake; + private final NoteDetector noteDetector; + private final Pivot pivot; + private boolean contact = false; + private double initialIntakePose = 0; + + public IndexerIOSim(SwerveDrive swerveDrive, Intake intake, NoteDetector noteDetector, Pivot pivot) { + this.pivot = pivot; + this.swerveDrive = swerveDrive; + this.intake = intake; + this.noteDetector = noteDetector; + } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + Rotation2d robotYaw = swerveDrive.getPose().getRotation(); + Translation2d intakeTranslation2d = Constants.IntakeConstants.KINTAKE_TRANSLATION3D.toTranslation2d(); + intakeTranslation2d = intakeTranslation2d.rotateBy(robotYaw); + intakeTranslation2d = intakeTranslation2d.plus(swerveDrive.getPose().getTranslation()); + + Translation3d intakeTranslation3d = new Translation3d( + intakeTranslation2d.getX(), + intakeTranslation2d.getY(), + Constants.IntakeConstants.KINTAKE_TRANSLATION3D.getZ()); + + + Logger.recordOutput("Indexer/intakePose", new Pose3d(intakeTranslation3d, new Rotation3d())); + + double dist = Double.MAX_VALUE; + dist = noteDetector.getNoteFieldRelativePose().getDistance(intakeTranslation3d.toTranslation2d()); + + if (dist <= .15 && !contact && intake.getVelocityMps() >= .2 && pivot.getDegAngle() >= 60) { + contact = true; + initialIntakePose = intake.getPoseMeters(); + } + // if (contact && intake.getPoseMeters() < initialIntakePose) { + // contact = false; + // inputs.inIntake = false; + // } + if (contact && intake.getPoseMeters() - initialIntakePose >= Constants.FieldConstants.kNOTE_DIAMETER_METERS) { + inputs.inIntake = true; + } + + Logger.recordOutput("Indexer/contact", contact); + Logger.recordOutput("Indexer/dist", dist); + Logger.recordOutput("Indexer/initialIntakePose", initialIntakePose); + + + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 04b2af7..0d95540 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,86 +1,86 @@ -package frc.robot.subsystems.intake; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.lib.util.RebelUtil; -import frc.robot.subsystems.indexer.Indexer; - -public class Intake extends SubsystemBase { - - private final IntakeIO io; - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - - private final PIDController m_velocityFeedbackController; - private final SimpleMotorFeedforward m_velocityFeedforwardController; - - private double desiredSpeedMps = 0; - - public Intake(Indexer indexer) { - switch (Constants.currentMode) { - case SIM: - io = new IntakeIOSim(indexer); - m_velocityFeedbackController = new PIDController(0.3, 0, 0); - m_velocityFeedforwardController = new SimpleMotorFeedforward(0, 0.006944444444); - break; - - default: - io = new IntakeIO() { - - }; - m_velocityFeedbackController = new PIDController(0, 0, 0); - m_velocityFeedforwardController = new SimpleMotorFeedforward(0, 0); - break; - } - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Intake", inputs); - - double voltage = m_velocityFeedbackController.calculate(inputs.intakeVelocityMps, desiredSpeedMps) + - m_velocityFeedforwardController.calculate(desiredSpeedMps); - RebelUtil.constrain(voltage, -12, 12); - - if (inputs.amps> 40 || - inputs.volts > 12 || - inputs.temp >= 94) { - voltage = 0; - - System.err.println("SHOOTER MOTOR NOT WITHIN OPERATION RANGE"); - Logger.recordOutput("Intake/withinOperationRange", false); - } - else { - Logger.recordOutput("Intake/withinOperationRange", true); - } - - io.setVoltage(voltage); - Logger.recordOutput("Intake/calculatedVoltage", voltage); - - } - - public void setSpeedMps(double mps) { - desiredSpeedMps = mps; - Logger.recordOutput("Intake/desiredSpeedMps", desiredSpeedMps); - } - - public boolean reachedSetpoint() { - return Math.abs(inputs.intakeVelocityMps - desiredSpeedMps) <= 0.02; - } - - public boolean inIntake() { - return inputs.inIntake; - } - - public double getPoseMeters() { - return inputs.intakePoseMeters; - } - - public double getVelocityMps() { - return inputs.intakeVelocityMps; - } -} +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.lib.util.RebelUtil; +import frc.robot.subsystems.indexer.Indexer; + +public class Intake extends SubsystemBase { + + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + private final PIDController m_velocityFeedbackController; + private final SimpleMotorFeedforward m_velocityFeedforwardController; + + private double desiredSpeedMps = 0; + + public Intake(Indexer indexer) { + switch (Constants.currentMode) { + case SIM: + io = new IntakeIOSim(indexer); + m_velocityFeedbackController = new PIDController(0.3, 0, 0); + m_velocityFeedforwardController = new SimpleMotorFeedforward(0, 0.006944444444); + break; + + default: + io = new IntakeIO() { + + }; + m_velocityFeedbackController = new PIDController(0, 0, 0); + m_velocityFeedforwardController = new SimpleMotorFeedforward(0, 0); + break; + } + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + double voltage = m_velocityFeedbackController.calculate(inputs.intakeVelocityMps, desiredSpeedMps) + + m_velocityFeedforwardController.calculate(desiredSpeedMps); + RebelUtil.constrain(voltage, -12, 12); + + if (inputs.amps> 40 || + inputs.volts > 12 || + inputs.temp >= 94) { + voltage = 0; + + System.err.println("SHOOTER MOTOR NOT WITHIN OPERATION RANGE"); + Logger.recordOutput("Intake/withinOperationRange", false); + } + else { + Logger.recordOutput("Intake/withinOperationRange", true); + } + + io.setVoltage(voltage); + Logger.recordOutput("Intake/calculatedVoltage", voltage); + + } + + public void setSpeedMps(double mps) { + desiredSpeedMps = mps; + Logger.recordOutput("Intake/desiredSpeedMps", desiredSpeedMps); + } + + public boolean reachedSetpoint() { + return Math.abs(inputs.intakeVelocityMps - desiredSpeedMps) <= 0.02; + } + + public boolean inIntake() { + return inputs.inIntake; + } + + public double getPoseMeters() { + return inputs.intakePoseMeters; + } + + public double getVelocityMps() { + return inputs.intakeVelocityMps; + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 3090816..01f1636 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -1,19 +1,19 @@ -package frc.robot.subsystems.intake; - -import org.littletonrobotics.junction.AutoLog; - -public interface IntakeIO { - @AutoLog - public static class IntakeIOInputs { - public double intakeVelocityMps = 0; - public double intakePoseMeters = 0; - public double temp = 0; - public double volts = 0; - public double amps = 0; - public boolean inIntake = false; - } - - public default void updateInputs(IntakeIOInputs inputs) {}; - public default void setVoltage(double voltage) {} - -} +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double intakeVelocityMps = 0; + public double intakePoseMeters = 0; + public double temp = 0; + public double volts = 0; + public double amps = 0; + public boolean inIntake = false; + } + + public default void updateInputs(IntakeIOInputs inputs) {}; + public default void setVoltage(double voltage) {} + +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index e9dfafe..fe5f9bf 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,46 +1,46 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import frc.robot.Constants; -import frc.robot.subsystems.indexer.Indexer; - -public class IntakeIOSim implements IntakeIO { - private static final double kMOTOR_TO_OUTPUT_RATIO = 2; - - private DCMotor m_gearbox = DCMotor.getNeo550(1); - private FlywheelSim m_motor = new FlywheelSim(m_gearbox, kMOTOR_TO_OUTPUT_RATIO, 0.007); - - private double voltage = 0; - - private final Indexer indexer; - public IntakeIOSim(Indexer indexer) { - this.indexer = indexer; - } - - @Override - public void updateInputs(IntakeIOInputs inputs) { - m_motor.update(0.020); - - inputs.intakeVelocityMps = m_motor.getAngularVelocityRPM() * - kMOTOR_TO_OUTPUT_RATIO * - Math.PI * 2 * - Constants.IntakeConstants.kROLLER_RADIUS_METERS; - - inputs.intakePoseMeters += inputs.intakeVelocityMps * 0.020; - - inputs.amps = m_motor.getCurrentDrawAmps(); - - inputs.temp = 0; - - inputs.volts = voltage; - - inputs.inIntake = indexer.inIntake(); - } - - @Override - public void setVoltage(double voltage) { - this.voltage = voltage; - m_motor.setInputVoltage(voltage); - } -} +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants; +import frc.robot.subsystems.indexer.Indexer; + +public class IntakeIOSim implements IntakeIO { + private static final double kMOTOR_TO_OUTPUT_RATIO = 2; + + private DCMotor m_gearbox = DCMotor.getNeo550(1); + private FlywheelSim m_motor = new FlywheelSim(m_gearbox, kMOTOR_TO_OUTPUT_RATIO, 0.007); + + private double voltage = 0; + + private final Indexer indexer; + public IntakeIOSim(Indexer indexer) { + this.indexer = indexer; + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + m_motor.update(0.020); + + inputs.intakeVelocityMps = m_motor.getAngularVelocityRPM() * + kMOTOR_TO_OUTPUT_RATIO * + Math.PI * 2 * + Constants.IntakeConstants.kROLLER_RADIUS_METERS; + + inputs.intakePoseMeters += inputs.intakeVelocityMps * 0.020; + + inputs.amps = m_motor.getCurrentDrawAmps(); + + inputs.temp = 0; + + inputs.volts = voltage; + + inputs.inIntake = indexer.inIntake(); + } + + @Override + public void setVoltage(double voltage) { + this.voltage = voltage; + m_motor.setInputVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/intakeComp/Intake.java b/src/main/java/frc/robot/subsystems/intakeComp/Intake.java index 8dd38df..cea48fc 100644 --- a/src/main/java/frc/robot/subsystems/intakeComp/Intake.java +++ b/src/main/java/frc/robot/subsystems/intakeComp/Intake.java @@ -1,88 +1,88 @@ -package frc.robot.subsystems.intakeComp; - -import org.littletonrobotics.junction.Logger; - - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Intake extends SubsystemBase{ - - private static final double kVelocityRadSecTolerance = Math.toRadians(.1); - - private static IntakeIO io; - private static Intake instance; - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - - PIDController velocityFeedBackController; - SimpleMotorFeedforward velocityFeedForwardController; - double desiredVelocityRadSec = 0; - - - - public Intake(IntakeIO io) { - Intake.io = io; - velocityFeedBackController = new PIDController(0.1,0,0.0); // 1 0 0 - velocityFeedBackController.setTolerance(kVelocityRadSecTolerance); - velocityFeedForwardController = new SimpleMotorFeedforward(0.3, .5, 0); - io.configureController(velocityFeedForwardController, velocityFeedBackController); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Intake", inputs); - - Logger.recordOutput("Intake/desiredVelocityRadSec", desiredVelocityRadSec); - - io.setVelocityRadSec(desiredVelocityRadSec); - } - - public void setVelocityRadSec(double velo) { - desiredVelocityRadSec = velo; - return; - } - - public void setVoltage(double voltage){ - io.setVoltage(voltage); - return; - } - - public double getVelocityDegSec() { - return Math.toDegrees(inputs.velocityRadSec); - } - - public double getVelocityRadSec() { - return inputs.velocityRadSec; - } - - public boolean reachedSetpoint() { - return inputs.reachedSetpoint; - } - - public boolean inIntake() { - return inputs.inIntake; - } - public void setIntakeStatus(boolean s){ - - } - public static Intake getInstance(){ - if(Intake.instance == null){ - return new Intake(Intake.io); - } - return instance; - } - public static Intake setInstance(Intake inst){ - Intake.instance = inst; - return inst; - } - - public double getVelocityMps() { - return inputs.velocityMps; - } - - public double getPoseMeters() { - return inputs.distanceMeters; - } -} +package frc.robot.subsystems.intakeComp; + +import org.littletonrobotics.junction.Logger; + + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase{ + + private static final double kVelocityRadSecTolerance = Math.toRadians(.1); + + private static IntakeIO io; + private static Intake instance; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + PIDController velocityFeedBackController; + SimpleMotorFeedforward velocityFeedForwardController; + double desiredVelocityRadSec = 0; + + + + public Intake(IntakeIO io) { + Intake.io = io; + velocityFeedBackController = new PIDController(0.1,0,0.0); // 1 0 0 + velocityFeedBackController.setTolerance(kVelocityRadSecTolerance); + velocityFeedForwardController = new SimpleMotorFeedforward(0.3, .5, 0); + io.configureController(velocityFeedForwardController, velocityFeedBackController); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + Logger.recordOutput("Intake/desiredVelocityRadSec", desiredVelocityRadSec); + + io.setVelocityRadSec(desiredVelocityRadSec); + } + + public void setVelocityRadSec(double velo) { + desiredVelocityRadSec = velo; + return; + } + + public void setVoltage(double voltage){ + io.setVoltage(voltage); + return; + } + + public double getVelocityDegSec() { + return Math.toDegrees(inputs.velocityRadSec); + } + + public double getVelocityRadSec() { + return inputs.velocityRadSec; + } + + public boolean reachedSetpoint() { + return inputs.reachedSetpoint; + } + + public boolean inIntake() { + return inputs.inIntake; + } + public void setIntakeStatus(boolean s){ + + } + public static Intake getInstance(){ + if(Intake.instance == null){ + return new Intake(Intake.io); + } + return instance; + } + public static Intake setInstance(Intake inst){ + Intake.instance = inst; + return inst; + } + + public double getVelocityMps() { + return inputs.velocityMps; + } + + public double getPoseMeters() { + return inputs.distanceMeters; + } +} diff --git a/src/main/java/frc/robot/subsystems/intakeComp/IntakeIO.java b/src/main/java/frc/robot/subsystems/intakeComp/IntakeIO.java index 57e4a6f..cd0595e 100644 --- a/src/main/java/frc/robot/subsystems/intakeComp/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intakeComp/IntakeIO.java @@ -1,26 +1,26 @@ -package frc.robot.subsystems.intakeComp; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; - -public interface IntakeIO { - @AutoLog - public static class IntakeIOInputs { - public double velocityRadSec = 0; - public double velocityMps = 0; - public double distanceMeters = 0; - public boolean inIntake = false; - public boolean reachedSetpoint = false; - } - - public default void updateInputs(IntakeIOInputs inputs) {} - - public default void setVelocityRadSec(double goalVelocityRadPerSec) {} - - public default void configureController(SimpleMotorFeedforward vff, PIDController vfb) {} - - public default void setVoltage(double voltage) {} - -} +package frc.robot.subsystems.intakeComp; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double velocityRadSec = 0; + public double velocityMps = 0; + public double distanceMeters = 0; + public boolean inIntake = false; + public boolean reachedSetpoint = false; + } + + public default void updateInputs(IntakeIOInputs inputs) {} + + public default void setVelocityRadSec(double goalVelocityRadPerSec) {} + + public default void configureController(SimpleMotorFeedforward vff, PIDController vfb) {} + + public default void setVoltage(double voltage) {} + +} diff --git a/src/main/java/frc/robot/subsystems/intakeComp/IntakeIONeo.java b/src/main/java/frc/robot/subsystems/intakeComp/IntakeIONeo.java index 4bdc47a..84cfb30 100644 --- a/src/main/java/frc/robot/subsystems/intakeComp/IntakeIONeo.java +++ b/src/main/java/frc/robot/subsystems/intakeComp/IntakeIONeo.java @@ -1,137 +1,137 @@ -package frc.robot.subsystems.intakeComp; - -import org.littletonrobotics.junction.Logger; - -import com.revrobotics.CANSparkMax; -// import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.subsystems.indexer.Indexer; - -public class IntakeIONeo extends SubsystemBase implements IntakeIO { - private final double kMotorToOutputShaftRatio = 0.25; - private CANSparkMax m_motor = new CANSparkMax(16, CANSparkMax.MotorType.kBrushless); - - private PIDController velocityFeedBackController = new PIDController(0, 0, 0); - private SimpleMotorFeedforward velocityFeedForwardController = new SimpleMotorFeedforward(0, 0, 0); - - // private Rev2mDistanceSensor distanceSensor; - private double currentVelocityRadPerSec; - - - private GenericEntry IntakeStatus; - - private final Indexer indexer; - - private static final double kMAX_VOLTAGE = 12; - - public IntakeIONeo(Indexer indexer) { - this.indexer = indexer; - m_motor.setIdleMode(CANSparkMax.IdleMode.kCoast); - m_motor.clearFaults(); //TODO: ALWAYS CHECK FOR FAULTS IN COMPETITION DO NOT IGNORE THEM - m_motor.setInverted(false); - // distanceSensor = new Rev2mDistanceSensor(Rev2mDistanceSensor.Port.kMXP, Rev2mDistanceSensor.Unit.kMillimeters, Rev2mDistanceSensor.RangeProfile.kDefault); - // distanceTolerance = 0.57; //Approximate distance assuming some tolerance, CHECK AGAIN - // distanceSensor.setEnabled(true); - - //distanceSensor.setAutomaticMode(true); << Probably not required but keep note that we need this if we have several of these 2m dist devices - - IntakeStatus = Shuffleboard.getTab("auto").add("INTAKE STATUS", inIntake()).getEntry(); - } - - @Override - public void updateInputs(IntakeIOInputs inputs) { - inputs.velocityRadSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio; // we divide by 60 because the motor out is in RPM - inputs.velocityMps = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * Math.PI * 2 * Constants.IntakeConstants.kROLLER_RADIUS_METERS; // we divide by 60 because the motor out is in RPM - inputs.distanceMeters += inputs.velocityMps * 0.020; - - inputs.reachedSetpoint = velocityFeedBackController.atSetpoint(); - inputs.inIntake = inIntake(); - currentVelocityRadPerSec = inputs.velocityRadSec; - } - - @Override - // should be called periodically - public void setVelocityRadSec(double goalVelocityRadPerSec) { - if(!inIntake() && goalVelocityRadPerSec == 0){ - goalVelocityRadPerSec = 2; - } - - double accel = 0; - - if (goalVelocityRadPerSec > currentVelocityRadPerSec) { - accel = 1; - } - else if (goalVelocityRadPerSec < currentVelocityRadPerSec) { - accel = -1; - } - - - - double feedForwardVoltage = velocityFeedForwardController.calculate(goalVelocityRadPerSec, accel); - - velocityFeedBackController.setSetpoint(goalVelocityRadPerSec); - double feedBackControllerVoltage = velocityFeedBackController.calculate(currentVelocityRadPerSec); - - double outVoltage = feedForwardVoltage + feedBackControllerVoltage; - - - if (outVoltage > kMAX_VOLTAGE) { - outVoltage = 12; - } - else if (outVoltage < -kMAX_VOLTAGE) { - outVoltage = -12; - } - Logger.recordOutput("Intake/voltageOut", outVoltage); - - // if (goalVelocityRadPerSec == 0) { - // m_motor.setVoltage(0); - - // } - // else { - // m_motor.setVoltage(4); - // } - - m_motor.setVoltage(outVoltage); - IntakeStatus.setBoolean(inIntake()); - } - - @Override - public void setVoltage(double voltage){ - m_motor.setVoltage(voltage); - } - - @Override - public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { - velocityFeedBackController = vfb; - velocityFeedForwardController = vff; - } - - public boolean inIntake() { - //Valid range(?) check aka 2m or less - - - /* //Uncomment this whenever you the 2mDistance sensor is there - if(distanceSensor.isRangeValid()){ - //distanceSensor.setMeasurementPeriod(); - //Using default measurementperiod, we get its range at that moment. - if(distanceSensor.getRange(Rev2mDistanceSensor.Unit.kMillimeters) < distanceTolerance){ - return true; - } - return false; - } - else{ - // System.out.println("Out of range"); - return false; - } - */ - - return indexer.inIntake(); - - } - +package frc.robot.subsystems.intakeComp; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.CANSparkMax; +// import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.indexer.Indexer; + +public class IntakeIONeo extends SubsystemBase implements IntakeIO { + private final double kMotorToOutputShaftRatio = 0.25; + private CANSparkMax m_motor = new CANSparkMax(16, CANSparkMax.MotorType.kBrushless); + + private PIDController velocityFeedBackController = new PIDController(0, 0, 0); + private SimpleMotorFeedforward velocityFeedForwardController = new SimpleMotorFeedforward(0, 0, 0); + + // private Rev2mDistanceSensor distanceSensor; + private double currentVelocityRadPerSec; + + + private GenericEntry IntakeStatus; + + private final Indexer indexer; + + private static final double kMAX_VOLTAGE = 12; + + public IntakeIONeo(Indexer indexer) { + this.indexer = indexer; + m_motor.setIdleMode(CANSparkMax.IdleMode.kCoast); + m_motor.clearFaults(); //TODO: ALWAYS CHECK FOR FAULTS IN COMPETITION DO NOT IGNORE THEM + m_motor.setInverted(false); + // distanceSensor = new Rev2mDistanceSensor(Rev2mDistanceSensor.Port.kMXP, Rev2mDistanceSensor.Unit.kMillimeters, Rev2mDistanceSensor.RangeProfile.kDefault); + // distanceTolerance = 0.57; //Approximate distance assuming some tolerance, CHECK AGAIN + // distanceSensor.setEnabled(true); + + //distanceSensor.setAutomaticMode(true); << Probably not required but keep note that we need this if we have several of these 2m dist devices + + IntakeStatus = Shuffleboard.getTab("auto").add("INTAKE STATUS", inIntake()).getEntry(); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.velocityRadSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio; // we divide by 60 because the motor out is in RPM + inputs.velocityMps = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * Math.PI * 2 * Constants.IntakeConstants.kROLLER_RADIUS_METERS; // we divide by 60 because the motor out is in RPM + inputs.distanceMeters += inputs.velocityMps * 0.020; + + inputs.reachedSetpoint = velocityFeedBackController.atSetpoint(); + inputs.inIntake = inIntake(); + currentVelocityRadPerSec = inputs.velocityRadSec; + } + + @Override + // should be called periodically + public void setVelocityRadSec(double goalVelocityRadPerSec) { + if(!inIntake() && goalVelocityRadPerSec == 0){ + goalVelocityRadPerSec = 2; + } + + double accel = 0; + + if (goalVelocityRadPerSec > currentVelocityRadPerSec) { + accel = 1; + } + else if (goalVelocityRadPerSec < currentVelocityRadPerSec) { + accel = -1; + } + + + + double feedForwardVoltage = velocityFeedForwardController.calculate(goalVelocityRadPerSec, accel); + + velocityFeedBackController.setSetpoint(goalVelocityRadPerSec); + double feedBackControllerVoltage = velocityFeedBackController.calculate(currentVelocityRadPerSec); + + double outVoltage = feedForwardVoltage + feedBackControllerVoltage; + + + if (outVoltage > kMAX_VOLTAGE) { + outVoltage = 12; + } + else if (outVoltage < -kMAX_VOLTAGE) { + outVoltage = -12; + } + Logger.recordOutput("Intake/voltageOut", outVoltage); + + // if (goalVelocityRadPerSec == 0) { + // m_motor.setVoltage(0); + + // } + // else { + // m_motor.setVoltage(4); + // } + + m_motor.setVoltage(outVoltage); + IntakeStatus.setBoolean(inIntake()); + } + + @Override + public void setVoltage(double voltage){ + m_motor.setVoltage(voltage); + } + + @Override + public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { + velocityFeedBackController = vfb; + velocityFeedForwardController = vff; + } + + public boolean inIntake() { + //Valid range(?) check aka 2m or less + + + /* //Uncomment this whenever you the 2mDistance sensor is there + if(distanceSensor.isRangeValid()){ + //distanceSensor.setMeasurementPeriod(); + //Using default measurementperiod, we get its range at that moment. + if(distanceSensor.getRange(Rev2mDistanceSensor.Unit.kMillimeters) < distanceTolerance){ + return true; + } + return false; + } + else{ + // System.out.println("Out of range"); + return false; + } + */ + + return indexer.inIntake(); + + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intakeComp/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intakeComp/IntakeIOSim.java index 8c10f02..e586c30 100644 --- a/src/main/java/frc/robot/subsystems/intakeComp/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intakeComp/IntakeIOSim.java @@ -1,40 +1,40 @@ -package frc.robot.subsystems.intakeComp; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.subsystems.indexer.Indexer; - -public class IntakeIOSim extends SubsystemBase implements IntakeIO { - - double desiredVelocityRadSec = 0; - - private final Indexer indexer; - public IntakeIOSim(Indexer i) { - indexer = i; - } - @Override - public void updateInputs(IntakeIOInputs inputs) { - inputs.velocityRadSec = desiredVelocityRadSec; - inputs.inIntake = indexer.inIntake(); - inputs.reachedSetpoint = true; - inputs.velocityMps = inputs.velocityRadSec * Math.PI * 2 * Constants.IntakeConstants.kROLLER_RADIUS_METERS; - inputs.distanceMeters += inputs.velocityMps * 0.020; - } - - @Override - // sould be called periodically - public void setVelocityRadSec(double goalVelocityRadPerSec) { - desiredVelocityRadSec = goalVelocityRadPerSec; - } - - @Override - public void setVoltage(double voltage){ - } - - @Override - public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { - } - +package frc.robot.subsystems.intakeComp; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.indexer.Indexer; + +public class IntakeIOSim extends SubsystemBase implements IntakeIO { + + double desiredVelocityRadSec = 0; + + private final Indexer indexer; + public IntakeIOSim(Indexer i) { + indexer = i; + } + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.velocityRadSec = desiredVelocityRadSec; + inputs.inIntake = indexer.inIntake(); + inputs.reachedSetpoint = true; + inputs.velocityMps = inputs.velocityRadSec * Math.PI * 2 * Constants.IntakeConstants.kROLLER_RADIUS_METERS; + inputs.distanceMeters += inputs.velocityMps * 0.020; + } + + @Override + // sould be called periodically + public void setVelocityRadSec(double goalVelocityRadPerSec) { + desiredVelocityRadSec = goalVelocityRadPerSec; + } + + @Override + public void setVoltage(double voltage){ + } + + @Override + public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivotComp/Pivot.java b/src/main/java/frc/robot/subsystems/pivotComp/Pivot.java index 8211916..7b37861 100644 --- a/src/main/java/frc/robot/subsystems/pivotComp/Pivot.java +++ b/src/main/java/frc/robot/subsystems/pivotComp/Pivot.java @@ -1,106 +1,106 @@ -package frc.robot.subsystems.pivotComp; - -import org.littletonrobotics.junction.Logger; - - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Pivot extends SubsystemBase{ - - private static final double kRadPositionTolerance = Math.toRadians(5); - - private static PivotIO io; - private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); - private boolean velocityControlmode; - - PIDController upPositionFeedBackController; - PIDController downPositionFeedBackController; - - ArmFeedforward downPositionFeedForwardController; - ArmFeedforward upPositionFeedForwardController; - - private static Pivot instance = null; - - PIDController velocityFeedBackController; - ArmFeedforward velocityFeedForwardController; - double desiredDegAngle = 0; - public Pivot(PivotIO io) { - Pivot.io = io; - upPositionFeedBackController = new PIDController(4, 0, 0.03); //0.04 - upPositionFeedBackController.setTolerance(kRadPositionTolerance); - upPositionFeedForwardController = new ArmFeedforward(0.0,0,5); // 0,0,76 - - downPositionFeedBackController = new PIDController(7, 0, 0); - downPositionFeedBackController.setTolerance(kRadPositionTolerance); - downPositionFeedForwardController = new ArmFeedforward(0,0, 3); // 8 - - io.configureController(downPositionFeedForwardController, downPositionFeedBackController); - } - - @Override - public void periodic() { - if (desiredDegAngle > 45) { - io.configureController(downPositionFeedForwardController, downPositionFeedBackController); - } - else if (desiredDegAngle < 45) { - io.configureController(upPositionFeedForwardController, upPositionFeedBackController); - } - io.updateInputs(inputs); - Logger.processInputs("Pivot", inputs); - - Logger.recordOutput("Pivot/desiredDegAngle", desiredDegAngle); - io.setPosition(Math.toRadians(desiredDegAngle)); - // io.setPosition(desiredDegAngle); - } - - public void setDegAngle(double angle) { - desiredDegAngle = angle; - } - - public void setVelocityControlMode(boolean b){ - velocityControlmode = b; - }; - - public void setVelocitySetPoint(double goalVelocityRadPerSec){ - io.setVelocity(goalVelocityRadPerSec); - return; - } - public void setVoltage(double voltage){ - io.setVoltage(voltage); - return; - } - - public double getDegAngle() { - return inputs.positionDeg; - } - - public double getRadAngle() { - return inputs.positionRad; - } - - public void zeroAngle() { - io.zeroAngle(); - } - public void TorusAngleReset(){ - io.TorusAngleReset(); - } - public boolean reachedSetpoint() { - return inputs.reachedSetpoint; - } - public void toggleMode() { - io.toggleMode(); - } - - public static Pivot getInstance(){ - if ( Pivot.instance == null){ - return new Pivot(Pivot.io); - } - return instance; - } - public static Pivot setInstance(Pivot inst){ - Pivot.instance = inst; - return Pivot.instance; - } -} +package frc.robot.subsystems.pivotComp; + +import org.littletonrobotics.junction.Logger; + + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Pivot extends SubsystemBase{ + + private static final double kRadPositionTolerance = Math.toRadians(5); + + private static PivotIO io; + private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); + private boolean velocityControlmode; + + PIDController upPositionFeedBackController; + PIDController downPositionFeedBackController; + + ArmFeedforward downPositionFeedForwardController; + ArmFeedforward upPositionFeedForwardController; + + private static Pivot instance = null; + + PIDController velocityFeedBackController; + ArmFeedforward velocityFeedForwardController; + double desiredDegAngle = 0; + public Pivot(PivotIO io) { + Pivot.io = io; + upPositionFeedBackController = new PIDController(4, 0, 0.03); //0.04 + upPositionFeedBackController.setTolerance(kRadPositionTolerance); + upPositionFeedForwardController = new ArmFeedforward(0.0,0,5); // 0,0,76 + + downPositionFeedBackController = new PIDController(7, 0, 0); + downPositionFeedBackController.setTolerance(kRadPositionTolerance); + downPositionFeedForwardController = new ArmFeedforward(0,0, 3); // 8 + + io.configureController(downPositionFeedForwardController, downPositionFeedBackController); + } + + @Override + public void periodic() { + if (desiredDegAngle > 45) { + io.configureController(downPositionFeedForwardController, downPositionFeedBackController); + } + else if (desiredDegAngle < 45) { + io.configureController(upPositionFeedForwardController, upPositionFeedBackController); + } + io.updateInputs(inputs); + Logger.processInputs("Pivot", inputs); + + Logger.recordOutput("Pivot/desiredDegAngle", desiredDegAngle); + io.setPosition(Math.toRadians(desiredDegAngle)); + // io.setPosition(desiredDegAngle); + } + + public void setDegAngle(double angle) { + desiredDegAngle = angle; + } + + public void setVelocityControlMode(boolean b){ + velocityControlmode = b; + }; + + public void setVelocitySetPoint(double goalVelocityRadPerSec){ + io.setVelocity(goalVelocityRadPerSec); + return; + } + public void setVoltage(double voltage){ + io.setVoltage(voltage); + return; + } + + public double getDegAngle() { + return inputs.positionDeg; + } + + public double getRadAngle() { + return inputs.positionRad; + } + + public void zeroAngle() { + io.zeroAngle(); + } + public void TorusAngleReset(){ + io.TorusAngleReset(); + } + public boolean reachedSetpoint() { + return inputs.reachedSetpoint; + } + public void toggleMode() { + io.toggleMode(); + } + + public static Pivot getInstance(){ + if ( Pivot.instance == null){ + return new Pivot(Pivot.io); + } + return instance; + } + public static Pivot setInstance(Pivot inst){ + Pivot.instance = inst; + return Pivot.instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/pivotComp/PivotIO.java b/src/main/java/frc/robot/subsystems/pivotComp/PivotIO.java index 3ac6e58..adc611c 100644 --- a/src/main/java/frc/robot/subsystems/pivotComp/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/pivotComp/PivotIO.java @@ -1,38 +1,38 @@ -package frc.robot.subsystems.pivotComp; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; - -public interface PivotIO { - @AutoLog - public static class PivotIOInputs { - public double positionRad; - public double positionDeg; - public double velocityDegSec; - public double velocityRadSec; - public boolean reachedSetpoint; - } - - public default void updateInputs(PivotIOInputs inputs) {} - - public default void setPosition(double goalPositionRad) {} - - public default void setVelocity(double goalVelocityRadPerSec) {} - - public default void configureController(ArmFeedforward pff, PIDController pfb) {} - - public default void setVoltage(double voltage) {} - - public default void zeroAngle() {} - - public default void toggleMode() {} - - public static Pivot getInstance(){ - return null; - } - public default void TorusAngleReset(){} - - -} +package frc.robot.subsystems.pivotComp; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; + +public interface PivotIO { + @AutoLog + public static class PivotIOInputs { + public double positionRad; + public double positionDeg; + public double velocityDegSec; + public double velocityRadSec; + public boolean reachedSetpoint; + } + + public default void updateInputs(PivotIOInputs inputs) {} + + public default void setPosition(double goalPositionRad) {} + + public default void setVelocity(double goalVelocityRadPerSec) {} + + public default void configureController(ArmFeedforward pff, PIDController pfb) {} + + public default void setVoltage(double voltage) {} + + public default void zeroAngle() {} + + public default void toggleMode() {} + + public static Pivot getInstance(){ + return null; + } + public default void TorusAngleReset(){} + + +} diff --git a/src/main/java/frc/robot/subsystems/pivotComp/PivotIONeo.java b/src/main/java/frc/robot/subsystems/pivotComp/PivotIONeo.java index 0c48c8c..4b85584 100644 --- a/src/main/java/frc/robot/subsystems/pivotComp/PivotIONeo.java +++ b/src/main/java/frc/robot/subsystems/pivotComp/PivotIONeo.java @@ -1,147 +1,147 @@ -package frc.robot.subsystems.pivotComp; - -import org.littletonrobotics.junction.Logger; - -import com.revrobotics.CANSparkMax; -// import com.revrobotics.CANSparkMaxLevel.MotorType; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.lib.util.RebelUtil; - -public class PivotIONeo extends SubsystemBase implements PivotIO { - private static final double kMotorToOutputShaftRatio = 0.00625; - private CANSparkMax m_motor = new CANSparkMax(15, CANSparkMax.MotorType.kBrushless); //Motor IDs DONE 2/8/2024 - - private PIDController positionFeedBackController = new PIDController(0, 0, 0); - private ArmFeedforward positionFeedForwardController = new ArmFeedforward(0, 0, 0); - private static final double kMAX_POSITION_RAD = Math.toRadians(910); //Math.toRadians(91) // 1.9 - private static final double kMIN_POSITION_RAD = Math.toRadians(-100); // -1 Math.toRadians(-1) - private static final double kMAX_VOLTAGE = 12; - private static double currentRadAngle; - private static double currentVelocityRadPerSec; - - // private DutyCycleEncoder absEncoder = new DutyCycleEncoder(5); - - public PivotIONeo() { - m_motor.setIdleMode(CANSparkMax.IdleMode.kBrake); //This is desirable, trust me kBrake - m_motor.clearFaults(); - m_motor.setInverted(true); - // absEncoder.setDistancePerRotation(-2 * Math.PI ); - zeroAngle(); - } - - - public void toggleMode() { - if (this.m_motor.getIdleMode() == CANSparkMax.IdleMode.kBrake) { - this.m_motor.setIdleMode(CANSparkMax.IdleMode.kCoast); - } - else { - this.m_motor.setIdleMode(CANSparkMax.IdleMode.kBrake); - } - } - - @Override - public void updateInputs(PivotIOInputs inputs) { - inputs.positionRad = m_motor.getEncoder().getPosition() * kMotorToOutputShaftRatio * Math.PI * 2; - currentRadAngle = m_motor.getEncoder().getPosition() * kMotorToOutputShaftRatio * Math.PI * 2; - // currentRadAngle = absEncoder.getDistance(); - // inputs.positionRad = currentRadAngle; - inputs.positionDeg = m_motor.getEncoder().getPosition() * kMotorToOutputShaftRatio * 360; - - inputs.velocityRadSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * Math.PI * 2; - currentVelocityRadPerSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * Math.PI * 2; - inputs.velocityDegSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * 360; - - inputs.reachedSetpoint = reachedSetpoint(true); - // Logger.recordOutput("pivotAbs", absEncoder.getDistance()); - // System.out.println("REV encoder: " + absEncoder.getDistance()); //0.692, 0.0146 - - // System.out.println("REV encoder: " + absEncoder.getAbsolutePosition()); //0.692, 0.0146 - - } - @Override - // sould be called periodically - public void setPosition(double goalPositionRad) { - - double ffVelo = 0; - if (goalPositionRad > currentRadAngle) { - ffVelo = 1; - } - else if (goalPositionRad < currentRadAngle) { - ffVelo = -1; - } - double feedForwardVoltage = positionFeedForwardController.calculate(goalPositionRad, ffVelo); - - if (Math.abs(currentRadAngle - goalPositionRad) < positionFeedBackController.getPositionTolerance()) { - feedForwardVoltage = 0; - } - Logger.recordOutput("Pivot/feedForwardVoltage", feedForwardVoltage); - - positionFeedBackController.setSetpoint(goalPositionRad); - double feedBackControllerVoltage = positionFeedBackController.calculate(currentRadAngle); - double voltageOut = feedForwardVoltage + feedBackControllerVoltage; - - voltageOut = RebelUtil.constrain(voltageOut, -12, 12); - - if ((currentRadAngle > kMAX_POSITION_RAD && voltageOut > 0) || - (currentRadAngle < kMIN_POSITION_RAD && voltageOut < 0) || - (goalPositionRad > kMAX_POSITION_RAD || goalPositionRad < kMIN_POSITION_RAD)) { - voltageOut = 0; - } - - Logger.recordOutput("Pivot/voltageOut", voltageOut); - m_motor.setVoltage(voltageOut); - } - - // @Override - // // sould be called periodically - // public void setVelocity(double goalVelocityRadPerSec) { - // double feedForwardVoltage = velocityFeedForwardController.calculate(goalVelocityRadPerSec, 0); - - // velocityFeedBackController.setSetpoint(goalVelocityRadPerSec); - // double feedBackControllerVoltage = velocityFeedBackController.calculate(currentVelocityRadPerSec); - // double outVoltage = feedForwardVoltage + feedBackControllerVoltage; - // if (outVoltage > kMAX_VOLTAGE) { - // outVoltage = 12; - // } - // else if (outVoltage < -kMAX_VOLTAGE) { - // outVoltage = -12; - // } - // //Logger.recordOutput("Pivot/voltageOut", outVoltage); - // m_motor.setVoltage(outVoltage); - - // } - - public void setVoltage(double voltage){ - if ((currentRadAngle > kMAX_POSITION_RAD && voltage > 0)|| (currentRadAngle < kMIN_POSITION_RAD && voltage < 0)) { - return; - } - else{ - Logger.recordOutput("Pivot/voltageOut", voltage); - m_motor.setVoltage(voltage); - } - } - - @Override - public void configureController(ArmFeedforward pff, PIDController pfb) { - positionFeedBackController = pfb; - positionFeedForwardController = pff; - } - - private boolean reachedSetpoint(boolean isPositionalControl) { - return positionFeedBackController.atSetpoint(); - } - - @Override - public void zeroAngle() { - m_motor.getEncoder().setPosition(0.0); - } - @Override - public void TorusAngleReset(){ - m_motor.getEncoder().setPosition(39.7); - } - +package frc.robot.subsystems.pivotComp; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.CANSparkMax; +// import com.revrobotics.CANSparkMaxLevel.MotorType; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.lib.util.RebelUtil; + +public class PivotIONeo extends SubsystemBase implements PivotIO { + private static final double kMotorToOutputShaftRatio = 0.00625; + private CANSparkMax m_motor = new CANSparkMax(15, CANSparkMax.MotorType.kBrushless); //Motor IDs DONE 2/8/2024 + + private PIDController positionFeedBackController = new PIDController(0, 0, 0); + private ArmFeedforward positionFeedForwardController = new ArmFeedforward(0, 0, 0); + private static final double kMAX_POSITION_RAD = Math.toRadians(910); //Math.toRadians(91) // 1.9 + private static final double kMIN_POSITION_RAD = Math.toRadians(-100); // -1 Math.toRadians(-1) + private static final double kMAX_VOLTAGE = 12; + private static double currentRadAngle; + private static double currentVelocityRadPerSec; + + // private DutyCycleEncoder absEncoder = new DutyCycleEncoder(5); + + public PivotIONeo() { + m_motor.setIdleMode(CANSparkMax.IdleMode.kBrake); //This is desirable, trust me kBrake + m_motor.clearFaults(); + m_motor.setInverted(true); + // absEncoder.setDistancePerRotation(-2 * Math.PI ); + zeroAngle(); + } + + + public void toggleMode() { + if (this.m_motor.getIdleMode() == CANSparkMax.IdleMode.kBrake) { + this.m_motor.setIdleMode(CANSparkMax.IdleMode.kCoast); + } + else { + this.m_motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + } + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + inputs.positionRad = m_motor.getEncoder().getPosition() * kMotorToOutputShaftRatio * Math.PI * 2; + currentRadAngle = m_motor.getEncoder().getPosition() * kMotorToOutputShaftRatio * Math.PI * 2; + // currentRadAngle = absEncoder.getDistance(); + // inputs.positionRad = currentRadAngle; + inputs.positionDeg = m_motor.getEncoder().getPosition() * kMotorToOutputShaftRatio * 360; + + inputs.velocityRadSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * Math.PI * 2; + currentVelocityRadPerSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * Math.PI * 2; + inputs.velocityDegSec = m_motor.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio * 360; + + inputs.reachedSetpoint = reachedSetpoint(true); + // Logger.recordOutput("pivotAbs", absEncoder.getDistance()); + // System.out.println("REV encoder: " + absEncoder.getDistance()); //0.692, 0.0146 + + // System.out.println("REV encoder: " + absEncoder.getAbsolutePosition()); //0.692, 0.0146 + + } + @Override + // sould be called periodically + public void setPosition(double goalPositionRad) { + + double ffVelo = 0; + if (goalPositionRad > currentRadAngle) { + ffVelo = 1; + } + else if (goalPositionRad < currentRadAngle) { + ffVelo = -1; + } + double feedForwardVoltage = positionFeedForwardController.calculate(goalPositionRad, ffVelo); + + if (Math.abs(currentRadAngle - goalPositionRad) < positionFeedBackController.getPositionTolerance()) { + feedForwardVoltage = 0; + } + Logger.recordOutput("Pivot/feedForwardVoltage", feedForwardVoltage); + + positionFeedBackController.setSetpoint(goalPositionRad); + double feedBackControllerVoltage = positionFeedBackController.calculate(currentRadAngle); + double voltageOut = feedForwardVoltage + feedBackControllerVoltage; + + voltageOut = RebelUtil.constrain(voltageOut, -12, 12); + + if ((currentRadAngle > kMAX_POSITION_RAD && voltageOut > 0) || + (currentRadAngle < kMIN_POSITION_RAD && voltageOut < 0) || + (goalPositionRad > kMAX_POSITION_RAD || goalPositionRad < kMIN_POSITION_RAD)) { + voltageOut = 0; + } + + Logger.recordOutput("Pivot/voltageOut", voltageOut); + m_motor.setVoltage(voltageOut); + } + + // @Override + // // sould be called periodically + // public void setVelocity(double goalVelocityRadPerSec) { + // double feedForwardVoltage = velocityFeedForwardController.calculate(goalVelocityRadPerSec, 0); + + // velocityFeedBackController.setSetpoint(goalVelocityRadPerSec); + // double feedBackControllerVoltage = velocityFeedBackController.calculate(currentVelocityRadPerSec); + // double outVoltage = feedForwardVoltage + feedBackControllerVoltage; + // if (outVoltage > kMAX_VOLTAGE) { + // outVoltage = 12; + // } + // else if (outVoltage < -kMAX_VOLTAGE) { + // outVoltage = -12; + // } + // //Logger.recordOutput("Pivot/voltageOut", outVoltage); + // m_motor.setVoltage(outVoltage); + + // } + + public void setVoltage(double voltage){ + if ((currentRadAngle > kMAX_POSITION_RAD && voltage > 0)|| (currentRadAngle < kMIN_POSITION_RAD && voltage < 0)) { + return; + } + else{ + Logger.recordOutput("Pivot/voltageOut", voltage); + m_motor.setVoltage(voltage); + } + } + + @Override + public void configureController(ArmFeedforward pff, PIDController pfb) { + positionFeedBackController = pfb; + positionFeedForwardController = pff; + } + + private boolean reachedSetpoint(boolean isPositionalControl) { + return positionFeedBackController.atSetpoint(); + } + + @Override + public void zeroAngle() { + m_motor.getEncoder().setPosition(0.0); + } + @Override + public void TorusAngleReset(){ + m_motor.getEncoder().setPosition(39.7); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivotComp/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivotComp/PivotIOSim.java index c82295b..12a5e29 100644 --- a/src/main/java/frc/robot/subsystems/pivotComp/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/pivotComp/PivotIOSim.java @@ -1,60 +1,60 @@ -package frc.robot.subsystems.pivotComp; - -import org.littletonrobotics.junction.Logger; - -import com.revrobotics.CANSparkMax; -// import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class PivotIOSim extends SubsystemBase implements PivotIO { - - double desiredPositionRad = 0; - double desiredPositionDeg = 0; - double desiredVelocityRadSec = 0; - double desiredVelocityDegSec = 0; - - private static double currentRadAngle; - private static double currentVelocityRadPerSec; - - @Override - public void updateInputs(PivotIOInputs inputs) { - inputs.positionRad = desiredPositionRad; - inputs.positionDeg = desiredPositionDeg; - currentRadAngle = inputs.positionRad; - - inputs.velocityRadSec = desiredVelocityRadSec; - currentVelocityRadPerSec = inputs.velocityRadSec; - inputs.velocityDegSec = desiredVelocityDegSec; - - inputs.reachedSetpoint = true; - } - - @Override - // sould be called periodically - public void setPosition(double goalPositionRad) { - desiredPositionRad = goalPositionRad; - desiredPositionDeg = Math.toDegrees(goalPositionRad); - } - - @Override - // sould be called periodically - public void setVelocity(double goalVelocityRadPerSec) { - desiredVelocityRadSec = goalVelocityRadPerSec; - desiredVelocityDegSec = Math.toDegrees(goalVelocityRadPerSec); - } - - public void setVoltage(double voltage){ - } - - @Override - public void configureController(ArmFeedforward pff, PIDController pfb ) { - } - - @Override - public void zeroAngle() { - } - +package frc.robot.subsystems.pivotComp; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.CANSparkMax; +// import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class PivotIOSim extends SubsystemBase implements PivotIO { + + double desiredPositionRad = 0; + double desiredPositionDeg = 0; + double desiredVelocityRadSec = 0; + double desiredVelocityDegSec = 0; + + private static double currentRadAngle; + private static double currentVelocityRadPerSec; + + @Override + public void updateInputs(PivotIOInputs inputs) { + inputs.positionRad = desiredPositionRad; + inputs.positionDeg = desiredPositionDeg; + currentRadAngle = inputs.positionRad; + + inputs.velocityRadSec = desiredVelocityRadSec; + currentVelocityRadPerSec = inputs.velocityRadSec; + inputs.velocityDegSec = desiredVelocityDegSec; + + inputs.reachedSetpoint = true; + } + + @Override + // sould be called periodically + public void setPosition(double goalPositionRad) { + desiredPositionRad = goalPositionRad; + desiredPositionDeg = Math.toDegrees(goalPositionRad); + } + + @Override + // sould be called periodically + public void setVelocity(double goalVelocityRadPerSec) { + desiredVelocityRadSec = goalVelocityRadPerSec; + desiredVelocityDegSec = Math.toDegrees(goalVelocityRadPerSec); + } + + public void setVoltage(double voltage){ + } + + @Override + public void configureController(ArmFeedforward pff, PIDController pfb ) { + } + + @Override + public void zeroAngle() { + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIO.java b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIO.java index 87e4541..ba665d9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIO.java @@ -1,22 +1,22 @@ -// package frc.robot.subsystems.shooter.pivot.flywheel; - -// import org.littletonrobotics.junction.AutoLog; - -// public interface FlywheeIO { -// @AutoLog -// public static class FlywheeIOInputs { -// public double RPM; - -// public double tTemp; -// public double bTemp; - -// public double tAmps; -// public double bAmps; - -// public double tVolts; -// public double bVolts; -// } - -// public default void updateInputs(FlywheeIOInputs inputs) {} -// public default void setVoltage(double voltage) {} -// } +// package frc.robot.subsystems.shooter.pivot.flywheel; + +// import org.littletonrobotics.junction.AutoLog; + +// public interface FlywheeIO { +// @AutoLog +// public static class FlywheeIOInputs { +// public double RPM; + +// public double tTemp; +// public double bTemp; + +// public double tAmps; +// public double bAmps; + +// public double tVolts; +// public double bVolts; +// } + +// public default void updateInputs(FlywheeIOInputs inputs) {} +// public default void setVoltage(double voltage) {} +// } diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSim.java b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSim.java index 5ae4f0b..0bc223f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSim.java @@ -1,41 +1,41 @@ -// package frc.robot.subsystems.shooter.pivot.flywheel; - - -// import edu.wpi.first.math.system.plant.DCMotor; -// import edu.wpi.first.wpilibj.simulation.FlywheelSim; - -// public class FlywheeIOSim implements FlywheeIO{ - -// private static final double kMOTOR_TO_OUTPUT_RATIO = 2; - -// private DCMotor m_gearBoxT = DCMotor.getNeo550(1); -// private DCMotor m_gearboxB = DCMotor.getNeo550(1); - -// private FlywheelSim m_topSim = new FlywheelSim(m_gearBoxT, kMOTOR_TO_OUTPUT_RATIO, 0.007); -// private FlywheelSim m_bottomSim = new FlywheelSim(m_gearboxB, kMOTOR_TO_OUTPUT_RATIO, 0.007); - -// private double voltage = 0; - -// @Override -// public void updateInputs(FlywheeIOInputs inputs) { -// m_topSim.update(0.020); -// m_bottomSim.update(0.020); - -// inputs.RPM = m_topSim.getAngularVelocityRPM(); - -// inputs.tAmps = m_topSim.getCurrentDrawAmps(); -// inputs.bAmps = m_bottomSim.getCurrentDrawAmps(); - -// inputs.tTemp = 0; -// inputs.bTemp = 0; - -// inputs.tVolts = voltage; -// inputs.bVolts = voltage; -// } - -// public void setVoltage(double voltage) { -// this.voltage = voltage; -// m_topSim.setInputVoltage(voltage); -// m_bottomSim.setInputVoltage(voltage); -// } -// } +// package frc.robot.subsystems.shooter.pivot.flywheel; + + +// import edu.wpi.first.math.system.plant.DCMotor; +// import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +// public class FlywheeIOSim implements FlywheeIO{ + +// private static final double kMOTOR_TO_OUTPUT_RATIO = 2; + +// private DCMotor m_gearBoxT = DCMotor.getNeo550(1); +// private DCMotor m_gearboxB = DCMotor.getNeo550(1); + +// private FlywheelSim m_topSim = new FlywheelSim(m_gearBoxT, kMOTOR_TO_OUTPUT_RATIO, 0.007); +// private FlywheelSim m_bottomSim = new FlywheelSim(m_gearboxB, kMOTOR_TO_OUTPUT_RATIO, 0.007); + +// private double voltage = 0; + +// @Override +// public void updateInputs(FlywheeIOInputs inputs) { +// m_topSim.update(0.020); +// m_bottomSim.update(0.020); + +// inputs.RPM = m_topSim.getAngularVelocityRPM(); + +// inputs.tAmps = m_topSim.getCurrentDrawAmps(); +// inputs.bAmps = m_bottomSim.getCurrentDrawAmps(); + +// inputs.tTemp = 0; +// inputs.bTemp = 0; + +// inputs.tVolts = voltage; +// inputs.bVolts = voltage; +// } + +// public void setVoltage(double voltage) { +// this.voltage = voltage; +// m_topSim.setInputVoltage(voltage); +// m_bottomSim.setInputVoltage(voltage); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSparkMAX.java b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSparkMAX.java index 2083f37..11ac531 100644 --- a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSparkMAX.java +++ b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/FlywheeIOSparkMAX.java @@ -1,42 +1,42 @@ -// package frc.robot.subsystems.shooter.pivot.flywheel; - -// import com.revrobotics.CANSparkBase.IdleMode; - -// import frc.robot.subsystems.indexer.Indexer; - -// import com.revrobotics.CANSparkMax; - -// public class FlywheeIOSparkMAX implements FlywheeIO{ - -// private static final double kMOTOR_TO_OUTPUT_RATIO = 2; - -// private CANSparkMax m_motorT = new CANSparkMax(88, CANSparkMax.MotorType.kBrushless); -// private CANSparkMax m_motorB = new CANSparkMax(89, CANSparkMax.MotorType.kBrushless); - -// public FlywheeIOSparkMAX() { -// m_motorT.setIdleMode(IdleMode.kBrake); -// m_motorB.setIdleMode(IdleMode.kBrake); - -// m_motorT.setInverted(false); -// m_motorB.setInverted(false); -// } - -// @Override -// public void updateInputs(FlywheeIOInputs inputs) { -// inputs.RPM = m_motorT.getEncoder().getVelocity() * kMOTOR_TO_OUTPUT_RATIO; - -// inputs.tAmps = m_motorT.getOutputCurrent(); -// inputs.bAmps = m_motorB.getOutputCurrent(); - -// inputs.tTemp = m_motorT.getMotorTemperature(); -// inputs.bTemp = m_motorB.getMotorTemperature(); - -// inputs.tVolts = m_motorT.getAppliedOutput() * 12; -// inputs.bVolts = m_motorB.getAppliedOutput() * 12; -// } - -// public void setVoltage(double voltage) { -// m_motorT.setVoltage(voltage); -// m_motorB.setVoltage(voltage); -// } -// } +// package frc.robot.subsystems.shooter.pivot.flywheel; + +// import com.revrobotics.CANSparkBase.IdleMode; + +// import frc.robot.subsystems.indexer.Indexer; + +// import com.revrobotics.CANSparkMax; + +// public class FlywheeIOSparkMAX implements FlywheeIO{ + +// private static final double kMOTOR_TO_OUTPUT_RATIO = 2; + +// private CANSparkMax m_motorT = new CANSparkMax(88, CANSparkMax.MotorType.kBrushless); +// private CANSparkMax m_motorB = new CANSparkMax(89, CANSparkMax.MotorType.kBrushless); + +// public FlywheeIOSparkMAX() { +// m_motorT.setIdleMode(IdleMode.kBrake); +// m_motorB.setIdleMode(IdleMode.kBrake); + +// m_motorT.setInverted(false); +// m_motorB.setInverted(false); +// } + +// @Override +// public void updateInputs(FlywheeIOInputs inputs) { +// inputs.RPM = m_motorT.getEncoder().getVelocity() * kMOTOR_TO_OUTPUT_RATIO; + +// inputs.tAmps = m_motorT.getOutputCurrent(); +// inputs.bAmps = m_motorB.getOutputCurrent(); + +// inputs.tTemp = m_motorT.getMotorTemperature(); +// inputs.bTemp = m_motorB.getMotorTemperature(); + +// inputs.tVolts = m_motorT.getAppliedOutput() * 12; +// inputs.bVolts = m_motorB.getAppliedOutput() * 12; +// } + +// public void setVoltage(double voltage) { +// m_motorT.setVoltage(voltage); +// m_motorB.setVoltage(voltage); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/Flywheel.java index b7237b9..9de958f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/pivot/flywheel/Flywheel.java @@ -1,73 +1,73 @@ -// package frc.robot.subsystems.shooter.pivot.flywheel; - -// import org.littletonrobotics.junction.Logger; - -// import edu.wpi.first.math.controller.SimpleMotorFeedforward; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import frc.robot.Constants; -// import frc.robot.lib.util.RebelUtil; - -// public class Flywheel extends SubsystemBase { -// private final FlywheeIOInputsAutoLogged inputs = new FlywheeIOInputsAutoLogged(); -// private FlywheeIO io; - -// private final SimpleMotorFeedforward realFF = new SimpleMotorFeedforward(0, 0, 0); -// private final SimpleMotorFeedforward simFF = new SimpleMotorFeedforward(0, 0.00208, .012); -// private SimpleMotorFeedforward feedforward; - -// private double desiredRPM = 0; -// public Flywheel() { -// switch(Constants.currentMode) { -// case REAL: -// io = new FlywheeIOSparkMAX(); -// feedforward = realFF; -// break; -// case SIM: -// io = new FlywheeIOSim(); -// feedforward = simFF; -// break; -// case REPLAY_REAL: -// io = new FlywheeIO() {}; -// feedforward = realFF; -// break; -// default: // REPLAY_SIM -// io = new FlywheeIO() {}; -// feedforward = simFF; -// break; -// } -// } - -// @Override -// public void periodic() { -// io.updateInputs(inputs); -// Logger.processInputs("Flywheel", inputs); - -// double voltage = feedforward.calculate(desiredRPM, Math.signum(desiredRPM)); -// RebelUtil.constrain(voltage, -12, 12); - -// if (Math.max(inputs.tAmps, inputs.bAmps) > 40 || -// Math.max(inputs.tVolts, inputs.tVolts) > 12 || -// Math.max(inputs.tTemp, inputs.tTemp) >= 94) { -// voltage = 0; - -// System.err.println("SHOOTER MOTOR NOT WITHIN OPERATION RANGE"); -// Logger.recordOutput("Flywheel/withinOperationRange", false); -// } -// else { -// Logger.recordOutput("Flywheel/withinOperationRange", true); -// } - -// io.setVoltage(voltage); -// Logger.recordOutput("Flywheel/calculatedVoltage", voltage); - -// } - -// public void setRPM(double rpm) { -// desiredRPM = rpm; -// Logger.recordOutput("Flywheel/desiredRMP", desiredRPM); -// } - -// public boolean reachedSetpoint() { -// return Math.abs(inputs.RPM - desiredRPM) <= 5; -// } -// } +// package frc.robot.subsystems.shooter.pivot.flywheel; + +// import org.littletonrobotics.junction.Logger; + +// import edu.wpi.first.math.controller.SimpleMotorFeedforward; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc.robot.Constants; +// import frc.robot.lib.util.RebelUtil; + +// public class Flywheel extends SubsystemBase { +// private final FlywheeIOInputsAutoLogged inputs = new FlywheeIOInputsAutoLogged(); +// private FlywheeIO io; + +// private final SimpleMotorFeedforward realFF = new SimpleMotorFeedforward(0, 0, 0); +// private final SimpleMotorFeedforward simFF = new SimpleMotorFeedforward(0, 0.00208, .012); +// private SimpleMotorFeedforward feedforward; + +// private double desiredRPM = 0; +// public Flywheel() { +// switch(Constants.currentMode) { +// case REAL: +// io = new FlywheeIOSparkMAX(); +// feedforward = realFF; +// break; +// case SIM: +// io = new FlywheeIOSim(); +// feedforward = simFF; +// break; +// case REPLAY_REAL: +// io = new FlywheeIO() {}; +// feedforward = realFF; +// break; +// default: // REPLAY_SIM +// io = new FlywheeIO() {}; +// feedforward = simFF; +// break; +// } +// } + +// @Override +// public void periodic() { +// io.updateInputs(inputs); +// Logger.processInputs("Flywheel", inputs); + +// double voltage = feedforward.calculate(desiredRPM, Math.signum(desiredRPM)); +// RebelUtil.constrain(voltage, -12, 12); + +// if (Math.max(inputs.tAmps, inputs.bAmps) > 40 || +// Math.max(inputs.tVolts, inputs.tVolts) > 12 || +// Math.max(inputs.tTemp, inputs.tTemp) >= 94) { +// voltage = 0; + +// System.err.println("SHOOTER MOTOR NOT WITHIN OPERATION RANGE"); +// Logger.recordOutput("Flywheel/withinOperationRange", false); +// } +// else { +// Logger.recordOutput("Flywheel/withinOperationRange", true); +// } + +// io.setVoltage(voltage); +// Logger.recordOutput("Flywheel/calculatedVoltage", voltage); + +// } + +// public void setRPM(double rpm) { +// desiredRPM = rpm; +// Logger.recordOutput("Flywheel/desiredRMP", desiredRPM); +// } + +// public boolean reachedSetpoint() { +// return Math.abs(inputs.RPM - desiredRPM) <= 5; +// } +// } diff --git a/src/main/java/frc/robot/subsystems/shooterComp/Shooter.java b/src/main/java/frc/robot/subsystems/shooterComp/Shooter.java index 2525944..40cd819 100644 --- a/src/main/java/frc/robot/subsystems/shooterComp/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooterComp/Shooter.java @@ -1,87 +1,87 @@ -package frc.robot.subsystems.shooterComp; - -import org.littletonrobotics.junction.Logger; - - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Shooter extends SubsystemBase{ - - private static final double kVelocityRadSecTolerance = 1; - - private static ShooterIO io; - private static Shooter instance = null; - private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - - private boolean isVariable = false; - private double Top = 0; - private double Bottom = 0; - - PIDController velocityFeedBackController; - SimpleMotorFeedforward velocityFeedForwardController; - - double desiredVelocityRadSec = 0; - public Shooter(ShooterIO io) { - Shooter.io = io; - velocityFeedBackController = new PIDController(0, 0, 0); // .68 - velocityFeedBackController.setTolerance(kVelocityRadSecTolerance); - velocityFeedForwardController = new SimpleMotorFeedforward(0.105, 0.13, 0.089); //0.135, 0.11, 0.035 - io.configureController(velocityFeedForwardController, velocityFeedBackController); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Shooter", inputs); - - Logger.recordOutput("Shooter/desiredVelocityRadSec", desiredVelocityRadSec); - io.setVelocityRadSec(desiredVelocityRadSec, isVariable, Bottom, Top); - } - - public void setVelocityRadSec(double velo, boolean isVar, double BottomWheel, double TopWheel) { - desiredVelocityRadSec = velo; - isVariable = isVar; - Bottom = BottomWheel; - Top = TopWheel; - return; - } - - public void setVoltage(double voltage){ - io.setVoltage(voltage); - return; - } - - public double getVelocityDegSec() { - return Math.toDegrees(inputs.velocityRadSec); - } - - public double getVelocityRadSec() { - return inputs.velocityRadSec; - } - - public boolean reachedSetpoint() { - return inputs.reachedSetpoint; - } - - public boolean inShooter() { - return inputs.inShooter; - } - public static Shooter getInstance(){ - if(Shooter.instance == null){ - return new Shooter(Shooter.io); - } - return instance; - } - public static Shooter setInstance(Shooter inst){ - Shooter.instance = inst; - return Shooter.instance; - } - - public double getDesiredVelocity() { - return inputs.desiredVelocityRadSec; - } - - -} +package frc.robot.subsystems.shooterComp; + +import org.littletonrobotics.junction.Logger; + + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase{ + + private static final double kVelocityRadSecTolerance = 1; + + private static ShooterIO io; + private static Shooter instance = null; + private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + + private boolean isVariable = false; + private double Top = 0; + private double Bottom = 0; + + PIDController velocityFeedBackController; + SimpleMotorFeedforward velocityFeedForwardController; + + double desiredVelocityRadSec = 0; + public Shooter(ShooterIO io) { + Shooter.io = io; + velocityFeedBackController = new PIDController(0, 0, 0); // .68 + velocityFeedBackController.setTolerance(kVelocityRadSecTolerance); + velocityFeedForwardController = new SimpleMotorFeedforward(0.105, 0.13, 0.089); //0.135, 0.11, 0.035 + io.configureController(velocityFeedForwardController, velocityFeedBackController); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Shooter", inputs); + + Logger.recordOutput("Shooter/desiredVelocityRadSec", desiredVelocityRadSec); + io.setVelocityRadSec(desiredVelocityRadSec, isVariable, Bottom, Top); + } + + public void setVelocityRadSec(double velo, boolean isVar, double BottomWheel, double TopWheel) { + desiredVelocityRadSec = velo; + isVariable = isVar; + Bottom = BottomWheel; + Top = TopWheel; + return; + } + + public void setVoltage(double voltage){ + io.setVoltage(voltage); + return; + } + + public double getVelocityDegSec() { + return Math.toDegrees(inputs.velocityRadSec); + } + + public double getVelocityRadSec() { + return inputs.velocityRadSec; + } + + public boolean reachedSetpoint() { + return inputs.reachedSetpoint; + } + + public boolean inShooter() { + return inputs.inShooter; + } + public static Shooter getInstance(){ + if(Shooter.instance == null){ + return new Shooter(Shooter.io); + } + return instance; + } + public static Shooter setInstance(Shooter inst){ + Shooter.instance = inst; + return Shooter.instance; + } + + public double getDesiredVelocity() { + return inputs.desiredVelocityRadSec; + } + + +} diff --git a/src/main/java/frc/robot/subsystems/shooterComp/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooterComp/ShooterIO.java index ce966eb..ebce44c 100644 --- a/src/main/java/frc/robot/subsystems/shooterComp/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooterComp/ShooterIO.java @@ -1,26 +1,26 @@ -package frc.robot.subsystems.shooterComp; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; - -public interface ShooterIO { - @AutoLog - public static class ShooterIOInputs { - public double velocityRadSec; - public double desiredVelocityRadSec; - public boolean reachedSetpoint; - public boolean inShooter; - } - - public default void updateInputs(ShooterIOInputs inputs) {} - - public default void setVelocityRadSec(double goalVelocityRadPerSec, boolean isVar, double Bot, double Top) {} - - public default double getDesiredVelocity() {return 0.0;} - - public default void configureController(SimpleMotorFeedforward vff, PIDController vfb) {} - - public default void setVoltage(double voltage) {} -} +package frc.robot.subsystems.shooterComp; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + +public interface ShooterIO { + @AutoLog + public static class ShooterIOInputs { + public double velocityRadSec; + public double desiredVelocityRadSec; + public boolean reachedSetpoint; + public boolean inShooter; + } + + public default void updateInputs(ShooterIOInputs inputs) {} + + public default void setVelocityRadSec(double goalVelocityRadPerSec, boolean isVar, double Bot, double Top) {} + + public default double getDesiredVelocity() {return 0.0;} + + public default void configureController(SimpleMotorFeedforward vff, PIDController vfb) {} + + public default void setVoltage(double voltage) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooterComp/ShooterIONeo.java b/src/main/java/frc/robot/subsystems/shooterComp/ShooterIONeo.java index 2a46d3b..bcbdf17 100644 --- a/src/main/java/frc/robot/subsystems/shooterComp/ShooterIONeo.java +++ b/src/main/java/frc/robot/subsystems/shooterComp/ShooterIONeo.java @@ -1,155 +1,155 @@ -package frc.robot.subsystems.shooterComp; - -import org.littletonrobotics.junction.Logger; - -// import com.ctre.phoenix6.hardware.TalonFX; -// import com.ctre.phoenix6.signals.NeutralModeValue; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; - -// import com.revrobotics.CANSparkMaxLevel.MotorType; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.lib.util.RebelUtil; -import frc.robot.subsystems.indexer.Indexer; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; - - -// import com.revrobotics.jni.VL53L0XJNI; - -public class ShooterIONeo extends SubsystemBase implements ShooterIO { - private static final double kMotorToOutputShaftRatio = 1; //Last Checked 2/6/2024 - private double wheelVelo = 0.0; - private CANSparkMax m_motor1 = new CANSparkMax(13, CANSparkMax.MotorType.kBrushless); - private CANSparkMax m_motor2 = new CANSparkMax(14, CANSparkMax.MotorType.kBrushless); - // private CANSparkMax m_motor1 = new CANSparkMax(29, MotorType.kBrushless); - // private CANSparkMax m_motor2 = new CANSparkMax(23, MotorType.kBrushless); - - private double goalVelocityRadPerSec = 0.0; - - private PIDController velocityFeedBackController = new PIDController(0, 0, 0); - private SimpleMotorFeedforward velocityFeedForwardController = new SimpleMotorFeedforward(0, 0, 0); - - private double distanceTolerance; - private double currentVelocityRadPerSec = 0 ; - private double currentVelocityRadPerSec2 = 0; //Second motor, m_motor2 - - private GenericEntry ShooterStatus; - private GenericEntry InShooter; - - - private static final double kMAX_VOLTAGE = 12; - - - private final Indexer indexer; - - public ShooterIONeo(Indexer indexer) { - this.indexer = indexer; - m_motor1.clearFaults(); - m_motor2.clearFaults(); - - m_motor1.setIdleMode(IdleMode.kBrake); - m_motor2.setIdleMode(IdleMode.kBrake); - - m_motor1.setInverted(false); - m_motor2.setInverted(false); - - ShooterStatus = Shuffleboard.getTab("auto").add("SHOOTER STATUS", currentVelocityRadPerSec > 60).getEntry(); - InShooter = Shuffleboard.getTab("auto").add("In Shooter", isInShooter()).getEntry(); - } - - @Override - public void updateInputs(ShooterIOInputs inputs) { - inputs.velocityRadSec = m_motor1.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio; // we divide by 60 because the motor out is in RPM - currentVelocityRadPerSec2 = m_motor2.getEncoder().getVelocity() /60 * kMotorToOutputShaftRatio; - currentVelocityRadPerSec = inputs.velocityRadSec; - inputs.reachedSetpoint = velocityFeedBackController.atSetpoint(); - inputs.inShooter = isInShooter(); - } - - @Override - public double getDesiredVelocity() { - return goalVelocityRadPerSec; - } - - @Override - // should be called periodically - public void setVelocityRadSec(double goalVelocityRadPerSec, boolean isVariable, double BottomSpeed, double TopSpeed) { - this.goalVelocityRadPerSec = goalVelocityRadPerSec; - if(isVariable){ - double ffVelo = 0; - if (BottomSpeed > currentVelocityRadPerSec) { - ffVelo = 1; - } - else if (BottomSpeed < currentVelocityRadPerSec) { - ffVelo = -1; - } - - double feedForwardVoltage = velocityFeedForwardController.calculate(BottomSpeed, ffVelo); - double outVoltage1 = feedForwardVoltage; - ffVelo = 0; - if (TopSpeed > currentVelocityRadPerSec) { - ffVelo = 1; - } - else if (TopSpeed < currentVelocityRadPerSec) { - ffVelo = -1; - } - feedForwardVoltage = velocityFeedForwardController.calculate(TopSpeed, ffVelo); - double outVoltage2 = feedForwardVoltage; - - m_motor1.setVoltage(outVoltage2); - m_motor2.setVoltage(outVoltage1); - }else{ - double ffVelo = 0; - if (goalVelocityRadPerSec > currentVelocityRadPerSec) { - ffVelo = 1; - } - else if (goalVelocityRadPerSec < currentVelocityRadPerSec) { - ffVelo = -1; - } - - double feedForwardVoltage = velocityFeedForwardController.calculate(goalVelocityRadPerSec, ffVelo); - velocityFeedBackController.setSetpoint(goalVelocityRadPerSec); - double feedBackControllerVoltage = velocityFeedBackController.calculate(currentVelocityRadPerSec); - double outVoltage = feedForwardVoltage + feedBackControllerVoltage; - // double outVoltage = feedBackControllerVoltage; - - outVoltage = RebelUtil.constrain(outVoltage, -12, 12); - // if (goalVelocityRadPerSec > 0) { - // outVoltage = 6; - // } - // else { - // outVoltage = 0; - // } - Logger.recordOutput("Shooter/voltageOut", outVoltage); - m_motor1.setVoltage(outVoltage); - m_motor2.setVoltage(outVoltage); - - ShooterStatus.setBoolean(currentVelocityRadPerSec > 60); - } - - InShooter.setBoolean(isInShooter()); - this.goalVelocityRadPerSec = goalVelocityRadPerSec; - } - - @Override - public void setVoltage(double voltage){ - m_motor1.setVoltage(voltage); - } - - @Override - public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { - velocityFeedBackController = vfb; - velocityFeedForwardController = vff; - } - - private boolean isInShooter(){ - return indexer.inShooter(); - } - +package frc.robot.subsystems.shooterComp; + +import org.littletonrobotics.junction.Logger; + +// import com.ctre.phoenix6.hardware.TalonFX; +// import com.ctre.phoenix6.signals.NeutralModeValue; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + +// import com.revrobotics.CANSparkMaxLevel.MotorType; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.lib.util.RebelUtil; +import frc.robot.subsystems.indexer.Indexer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; + + +// import com.revrobotics.jni.VL53L0XJNI; + +public class ShooterIONeo extends SubsystemBase implements ShooterIO { + private static final double kMotorToOutputShaftRatio = 1; //Last Checked 2/6/2024 + private double wheelVelo = 0.0; + private CANSparkMax m_motor1 = new CANSparkMax(13, CANSparkMax.MotorType.kBrushless); + private CANSparkMax m_motor2 = new CANSparkMax(14, CANSparkMax.MotorType.kBrushless); + // private CANSparkMax m_motor1 = new CANSparkMax(29, MotorType.kBrushless); + // private CANSparkMax m_motor2 = new CANSparkMax(23, MotorType.kBrushless); + + private double goalVelocityRadPerSec = 0.0; + + private PIDController velocityFeedBackController = new PIDController(0, 0, 0); + private SimpleMotorFeedforward velocityFeedForwardController = new SimpleMotorFeedforward(0, 0, 0); + + private double distanceTolerance; + private double currentVelocityRadPerSec = 0 ; + private double currentVelocityRadPerSec2 = 0; //Second motor, m_motor2 + + private GenericEntry ShooterStatus; + private GenericEntry InShooter; + + + private static final double kMAX_VOLTAGE = 12; + + + private final Indexer indexer; + + public ShooterIONeo(Indexer indexer) { + this.indexer = indexer; + m_motor1.clearFaults(); + m_motor2.clearFaults(); + + m_motor1.setIdleMode(IdleMode.kBrake); + m_motor2.setIdleMode(IdleMode.kBrake); + + m_motor1.setInverted(false); + m_motor2.setInverted(false); + + ShooterStatus = Shuffleboard.getTab("auto").add("SHOOTER STATUS", currentVelocityRadPerSec > 60).getEntry(); + InShooter = Shuffleboard.getTab("auto").add("In Shooter", isInShooter()).getEntry(); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.velocityRadSec = m_motor1.getEncoder().getVelocity() / 60 * kMotorToOutputShaftRatio; // we divide by 60 because the motor out is in RPM + currentVelocityRadPerSec2 = m_motor2.getEncoder().getVelocity() /60 * kMotorToOutputShaftRatio; + currentVelocityRadPerSec = inputs.velocityRadSec; + inputs.reachedSetpoint = velocityFeedBackController.atSetpoint(); + inputs.inShooter = isInShooter(); + } + + @Override + public double getDesiredVelocity() { + return goalVelocityRadPerSec; + } + + @Override + // should be called periodically + public void setVelocityRadSec(double goalVelocityRadPerSec, boolean isVariable, double BottomSpeed, double TopSpeed) { + this.goalVelocityRadPerSec = goalVelocityRadPerSec; + if(isVariable){ + double ffVelo = 0; + if (BottomSpeed > currentVelocityRadPerSec) { + ffVelo = 1; + } + else if (BottomSpeed < currentVelocityRadPerSec) { + ffVelo = -1; + } + + double feedForwardVoltage = velocityFeedForwardController.calculate(BottomSpeed, ffVelo); + double outVoltage1 = feedForwardVoltage; + ffVelo = 0; + if (TopSpeed > currentVelocityRadPerSec) { + ffVelo = 1; + } + else if (TopSpeed < currentVelocityRadPerSec) { + ffVelo = -1; + } + feedForwardVoltage = velocityFeedForwardController.calculate(TopSpeed, ffVelo); + double outVoltage2 = feedForwardVoltage; + + m_motor1.setVoltage(outVoltage2); + m_motor2.setVoltage(outVoltage1); + }else{ + double ffVelo = 0; + if (goalVelocityRadPerSec > currentVelocityRadPerSec) { + ffVelo = 1; + } + else if (goalVelocityRadPerSec < currentVelocityRadPerSec) { + ffVelo = -1; + } + + double feedForwardVoltage = velocityFeedForwardController.calculate(goalVelocityRadPerSec, ffVelo); + velocityFeedBackController.setSetpoint(goalVelocityRadPerSec); + double feedBackControllerVoltage = velocityFeedBackController.calculate(currentVelocityRadPerSec); + double outVoltage = feedForwardVoltage + feedBackControllerVoltage; + // double outVoltage = feedBackControllerVoltage; + + outVoltage = RebelUtil.constrain(outVoltage, -12, 12); + // if (goalVelocityRadPerSec > 0) { + // outVoltage = 6; + // } + // else { + // outVoltage = 0; + // } + Logger.recordOutput("Shooter/voltageOut", outVoltage); + m_motor1.setVoltage(outVoltage); + m_motor2.setVoltage(outVoltage); + + ShooterStatus.setBoolean(currentVelocityRadPerSec > 60); + } + + InShooter.setBoolean(isInShooter()); + this.goalVelocityRadPerSec = goalVelocityRadPerSec; + } + + @Override + public void setVoltage(double voltage){ + m_motor1.setVoltage(voltage); + } + + @Override + public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { + velocityFeedBackController = vfb; + velocityFeedForwardController = vff; + } + + private boolean isInShooter(){ + return indexer.inShooter(); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooterComp/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooterComp/ShooterIOSim.java index 4c39df1..82f0f23 100644 --- a/src/main/java/frc/robot/subsystems/shooterComp/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooterComp/ShooterIOSim.java @@ -1,36 +1,36 @@ -package frc.robot.subsystems.shooterComp; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.indexer.Indexer; - -public class ShooterIOSim extends SubsystemBase implements ShooterIO { - - double desiredVelocityRadSec = 0; - - private final Indexer indexer; - public ShooterIOSim(Indexer indexer) { - this.indexer = indexer; - } - @Override - public void updateInputs(ShooterIOInputs inputs) { - inputs.velocityRadSec = desiredVelocityRadSec; - inputs.inShooter = indexer.inShooter(); - } - - @Override - // sould be called periodically - public void setVelocityRadSec(double goalVelocityRadPerSec, boolean isVar, double Bottom, double Top) { - desiredVelocityRadSec = goalVelocityRadPerSec; - } - - @Override - public void setVoltage(double voltage){ - } - - @Override - public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { - } - +package frc.robot.subsystems.shooterComp; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.indexer.Indexer; + +public class ShooterIOSim extends SubsystemBase implements ShooterIO { + + double desiredVelocityRadSec = 0; + + private final Indexer indexer; + public ShooterIOSim(Indexer indexer) { + this.indexer = indexer; + } + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.velocityRadSec = desiredVelocityRadSec; + inputs.inShooter = indexer.inShooter(); + } + + @Override + // sould be called periodically + public void setVelocityRadSec(double goalVelocityRadPerSec, boolean isVar, double Bottom, double Top) { + desiredVelocityRadSec = goalVelocityRadPerSec; + } + + @Override + public void setVoltage(double voltage){ + } + + @Override + public void configureController(SimpleMotorFeedforward vff, PIDController vfb ) { + } + } \ No newline at end of file