diff --git a/AdvantageScope 5-20-2026.json b/AdvantageScope 5-20-2026.json new file mode 100644 index 00000000..dc42e2d3 --- /dev/null +++ b/AdvantageScope 5-20-2026.json @@ -0,0 +1,1556 @@ +{ + "hubs": [ + { + "x": -8, + "y": -8, + "width": 1552, + "height": 928, + "state": { + "sidebar": { + "width": 304, + "expanded": [ + "/SmartDashboard/Turret", + "/NT/SmartDashboard/Robot/Scheduled Commands/Names", + "/NT/Robot Pose/translation", + "/SmartDashboard/EnergyLogger", + "/SmartDashboard/EnergyLogger/Supply Current Amps", + "/SmartDashboard/EnergyLogger/Energy watt hours", + "/NT/SmartDashboard/Swerve/Modules/Module 3", + "/NT/SmartDashboard/Swerve/Modules/Module 2", + "/NT/SmartDashboard/Swerve/Modules/Module 0", + "/NT/SmartDashboard/Swerve/Modules/Module 1", + "/SmartDashboard/SuperStructure", + "/NT/SmartDashboard/SuperStructure/Turret", + "/NT/SmartDashboard/SuperStructure", + "/SmartDashboard/Intake/Pivot", + "/SmartDashboard/Superstructure/Shooter", + "/SmartDashboard/Roobt", + "/SmartDashboard/Swerve/SOTM", + "/SmartDashboard/Superstructure", + "/Robot Pose", + "/Robot Pose/rotation", + "/SmartDashboard/EnergyUtil", + "/SmartDashboard/EnergyUtil/Supply Current Amps", + "/SmartDashboard/Superstructure/Hood", + "/SmartDashboard/FieldPositions", + "/FieldPositions", + "/SmartDashboard/Spindexer", + "/SmartDashboard/Spindexer/Should Stop", + "/SmartDashboard/Handoff", + "/SmartDashboard/Robot/CAN", + "/SmartDashboard/Robot/CAN/Main", + "/SmartDashboard/Swerve", + "/NT/SmartDashboard/Robot/Scheduled Commands", + "/NT/SmartDashboard/Handoff/Should Stop", + "/NT/Robot Pose/rotation", + "/NT/SmartDashboard/Robot", + "/NT/PathPlanner/currentPose", + "/NTConnection", + "/SmartDashboard/Vision", + "/SmartDashboard/Vision/limelight-back", + "/NT/limelight-left/hw", + "/NT/limelight-right/hw", + "/NT/limelight-back/hw", + "/NT/LiveWindow/.status", + "/NT/CameraPublisher/limelight-left/streams", + "/Phoenix6", + "/DSLog", + "/Log1", + "/Log1/DSLog", + "/Log1/DSLog/Status", + "/NT/SmartDashboard", + "/NT", + "/Phoenix6/TalonFX-20", + "/NT/FMSInfo", + "/NT/SmartDashboard/EnergyUtil/Supply Current Amps", + "/NT/SmartDashboard/Swerve/Modules", + "/NT/SmartDashboard/EnergyUtil", + "/SmartDashboard/Swerve/Modules", + "/SmartDashboard/Swerve/Modules/Module 0", + "/SmartDashboard/Swerve/Modules/Module 1", + "/NT/SmartDashboard/Spindexer", + "/NT/SmartDashboard/Handoff", + "/Robot/Robot/CAN/Canivore", + "/DS", + "/Robot/RadioStatus/StatusJson", + "/limelight-back/rawtargets", + "/limelight-back/rawdetections", + "/limelight-left/rawfiducials", + "/limelight-left/rawtargets", + "/limelight-back/rawfiducials", + "/Robot/Swerve/Modules", + "/Robot/Swerve/Modules/Module 0", + "/Robot/Swerve/Modules/Module 3", + "/Robot/Vision/null", + "/SmartDashboard/Intake/Pivot/Gains", + "/Robot", + "/DS/joystick0", + "/Robot/Swerve/Modules/Module 2", + "/Robot/Swerve/Modules/Module 1", + "/Robot/Swerve/Pose", + "/Robot/Vision/limelight-right/Pose MT2", + "/Robot/Vision/limelight-right/Pose MT2/translation", + "/Robot/Vision/limelight-right/Pose MT2/rotation", + "/limelight-right/rawfiducials", + "/limelight-right/hw", + "/Robot/Robot/CAN", + "/Robot/Superstructure/SOTM", + "/limelight-back/hw", + "/Robot/Vision/limelight-right", + "/Robot/Vision/limelight-right/Temp (C)", + "/Robot/Vision/limelight-left/Temp (C)", + "/Robot/Vision/limelight-back/Temp (C)", + "/SmartDashboard", + "/SmartDashboard/Robot/Scheduled Commands/Names", + "/Robot/Superstructure", + "/Robot/Superstructure/Shooter", + "/Robot/EnergyUtil", + "/Robot/EnergyUtil/Supply Current Amps", + "/Robot/EnergyUtil/Energy Watt Hours" + ] + }, + "tabs": { + "selected": 1, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "o8ft5hbh5qnj2483ziz131p432efteo7", + "renderer": "", + "controlsHeight": 0 + }, + { + "type": 2, + "title": "2D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/Robot/Swerve/Pose", + "logType": "Pose2d", + "visible": true, + "options": { + "bumpers": "" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Vision/limelight-back/Pose MT2", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ffff00" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Vision/limelight-left/Pose MT2", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff8c00" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Vision/limelight-right/Pose MT2", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff00ff" + } + }, + { + "type": "robot", + "logKey": "NT:/Robot/Swerve/Pose", + "logType": "Pose2d", + "visible": true, + "options": { + "bumpers": "#ff0000" + } + }, + { + "type": "ghost", + "logKey": "/Robot/Superstructure/SOTM/virtual pose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ff00" + } + } + ], + "field": "FRC:2026 Field", + "orientation": 0, + "size": "large" + }, + "controllerUUID": "vkwhmy5zet2ict9f3q2r29w24rov9e6u", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 7, + "title": "Video", + "controller": null, + "controllerUUID": "iq9ir2li76pjy8a8ieh11h7czu0vl598", + "renderer": null, + "controlsHeight": 85 + }, + { + "type": 1, + "title": "LL data", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-right/Temp (C)/0", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-right/Heartbeat", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-left/Temp (C)/0", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-left/Heartbeat", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-back/Heartbeat", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-back/Temp (C)/0", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-back/Time since last boot", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Vision/limelight-right/Time since last boot", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/FMSInfo/FMSControlData", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "DS:enabled", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#af2437" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "w211nur4mw66budagt98wnlzbi6tb1c9", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 5, + "title": "Console", + "controller": "console", + "controllerUUID": "xcdyalich0rba2icj86xy0lly302g17b", + "renderer": { + "highlight": false + }, + "controlsHeight": 0 + }, + { + "type": 4, + "title": "Table", + "controller": [ + "NT:/SmartDashboard/Robot/Scheduled Commands/Names", + "NT:/Robot/Leds/State" + ], + "controllerUUID": "1iy7p3lq4owbb5o2wvw4pk2dgyxvfgil", + "renderer": null, + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Drive Train Current", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 0/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 1/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 2/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 3/Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 0/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 1/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 2/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Swerve/Modules/Module 3/Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/FMSInfo/FMSControlData", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "4v4b2vbhqa8pntvp2k6vib5cleiwpdum", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 4, + "title": "Table", + "controller": [ + "NT:/SmartDashboard/Robot/Scheduled Commands/Names" + ], + "controllerUUID": "qgz1f0xamwpqf6k7kx9tad3ovosakmi9", + "renderer": null, + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Handoff", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Current RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Velocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Velocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/Robot/Handoff/Lead Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Follow Voltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Stator Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Supply Current", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Handoff/Lead Voltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Handoff/State", + "logType": "String", + "visible": true, + "options": { + "color": "#858584" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/State", + "logType": "String", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Shooter At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#d993aa" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Hood At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#2b66a2" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "3aqzg2twfsajtgvmavr5a5id4llsuy3s", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Intake", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Leader Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Current Angle (deg)", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Target Angle (deg)", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Pivot Temperature (C)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Pivot Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Pivot Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Leader Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Follower Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Leader Voltage (volts)", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Intake/Roller Follower Voltage (volts)", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot State", + "logType": "String", + "visible": true, + "options": { + "color": "#c0b487" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#858584" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Roller State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot Pushdown Voltage Applied?", + "logType": "Boolean", + "visible": false, + "options": { + "color": "#80588e" + } + }, + { + "type": "stripes", + "logKey": "NT:/FMSInfo/FMSControlData", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Pivot is below pushdown Threshold", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Hood/State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "lv0zopsqi5hzgd5qb1ogder5fkbl8lx4", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Spindexer", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Motor RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Spindexer/Leader Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/At Tolerance", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/State", + "logType": "String", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Spindexer/At Tolerance", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#80588e" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Spindexer/State", + "logType": "String", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Superstructure/State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "stripes", + "logKey": "/Robot/RadioStatus/Connected", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "/Robot/Superstructure/State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "stripes", + "logKey": "/Robot/Superstructure/Should Stop?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e5b31b" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "nzjev0kx9o0kc8x55u0zt5yd918sbr5s", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turret", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Target Angle", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Current Angle", + "logType": "Number", + "visible": false, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Current Angle", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Wrapped Target Angle (deg)", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Encoder17t Abs Position (Rot)", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Encoder18t Abs Position (Rot)", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Relative Encoder Position (deg)", + "logType": "Number", + "visible": false, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Turret/Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret/State", + "logType": "String", + "visible": true, + "options": { + "color": "#5f4528" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret At Tolerance?", + "logType": "Boolean", + "visible": false, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/Should Stop/Is Turret Wrapping?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Turret At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e48b32" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Spindexer/State", + "logType": "String", + "visible": true, + "options": { + "color": "#80588e" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "1l62ufdzjd96aeg8je5ptz4duv0yv8mb", + "renderer": null, + "controlsHeight": 202 + }, + { + "type": 1, + "title": "Shooter", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Target RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Target RPM", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Motor Temp (C)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Motor Temp (C)", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Implemented Error (RPM)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Voltage (volts)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Supply Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Follower Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/Superstructure/Shooter/Leader Stator Current (amps)", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Shooter/State", + "logType": "String", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Superstructure/Shooter At Tolerance?", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#d993aa" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Handoff/State", + "logType": "String", + "visible": true, + "options": { + "color": "#5f4528" + } + }, + { + "type": "stripes", + "logKey": "NT:/Robot/Intake/Roller State", + "logType": "String", + "visible": true, + "options": { + "color": "#2b66a2" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "taexnipg1s9d9n3iawednkasi1c36sai", + "renderer": null, + "controlsHeight": 186 + }, + { + "type": 1, + "title": "Energy data", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/CommandSwerveDrivetrain Drive", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/CommandSwerveDrivetrain Turn", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/HandoffImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/HoodImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/IntakeImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/ShooterImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/SpindexerImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Supply Current Amps/TurretImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Total Used Energy", + "logType": "Number", + "visible": true, + "options": { + "color": "#d993aa", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/CommandSwerveDrivetrain Drive", + "logType": "Number", + "visible": true, + "options": { + "color": "#5f4528", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/CommandSwerveDrivetrain Turn", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/HandoffImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/HoodImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/IntakeImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/ShooterImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/SpindexerImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Robot/EnergyUtil/Energy Watt Hours/TurretImpl", + "logType": "Number", + "visible": true, + "options": { + "color": "#858584", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "teziz9fbwjzdw99zhiquulz49m5d3pm8", + "renderer": null, + "controlsHeight": 268 + }, + { + "type": 1, + "title": "QueuedLogs", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/DogLog/QueuedLogs", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Robot/DogLog/QueueRemainingCapacity", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "gq8c8iov24h4txn3epzzwqswvay9n04f", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "26.0.0" +} diff --git a/build.gradle b/build.gradle index 057c2477..0ea9f06f 100644 --- a/build.gradle +++ b/build.gradle @@ -12,7 +12,7 @@ allprojects { // Set this to the latest version of StuyLib. // You can check here: https://github.com/StuyPulse/StuyLib/releases. -final String STUYLIB_VERSION = '2026.1.1-SNAPSHOT' +final String STUYLIB_VERSION = '61cedd1b41' def ROBOT_MAIN_CLASS = "com.stuypulse.robot.Main" diff --git a/logs.py b/logs.py new file mode 100644 index 00000000..da4fab34 --- /dev/null +++ b/logs.py @@ -0,0 +1,47 @@ +#!/usr/bin/python3 +import os +from fabric import Connection + +MAX_NUMBER_OF_LOGS_TO_PULL = 5 +TARGET_IP_ADDRESS = "10.6.94.2" +USB_DIRECTORY = "D:\\MILSTEIN_LOGS" +LOCAL_DIRECTORY_TO_SAVE_LOGS = "C:\\Users\\test\\logs\\CHAMPS_LOGS_26" +os.makedirs(LOCAL_DIRECTORY_TO_SAVE_LOGS, exist_ok=True) # if the dir does not exist, create a new one + +SAVE_DIRECTORY = USB_DIRECTORY if os.path.exists(USB_DIRECTORY) else LOCAL_DIRECTORY_TO_SAVE_LOGS + +raw_local_logs = os.listdir(path=SAVE_DIRECTORY) + +print("DIRECTORY BEING WRITTEN TO: " + SAVE_DIRECTORY) +print(f"THESE FILES WERE ALREADY FOUND FROM {SAVE_DIRECTORY}: " + str(raw_local_logs)) + +wpilogs_local = set(filter(lambda x: x.endswith(".wpilog"), raw_local_logs)) + +ssh_connection = Connection( + host=TARGET_IP_ADDRESS, + user="lvuser", + connect_kwargs={"password": ""}, + connect_timeout=4 +) + +raw_remote_logs = ssh_connection.run("ls -t /home/lvuser/logs/", hide=True) +remote_files = raw_remote_logs.stdout.strip().split() + +all_remote_wpilog_logs = [f for f in remote_files if f.endswith(".wpilog")] +wpilogs_remote_newest = all_remote_wpilog_logs[:MAX_NUMBER_OF_LOGS_TO_PULL] + +logs_to_be_downloaded = [] +for log_name in wpilogs_remote_newest: + if log_name not in wpilogs_local: + print(f"Missing log found, appending {log_name} to {SAVE_DIRECTORY}" ) + logs_to_be_downloaded.append(log_name) + ssh_connection.get(f"/home/lvuser/logs/{log_name}", local=f"{SAVE_DIRECTORY}/{log_name}") # need to include the name of the file + if (os.path.exists(SAVE_DIRECTORY)): + print(f"SUCCESS: {SAVE_DIRECTORY} found and being written to") + else: + print(f"ERROR: {SAVE_DIRECTORY} not found!") + + +#todo: add a post request to an api endpoint that allows us to automatically upload logs to a server + +print(f"Downloaded {len(logs_to_be_downloaded)} log(s): {logs_to_be_downloaded} directory: {SAVE_DIRECTORY} ") diff --git a/simgui.json b/simgui.json index 92d7c18b..c86a9772 100644 --- a/simgui.json +++ b/simgui.json @@ -108,7 +108,8 @@ "Robot": { "Auton": { "open": true - } + }, + "open": true }, "Spindexer": { "open": true diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto new file mode 100644 index 00000000..06fa4b7c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite CD.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Left Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto new file mode 100644 index 00000000..06fa4b7c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Corner Bite.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Left Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto new file mode 100644 index 00000000..7b676264 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left One Cycle.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Omit Second Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Dot to Middle" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Extras", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left Shallow One Cycle Dot.auto b/src/main/deploy/pathplanner/autos/BC Left Shallow One Cycle Dot.auto new file mode 100644 index 00000000..8d807cdf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Shallow One Cycle Dot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Left To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Corner To Dot Straight" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto new file mode 100644 index 00000000..3b482828 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Left Two Cycle.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "path", + "data": { + "pathName": "Left Dot to Middle" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Extras", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto new file mode 100644 index 00000000..e31b801b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite CD.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Right Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto new file mode 100644 index 00000000..e31b801b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Right Corner Bite.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Right Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Right Shallow One Cycle Dot.auto b/src/main/deploy/pathplanner/autos/BC Right Shallow One Cycle Dot.auto new file mode 100644 index 00000000..d0153b3a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Right Shallow One Cycle Dot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BC Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Corner To Dot Straight" + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Main", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC Slow.auto b/src/main/deploy/pathplanner/autos/BC Slow.auto new file mode 100644 index 00000000..85471c5f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC Slow.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Corner Bite Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BC to Dot test.auto b/src/main/deploy/pathplanner/autos/BC to Dot test.auto new file mode 100644 index 00000000..89695d9e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BC to Dot test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": null + } + } + ] + } + }, + "resetOdom": true, + "folder": "BC Extras", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Champs + NY.auto b/src/main/deploy/pathplanner/autos/Champs + NY.auto new file mode 100644 index 00000000..2fe69e23 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Champs + NY.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Champs Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "316 NY Right Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Champs.auto b/src/main/deploy/pathplanner/autos/Champs.auto new file mode 100644 index 00000000..7d24a972 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Champs.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Champs Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Champs Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Anti Colliding NY.auto b/src/main/deploy/pathplanner/autos/Left Anti Colliding NY.auto new file mode 100644 index 00000000..e8d8f532 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Anti Colliding NY.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Corner Bite Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score Anti Collision" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto index 55916030..6fc6340e 100644 --- a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -13,19 +13,25 @@ { "type": "path", "data": { - "pathName": "Left Corner Bite To Score" + "pathName": "Left NZ To Score" } }, { "type": "path", "data": { - "pathName": "Left Bite Score To Score" + "pathName": "Left Score To Corner" } }, { "type": "path", "data": { - "pathName": "Left Score Jiggle" + "pathName": "Left Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left NY Second Pass.auto b/src/main/deploy/pathplanner/autos/Left NY Second Pass.auto new file mode 100644 index 00000000..603fdf5c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left NY Second Pass.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Left Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto new file mode 100644 index 00000000..46ee95a1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Shallow 2 Cycle.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto index 513b4a1c..45cb3da1 100644 --- a/src/main/deploy/pathplanner/autos/Left Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto @@ -16,6 +16,12 @@ "pathName": "Left NZ To Score" } }, + { + "type": "path", + "data": { + "pathName": "Left Score To Corner" + } + }, { "type": "path", "data": { @@ -25,7 +31,7 @@ { "type": "path", "data": { - "pathName": "Left Score Jiggle" + "pathName": "Left Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/NY.auto b/src/main/deploy/pathplanner/autos/NY.auto new file mode 100644 index 00000000..a62cb56b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NY.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NY Right Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "NY Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "NY Right Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Duel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto index 9000ca89..1953c829 100644 --- a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto @@ -16,6 +16,12 @@ "pathName": "Right NZ To Score" } }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, { "type": "path", "data": { @@ -25,7 +31,7 @@ { "type": "path", "data": { - "pathName": "Right Score Jiggle" + "pathName": "Right Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto b/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto new file mode 100644 index 00000000..d249d7ee --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right NY Second Pass.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "BC Right Score To Score NY" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto new file mode 100644 index 00000000..baa1fe96 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Shallow 2 Cycle.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right To Shallow" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Shallow To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto index 78af2d36..60f97953 100644 --- a/src/main/deploy/pathplanner/autos/Right Two Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto @@ -16,6 +16,12 @@ "pathName": "Right NZ To Score" } }, + { + "type": "path", + "data": { + "pathName": "Right Score To Corner" + } + }, { "type": "path", "data": { @@ -25,7 +31,7 @@ { "type": "path", "data": { - "pathName": "Right Score Jiggle" + "pathName": "Right Score To Corner" } } ] diff --git a/src/main/deploy/pathplanner/paths/316 Champs Left Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/316 Champs Left Shallow To Score Wide.path new file mode 100644 index 00000000..c8bad09f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Left Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 5.1 + }, + "prevControl": null, + "nextControl": { + "x": 7.100149920299288, + "y": 7.758980933132961 + }, + "isLocked": false, + "linkedName": "316 Left" + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 7.092826633788956, + "y": 7.806849621436789 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 Champs Left To Shallow.path b/src/main/deploy/pathplanner/paths/316 Champs Left To Shallow.path new file mode 100644 index 00000000..e9bd8053 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 5.1 + }, + "prevControl": { + "x": 7.361981455064195, + "y": 6.8596576319543505 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "316 Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 Champs Right Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/316 Champs Right Shallow To Score Wide.path new file mode 100644 index 00000000..ae57bc5f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Right Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 2.9 + }, + "prevControl": null, + "nextControl": { + "x": 7.100149920299288, + "y": 0.2410190668670391 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 7.092826633788956, + "y": 0.19315037856321204 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 Champs Right To Shallow.path b/src/main/deploy/pathplanner/paths/316 Champs Right To Shallow.path new file mode 100644 index 00000000..a5991b1d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 Champs Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 0.325 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728943436424, + "y": 0.32297044805622777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.569, + "y": 2.9 + }, + "prevControl": { + "x": 7.36197644313434, + "y": 1.1403429576916921 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "316 Right" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path b/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path new file mode 100644 index 00000000..48860d93 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 NY Left Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.445245848532716, + "y": 7.510077505314976 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 5.549 + }, + "prevControl": { + "x": 6.024782241558084, + "y": 7.541389396183491 + }, + "nextControl": { + "x": 5.7807461618646405, + "y": 4.752044241526603 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.353, + "y": 5.349 + }, + "prevControl": { + "x": 7.273386284835411, + "y": 4.452528217757139 + }, + "nextControl": { + "x": 7.575122265309203, + "y": 7.850156272457583 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 7.67300628501201, + "y": 7.479062563248229 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.65, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.6, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path b/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path new file mode 100644 index 00000000..0405eab1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/316 NY Right Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.445245848532716, + "y": 0.48992249468502347 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 2.451 + }, + "prevControl": { + "x": 6.024782241558084, + "y": 0.4586106038165092 + }, + "nextControl": { + "x": 5.7807461618646405, + "y": 3.247955758473397 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.353, + "y": 2.651 + }, + "prevControl": { + "x": 7.273381801565748, + "y": 3.547471384082104 + }, + "nextControl": { + "x": 7.575134773631564, + "y": 0.14984483841092855 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 7.673006390654894, + "y": 0.520948709769672 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.65, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.6, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score LONG.path b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score LONG.path new file mode 100644 index 00000000..915897b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score LONG.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.57, + "y": 7.42 + }, + "prevControl": null, + "nextControl": { + "x": 7.8268188302425035, + "y": 7.488123468654376 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.7265763195435095, + "y": 5.904636233951498 + }, + "prevControl": { + "x": 7.687760342368046, + "y": 7.884251069900142 + }, + "nextControl": { + "x": 7.76360928403665, + "y": 4.015955044801327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.794992867332382, + "y": 3.782696148359487 + }, + "prevControl": { + "x": 7.6166741233975355, + "y": 3.782696148359487 + }, + "nextControl": { + "x": 5.966918687589157, + "y": 3.782696148359487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 6.059900142653353 + }, + "prevControl": { + "x": 6.143981742457801, + "y": 4.624070860008561 + }, + "nextControl": { + "x": 6.070427960057062, + "y": 7.664293865905849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.651, + "y": 7.441 + }, + "prevControl": { + "x": 6.341589561259271, + "y": 7.591629770992365 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.307036247334758, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8272921108741973, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.2366737739872133, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.7421203438395327, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.6183368869935886, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0533049040511733, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.3176972281449895, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8835202761000936, + "maxWaypointRelativePos": 3.057808455565133, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path new file mode 100644 index 00000000..d5713997 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.57, + "y": 7.42 + }, + "prevControl": null, + "nextControl": { + "x": 5.483737517831668, + "y": 7.453938659058489 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.04, + "y": 6.999 + }, + "prevControl": { + "x": 8.017259660107046, + "y": 7.684860122523228 + }, + "nextControl": { + "x": 8.117647798240464, + "y": 4.6571032356791555 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302, + "y": 4.075 + }, + "prevControl": { + "x": 8.414514968383727, + "y": 4.053397767604201 + }, + "nextControl": { + "x": 5.969318116975748, + "y": 4.100877318116975 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.332, + "y": 5.912 + }, + "prevControl": { + "x": 6.310128594617119, + "y": 4.825295699627398 + }, + "nextControl": { + "x": 6.370800820617092, + "y": 7.839860504820692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 7.441 + }, + "prevControl": { + "x": 6.148059987278764, + "y": 7.453924368494453 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.58, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.06, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.38, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.98, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.08, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.44, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path b/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path new file mode 100644 index 00000000..e7041c51 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 7.605 + }, + "prevControl": null, + "nextControl": { + "x": 7.584251069900143, + "y": 7.693557554093935 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.909455555555555, + "y": 5.61 + }, + "prevControl": { + "x": 7.9484375712176165, + "y": 6.915755429044664 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17621776504297812, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.41791044776119385, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5077650236326793, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.2, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path new file mode 100644 index 00000000..663980ce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left NZ To Score.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.909455555555555, + "y": 5.61 + }, + "prevControl": null, + "nextControl": { + "x": 6.857055555555556, + "y": 5.574622222222222 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.419771754636234, + "y": 6.507 + }, + "prevControl": { + "x": 6.428299999999999, + "y": 6.091077777777778 + }, + "nextControl": { + "x": 6.3979760418229406, + "y": 7.569976136537918 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.65, + "y": 7.441 + }, + "prevControl": { + "x": 5.959506149944524, + "y": 7.416322222222221 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 1.2, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path new file mode 100644 index 00000000..f4d10dba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Score To Score NY.path @@ -0,0 +1,167 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.486898777611385, + "y": 7.524587798893349 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.163, + "y": 5.645 + }, + "prevControl": { + "x": 6.488590333125494, + "y": 7.49151453689789 + }, + "nextControl": { + "x": 6.001159898414421, + "y": 4.7271591741926215 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.637, + "y": 5.672 + }, + "prevControl": { + "x": 7.637, + "y": 4.771999999999999 + }, + "nextControl": { + "x": 7.637, + "y": 5.922 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.067, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 8.01667550487931, + "y": 7.416078370774993 + }, + "nextControl": { + "x": 5.087385164051356, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.294108416547789, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.34587995930824006, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.05, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 0.7, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.2, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.67, + "maxWaypointRelativePos": 1.92, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Score To Score.path b/src/main/deploy/pathplanner/paths/BC Left Score To Score.path new file mode 100644 index 00000000..63f3db33 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Score To Score.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.57, + "y": 7.42 + }, + "prevControl": null, + "nextControl": { + "x": 6.983047455470738, + "y": 7.572326844783714 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.131, + "y": 5.399222222222222 + }, + "prevControl": { + "x": 6.150488888888891, + "y": 7.640444444444444 + }, + "nextControl": { + "x": 6.122892552012039, + "y": 4.466865703606724 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.408, + "y": 4.9314888888888895 + }, + "prevControl": { + "x": 7.037694923137463, + "y": 4.655234747475271 + }, + "nextControl": { + "x": 8.049714513916308, + "y": 5.410219274052943 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.267, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 7.564768556045968, + "y": 7.433121689698743 + }, + "nextControl": { + "x": 5.287385164051356, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6506870229007635, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.306835382387213, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.882729211087425, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/BC Left Shallow To Score.path new file mode 100644 index 00000000..0593e3eb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.791269614835947, + "y": 5.174 + }, + "prevControl": null, + "nextControl": { + "x": 6.45637470114939, + "y": 7.949690106876804 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6506870229007635, + "y": 7.441 + }, + "prevControl": { + "x": 6.09859528645011, + "y": 7.650208015267176 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3816631130063977, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Left To Shallow.path b/src/main/deploy/pathplanner/paths/BC Left To Shallow.path new file mode 100644 index 00000000..d1fdb1ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.63948673912623, + "y": 7.707882561953116 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.791269614835947, + "y": 5.174 + }, + "prevControl": { + "x": 7.584251069900143, + "y": 6.933657631954351 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path new file mode 100644 index 00000000..8e70455a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.53, + "y": 0.57 + }, + "prevControl": null, + "nextControl": { + "x": 5.483737517831668, + "y": 0.5829386590584889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.04, + "y": 1.0008844507845935 + }, + "prevControl": { + "x": 8.017264300231318, + "y": 0.31502417442937847 + }, + "nextControl": { + "x": 8.117631954350927, + "y": 3.3427817403708993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302, + "y": 3.925021398002854 + }, + "prevControl": { + "x": 8.414514968383727, + "y": 3.9034191656070543 + }, + "nextControl": { + "x": 5.969318116975748, + "y": 3.9508987161198283 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.332, + "y": 2.0877318116975756 + }, + "prevControl": { + "x": 6.310120051106661, + "y": 3.174435940086786 + }, + "nextControl": { + "x": 6.370815977175208, + "y": 0.15987161198288025 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 6.1480599144079875, + "y": 0.5460613409415132 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.58, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.06, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.38, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.98, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.08, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.44, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path new file mode 100644 index 00000000..980515ba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 7.584251069900143, + "y": 0.48333808844507875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.909455555555555, + "y": 2.39 + }, + "prevControl": { + "x": 7.948433333333332, + "y": 1.0842444444444457 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17621776504297812, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.41791044776119385, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5077650236326793, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.2, + "rotation": 90.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path new file mode 100644 index 00000000..71b53033 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right NZ To Score.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.909455555555555, + "y": 2.39 + }, + "prevControl": null, + "nextControl": { + "x": 6.701144444444445, + "y": 2.3687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.419771754636234, + "y": 1.4925534950071324 + }, + "prevControl": { + "x": 6.433111579891319, + "y": 1.793603073252706 + }, + "nextControl": { + "x": 6.379577777777778, + "y": 0.5854666666666649 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.028695038833414, + "y": 0.5367444444444436 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 1.2, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path new file mode 100644 index 00000000..758c9fd0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score NY.path @@ -0,0 +1,167 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.46089863956213, + "y": 0.4753134455838692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.163, + "y": 2.355 + }, + "prevControl": { + "x": 6.488590333125495, + "y": 0.5084854631021092 + }, + "nextControl": { + "x": 6.001159898414421, + "y": 3.272840825807378 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.637, + "y": 2.328 + }, + "prevControl": { + "x": 7.637, + "y": 3.228 + }, + "nextControl": { + "x": 7.637, + "y": 2.078 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.067, + "y": 0.559 + }, + "prevControl": { + "x": 8.016675326208684, + "y": 0.583834950985082 + }, + "nextControl": { + "x": 5.087385536493331, + "y": 0.5072311198219507 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 5.268148066661913, + "y": 0.6003141499164485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.34587995930824006, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.05, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.67, + "maxWaypointRelativePos": 1.92, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 0.7, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.25, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Score To Score.path b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path new file mode 100644 index 00000000..7e0dc8a9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.53, + "y": 0.57 + }, + "prevControl": null, + "nextControl": { + "x": 7.133082715798332, + "y": 0.5201980499756428 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.05, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 6.05, + "y": 0.3280741797432247 + }, + "nextControl": { + "x": 6.05, + "y": 4.150659142168011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.503, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 7.415934887606322, + "y": 3.83143356936277 + }, + "nextControl": { + "x": 7.725175792507205, + "y": 0.34949567723342856 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 7.673089129599257, + "y": 0.5209487097696721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.1812366737739914, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.4179104477611943, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/BC Right Shallow To Score.path new file mode 100644 index 00000000..6150939e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.868901569186875, + "y": 2.935 + }, + "prevControl": null, + "nextControl": { + "x": 6.404423364113454, + "y": 0.20272922519291026 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 6.018673323823109, + "y": 0.36492011412268166 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BC Right To Shallow.path b/src/main/deploy/pathplanner/paths/BC Right To Shallow.path new file mode 100644 index 00000000..859548fe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BC Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 6.938723126508627, + "y": 0.3260177844037628 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.868901569186875, + "y": 2.935 + }, + "prevControl": { + "x": 7.902374851629624, + "y": 1.7048568702290083 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.20916905444126394, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5200573065902583, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7736389684813755, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6143226919758464, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Champs Left Bite Score To Score.path new file mode 100644 index 00000000..f65f9f2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3075858778625955, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 7.564404708105101, + "y": 7.509029957203994 + }, + "isLocked": false, + "linkedName": "Left Corner" + }, + { + "anchor": { + "x": 7.7265763195435095, + "y": 5.904636233951498 + }, + "prevControl": { + "x": 7.687760342368046, + "y": 7.884251069900142 + }, + "nextControl": { + "x": 7.76360928403665, + "y": 4.015955044801327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.794992867332382, + "y": 3.782696148359487 + }, + "prevControl": { + "x": 7.6166741233975355, + "y": 3.782696148359487 + }, + "nextControl": { + "x": 5.966918687589157, + "y": 3.782696148359487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 6.059900142653353 + }, + "prevControl": { + "x": 6.143981742457801, + "y": 4.624070860008561 + }, + "nextControl": { + "x": 6.070427960057062, + "y": 7.664293865905849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.328549618320611, + "y": 7.591536259541984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.307036247334758, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8272921108741973, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.2366737739872133, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.7421203438395327, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.6183368869935886, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0533049040511733, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.3176972281449895, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8835202761000936, + "maxWaypointRelativePos": 3.057808455565133, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Champs Left Score To Corner.path new file mode 100644 index 00000000..082d1fcb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 3.2238741384420084, + "y": 7.349786421407005 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 3.3075858778625955, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.553822420306369, + "y": 7.484121823389645 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path new file mode 100644 index 00000000..b34d5a08 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 5.265 + }, + "prevControl": null, + "nextControl": { + "x": 7.117514738065982, + "y": 7.8255001578317405 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 7.092826633788956, + "y": 7.806849621436789 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score.path new file mode 100644 index 00000000..3a39c558 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 5.374151212553495 + }, + "prevControl": null, + "nextControl": { + "x": 6.235545760690833, + "y": 7.811409368840588 + }, + "isLocked": false, + "linkedName": "Left Shallow" + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.085868320610687, + "y": 7.650114503816795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3816631130063977, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Left To Shallow.path b/src/main/deploy/pathplanner/paths/Champs Left To Shallow.path new file mode 100644 index 00000000..53fa076b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 5.374151212553495 + }, + "prevControl": { + "x": 7.361981455064195, + "y": 7.133808844507846 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Shallow" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Champs Right Bite Score To Score.path new file mode 100644 index 00000000..238c139c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right Bite Score To Score.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 5.2357375178316685, + "y": 0.5719386590584888 + }, + "isLocked": false, + "linkedName": "Right Corner" + }, + { + "anchor": { + "x": 7.739514978601996, + "y": 1.0008844507845935 + }, + "prevControl": { + "x": 7.7167792788333145, + "y": 0.31502417442937847 + }, + "nextControl": { + "x": 7.817146932952923, + "y": 3.3427817403708993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.002011412268189, + "y": 3.925021398002854 + }, + "prevControl": { + "x": 8.114526380651917, + "y": 3.9034191656070543 + }, + "nextControl": { + "x": 5.669329529243937, + "y": 3.9508987161198283 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.031611982881596, + "y": 2.0877318116975756 + }, + "prevControl": { + "x": 6.009732033987853, + "y": 3.174435940086786 + }, + "nextControl": { + "x": 6.070427960057061, + "y": 0.15987161198288047 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.147977175463622, + "y": 0.5460613409415126 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5714285714285716, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.9722814498933815, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3475479744136523, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.936034115138597, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.4818763326225852, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0618336886993425, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.4968017057569094, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Champs Right Score To Corner.path new file mode 100644 index 00000000..adfdfdba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 3.199971130397284, + "y": 0.6068359978960554 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": { + "x": 3.5164317899184905, + "y": 0.4721568317274595 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score Wide.path b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score Wide.path new file mode 100644 index 00000000..b854b584 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score Wide.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 2.7346647646219684 + }, + "prevControl": null, + "nextControl": { + "x": 6.713949641685828, + "y": 0.3854332126571971 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.721732371225743, + "y": 0.396025832259473 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score.path new file mode 100644 index 00000000..c8212757 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 2.7346647646219684 + }, + "prevControl": null, + "nextControl": { + "x": 6.1198701854493605, + "y": 0.34101283880171307 + }, + "isLocked": false, + "linkedName": "Right Shallow NZ" + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.018590584878744, + "y": 0.3649201141226812 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Champs Right To Shallow.path b/src/main/deploy/pathplanner/paths/Champs Right To Shallow.path new file mode 100644 index 00000000..9719aeb3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Champs Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 6.939437022900763, + "y": 0.36130725190839597 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 2.7346647646219684 + }, + "prevControl": { + "x": 7.602473282442749, + "y": 1.5045216348509767 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Shallow NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.20916905444126394, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5200573065902583, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7736389684813755, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6143226919758464, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of BC Right Score To Score NY.path b/src/main/deploy/pathplanner/paths/Copy of BC Right Score To Score NY.path new file mode 100644 index 00000000..0f6099e8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of BC Right Score To Score NY.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.46089863956213, + "y": 0.4753134455838692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.163, + "y": 2.355 + }, + "prevControl": { + "x": 6.488590333125495, + "y": 0.5084854631021092 + }, + "nextControl": { + "x": 6.001159898414421, + "y": 3.272840825807378 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.637, + "y": 2.328 + }, + "prevControl": { + "x": 7.637, + "y": 3.228 + }, + "nextControl": { + "x": 7.637, + "y": 2.078 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.067, + "y": 0.559 + }, + "prevControl": { + "x": 8.016675326208684, + "y": 0.583834950985082 + }, + "nextControl": { + "x": 5.087385536493331, + "y": 0.5072311198219507 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 5.268148066661913, + "y": 0.6003141499164485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.34587995930824006, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.05, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC modified", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path index 0a057cd8..06ea202b 100644 --- a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.63796005706134, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 7.894778887303849, + "x": 7.5644047081051005, "y": 7.509029957203994 }, "isLocked": false, - "linkedName": "Left Trench Score" + "linkedName": "Left Corner" }, { "anchor": { @@ -68,8 +68,8 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 6.0057346647646215, - "y": 7.664293865905849 + "x": 6.328549618320611, + "y": 7.591536259541984 }, "nextControl": null, "isLocked": false, @@ -90,7 +90,7 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 1.9104477611940158, + "waypointRelativePos": 1.7421203438395327, "rotationDegrees": 180.0 }, { @@ -106,7 +106,21 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8835202761000936, + "maxWaypointRelativePos": 3.057808455565133, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path new file mode 100644 index 00000000..5494bd8c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite Anti Collision.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.764, + "y": 4.978 + }, + "prevControl": { + "x": 7.267033333333333, + "y": 7.940311111111111 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "L-Anti-Collision" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9402985074626858, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6212018906144481, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Anti Collision", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path index 5e2c9e3f..55b7a9a4 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.231184022824536, - "y": 5.257703281027104 + "x": 8.295104961832061, + "y": 4.65425572519084 }, "prevControl": null, "nextControl": { - "x": 7.403109843081312, - "y": 4.339058487874465 + "x": 7.4670307820888375, + "y": 3.7356109320382007 }, "isLocked": false, "linkedName": "Left NZ Corner" diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path index 9ec5ab2a..2f188ae5 100644 --- a/src/main/deploy/pathplanner/paths/Left Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 8.231184022824536, - "y": 5.257703281027104 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 6.782054208273893, - "y": 7.276134094151212 + "x": 7.751514946619217, + "y": 7.583687149070779 }, "nextControl": null, "isLocked": false, - "linkedName": "Left NZ Corner" + "linkedName": "Left NZ" } ], "rotationTargets": [ @@ -38,8 +38,8 @@ "rotationDegrees": -55.0 }, { - "waypointRelativePos": 0.9253731343283487, - "rotationDegrees": -55.0 + "waypointRelativePos": 0.9402985074626858, + "rotationDegrees": -90.0 } ], "constraintZones": [ @@ -68,7 +68,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": -90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Center Dot.path b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot.path new file mode 100644 index 00000000..12fedec1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Center Dot.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.282, + "y": 7.441 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.838, + "y": 7.179 + }, + "prevControl": { + "x": 6.7323454345648255, + "y": 7.405576946759163 + }, + "nextControl": { + "x": 7.209490856875041, + "y": 6.382335286525449 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.289, + "y": 4.064233333333333 + }, + "prevControl": { + "x": 8.164, + "y": 4.2807396842794425 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot Straight.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot Straight.path new file mode 100644 index 00000000..037feffc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Dot Straight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.31, + "y": 7.44 + }, + "prevControl": null, + "nextControl": { + "x": 4.835636776657973, + "y": 7.44 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 7.931192525525695, + "y": 7.441 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Dot Turn.path b/src/main/deploy/pathplanner/paths/Left Corner To Dot Turn.path new file mode 100644 index 00000000..9b02e7c4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Dot Turn.path @@ -0,0 +1,97 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.911440798858774, + "y": 8.117146932952926 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 6.072838801711842 + }, + "prevControl": { + "x": 6.86861310177917, + "y": 5.172517037410415 + }, + "nextControl": { + "x": 8.632282453637659, + "y": 6.31867332382311 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": { + "x": 8.254, + "y": 7.190999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3837953091684457, + "rotationDegrees": -35.0 + }, + { + "waypointRelativePos": 0.6652452025586366, + "rotationDegrees": -33.0 + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Dot to Middle.path b/src/main/deploy/pathplanner/paths/Left Dot to Middle.path new file mode 100644 index 00000000..c3f271d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Dot to Middle.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.254, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.468955322425778, + "y": 7.441 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 4.07556350626118 + }, + "prevControl": { + "x": 4.535578251580112, + "y": 3.6328315855227107 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2994652406417112, + "rotationDegrees": -117.87026328105652 + }, + { + "waypointRelativePos": 0.6925133689839572, + "rotationDegrees": -51.403799535528066 + }, + { + "waypointRelativePos": 0.9147121535181245, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.845374746792703, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC paths", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Follow To Score.path b/src/main/deploy/pathplanner/paths/Left Follow To Score.path index 5d36788c..36f13005 100644 --- a/src/main/deploy/pathplanner/paths/Left Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Score.path @@ -48,7 +48,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8317617866005099, + "minWaypointRelativePos": 0.8662640207075034, "maxWaypointRelativePos": 2.0, "constraints": { "maxVelocity": 1.75, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path new file mode 100644 index 00000000..703a4b27 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score Anti Collision.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.764, + "y": 4.978 + }, + "prevControl": null, + "nextControl": { + "x": 6.117188888888889, + "y": 4.929277777777779 + }, + "isLocked": false, + "linkedName": "L-Anti-Collision" + }, + { + "anchor": { + "x": 6.510342368045648, + "y": 6.383 + }, + "prevControl": { + "x": 6.4994078379642675, + "y": 5.628079185575136 + }, + "nextControl": { + "x": 6.530430041188528, + "y": 7.769854529281173 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.471645180674328, + "y": 7.4831512540767555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.3432835820895521, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Anti Collision", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 88a55f71..2415dfc2 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 5.7738671411625155, - "y": 4.589098457888493 + "x": 6.601670502174773, + "y": 4.572653815737446 }, "isLocked": false, "linkedName": "Left NZ" }, { "anchor": { - "x": 6.052395038167939, - "y": 6.378129770992366 + "x": 6.510342368045648, + "y": 6.38336661911555 }, "prevControl": { - "x": 6.055976129616249, - "y": 5.761441920527078 + "x": 6.496511111111111, + "y": 5.428455555555555 }, "nextControl": { - "x": 6.044550641940085, - "y": 7.7289871611982885 + "x": 6.530424413276881, + "y": 7.769832596935215 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 6.01867332382311, - "y": 7.444336661911555 + "x": 6.4717380840892496, + "y": 7.483152639087019 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score Jiggle.path b/src/main/deploy/pathplanner/paths/Left Score Jiggle.path deleted file mode 100644 index 75ea4914..00000000 --- a/src/main/deploy/pathplanner/paths/Left Score Jiggle.path +++ /dev/null @@ -1,134 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": null, - "nextControl": { - "x": 3.3895155587886268, - "y": 7.413061717442285 - }, - "isLocked": false, - "linkedName": "Left Trench Score" - }, - { - "anchor": { - "x": 3.648639663594734, - "y": 7.442254096935589 - }, - "prevControl": { - "x": 3.400211195655958, - "y": 7.4142666655137415 - }, - "nextControl": { - "x": 3.89706813153351, - "y": 7.470241528357436 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.648639663594734, - "y": 7.458193687201929 - }, - "prevControl": { - "x": 3.8970801117553107, - "y": 7.486074571652958 - }, - "nextControl": { - "x": 3.400199215434157, - "y": 7.430312802750899 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.648639663594734, - "y": 7.458193687201929 - }, - "prevControl": { - "x": 3.894935520576916, - "y": 7.4153176935293486 - }, - "nextControl": { - "x": 3.4023438066125493, - "y": 7.501069680874509 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.87561279347943, - "y": 7.5185027311970165 - }, - "nextControl": { - "x": 3.400307320643253, - "y": 7.3633102459022215 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.63796005706134, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.391228490406765, - "y": 7.40061338720238 - }, - "nextControl": { - "x": 3.884691623715916, - "y": 7.481199589896857 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2808444444444445, - "y": 7.440906488549619 - }, - "prevControl": { - "x": 3.530395607526691, - "y": 7.455880365946537 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Corner" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Score To Corner.path index 0be2c04f..082d1fcb 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Corner.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.2245896483318734, - "y": 7.343909351042221 + "x": 3.2238741384420084, + "y": 7.349786421407005 }, "isLocked": false, "linkedName": "Left Trench Score" }, { "anchor": { - "x": 3.2808444444444445, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": { - "x": 3.524888980928413, - "y": 7.49514913056054 + "x": 3.553822420306369, + "y": 7.484121823389645 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index 4bd6d40f..b8c3689a 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.63796005706134, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.846747503566332, - "y": 7.440906488549619 + "x": 6.889227099236641, + "y": 7.524589694656489 }, "isLocked": false, - "linkedName": "Left Trench Score" + "linkedName": "Left Corner" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score NY.path b/src/main/deploy/pathplanner/paths/Left Score To Score NY.path new file mode 100644 index 00000000..8a3f8436 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score To Score NY.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.717360912981454, + "y": 7.521968616262482 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.863409415121255, + "y": 5.244764621968616 + }, + "prevControl": { + "x": 5.962233970579355, + "y": 7.616553952963002 + }, + "nextControl": { + "x": 5.824593437945791, + "y": 4.31318116975749 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.937318116975749, + "y": 4.571954350927247 + }, + "prevControl": { + "x": 6.567013040113212, + "y": 4.295700209513629 + }, + "nextControl": { + "x": 7.5790326308920575, + "y": 5.0506847360913 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0667047075606275, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 7.3644732636065955, + "y": 7.433121689698743 + }, + "nextControl": { + "x": 5.087089871611983, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.294108416547789, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.2, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 7e3d5fa7..347260f5 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.63796005706134, + "x": 3.3075858778625955, "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.717360912981455, - "y": 7.521968616262482 + "x": 6.720633333333333, + "y": 7.572233333333333 }, "isLocked": false, - "linkedName": "Left Trench Score" + "linkedName": "Left Corner" }, { "anchor": { - "x": 5.863409415121255, - "y": 5.244764621968616 + "x": 5.931333333333333, + "y": 5.399222222222222 }, "prevControl": { - "x": 5.962233970579355, - "y": 7.616553952963002 + "x": 5.950822222222223, + "y": 7.640444444444444 }, "nextControl": { - "x": 5.824593437945791, - "y": 4.31318116975749 + "x": 5.923225885345372, + "y": 4.466865703606724 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.937318116975749, - "y": 4.571954350927247 + "x": 7.207855555555556, + "y": 4.9314888888888895 }, "prevControl": { - "x": 6.567013040113212, - "y": 4.295700209513629 + "x": 6.837550478693018, + "y": 4.655234747475271 }, "nextControl": { - "x": 7.5790326308920575, - "y": 5.0506847360913 + "x": 7.849570069471864, + "y": 5.410219274052943 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Left Shallow To Score.path b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path new file mode 100644 index 00000000..3a39c558 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 5.374151212553495 + }, + "prevControl": null, + "nextControl": { + "x": 6.235545760690833, + "y": 7.811409368840588 + }, + "isLocked": false, + "linkedName": "Left Shallow" + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.085868320610687, + "y": 7.650114503816795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3816631130063977, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left To Shallow.path b/src/main/deploy/pathplanner/paths/Left To Shallow.path new file mode 100644 index 00000000..53fa076b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 5.374151212553495 + }, + "prevControl": { + "x": 7.361981455064195, + "y": 7.133808844507846 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Shallow" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6867989646246767, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Left NZ To Score.path b/src/main/deploy/pathplanner/paths/NY Left NZ To Score.path new file mode 100644 index 00000000..52c62217 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left NZ To Score.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.248481613285884, + "y": 4.621376037959667 + }, + "prevControl": null, + "nextControl": { + "x": 5.7738671411625155, + "y": 4.589098457888493 + }, + "isLocked": false, + "linkedName": "Left NZ" + }, + { + "anchor": { + "x": 6.052395038167939, + "y": 6.378129770992366 + }, + "prevControl": { + "x": 6.055976129616249, + "y": 5.761441920527078 + }, + "nextControl": { + "x": 6.044550641940085, + "y": 7.7289871611982885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.018673323823109, + "y": 7.444336661911555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.3432835820895521, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/NY Left Score To NZ (F).path new file mode 100644 index 00000000..4bd6d40f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left Score To NZ (F).path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.846747503566332, + "y": 7.440906488549619 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 8.248481613285884, + "y": 4.621376037959667 + }, + "prevControl": { + "x": 7.860321841531247, + "y": 7.196169190598754 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1902339776195354, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Left Score To Score.path b/src/main/deploy/pathplanner/paths/NY Left Score To Score.path new file mode 100644 index 00000000..8a3f8436 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left Score To Score.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 6.717360912981454, + "y": 7.521968616262482 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.863409415121255, + "y": 5.244764621968616 + }, + "prevControl": { + "x": 5.962233970579355, + "y": 7.616553952963002 + }, + "nextControl": { + "x": 5.824593437945791, + "y": 4.31318116975749 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.937318116975749, + "y": 4.571954350927247 + }, + "prevControl": { + "x": 6.567013040113212, + "y": 4.295700209513629 + }, + "nextControl": { + "x": 7.5790326308920575, + "y": 5.0506847360913 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0667047075606275, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 7.3644732636065955, + "y": 7.433121689698743 + }, + "nextControl": { + "x": 5.087089871611983, + "y": 7.49266112478357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 5.294108416547789, + "y": 7.399604078672524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.2, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/NY Left Trench To NZ.path new file mode 100644 index 00000000..3568773d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Left Trench To NZ.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 8.517461447212337, + "y": 7.70926453143535 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 8.248481613285884, + "y": 4.621376037959667 + }, + "prevControl": { + "x": 8.196726977051933, + "y": 7.442003712710024 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.140401146131807, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.55, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Right NZ To Score.path b/src/main/deploy/pathplanner/paths/NY Right NZ To Score.path new file mode 100644 index 00000000..74fbafa9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Right NZ To Score.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.237722419928826, + "y": 3.4271055753262156 + }, + "prevControl": null, + "nextControl": { + "x": 5.763107947805457, + "y": 3.5131791221826814 + }, + "isLocked": false, + "linkedName": "Right NZ" + }, + { + "anchor": { + "x": 6.053851272542522, + "y": 1.6151808747904899 + }, + "prevControl": { + "x": 6.040480921648686, + "y": 2.6846376869648685 + }, + "nextControl": { + "x": 6.070427960057061, + "y": 0.289258202567761 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.559 + }, + "prevControl": { + "x": 6.290385164051354, + "y": 0.5848773181169763 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/NY Right Score To NZ (F).path new file mode 100644 index 00000000..82c6f9a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Right Score To NZ (F).path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 7.183069900142653, + "y": 0.5460613409415126 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 8.237722419928826, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 8.185967783694872, + "y": 0.697048513985274 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.13936927772126403, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Right Score To Score.path b/src/main/deploy/pathplanner/paths/NY Right Score To Score.path new file mode 100644 index 00000000..d847f06a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Right Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.244952924393722, + "y": 0.6236932952924387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 0.3280741797432247 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.150659142168011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302881844380403, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 7.215816731986725, + "y": 3.83143356936277 + }, + "nextControl": { + "x": 7.525057636887608, + "y": 0.34949567723342856 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 7.673006390654899, + "y": 0.520948709769672 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.6703967446591785, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NY Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/NY Right Trench To NZ.path new file mode 100644 index 00000000..13b73e23 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NY Right Trench To NZ.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 9.06721902017291, + "y": 0.41484149855907726 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 8.237722419928826, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 8.14623827007292, + "y": 0.4342669586115182 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.12255772646536249, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.55, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OLD 316 NY Left Score To Score.path b/src/main/deploy/pathplanner/paths/OLD 316 NY Left Score To Score.path new file mode 100644 index 00000000..9377a06e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OLD 316 NY Left Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 6.2449526994735765, + "y": 7.376296404185324 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 5.549 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 8.072000000000001 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.7490000000000006 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.353, + "y": 5.349 + }, + "prevControl": { + "x": 7.273386284835411, + "y": 4.452528217757139 + }, + "nextControl": { + "x": 7.575122265309203, + "y": 7.850156272457584 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 7.441 + }, + "prevControl": { + "x": 7.673006285012011, + "y": 7.479062563248229 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.65, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.6, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Omit Second Shot.path b/src/main/deploy/pathplanner/paths/Omit Second Shot.path new file mode 100644 index 00000000..60be0894 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Omit Second Shot.path @@ -0,0 +1,137 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3075858778625955, + "y": 7.441 + }, + "prevControl": null, + "nextControl": { + "x": 7.2996636803917525, + "y": 7.441 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.006, + "y": 5.154194008559203 + }, + "prevControl": { + "x": 6.006, + "y": 7.340827389443653 + }, + "nextControl": { + "x": 6.006, + "y": 4.22180224083617 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.207855555555556, + "y": 4.9314888888888895 + }, + "prevControl": { + "x": 6.941432423475657, + "y": 4.554048939637246 + }, + "nextControl": { + "x": 7.428987161198288, + "y": 5.244764621968616 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.946533523537802, + "y": 6.460998573466477 + }, + "prevControl": { + "x": 7.899066186613268, + "y": 6.028698234305561 + }, + "nextControl": { + "x": 7.994000860462336, + "y": 6.893298912627393 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.2, + "y": 7.441 + }, + "prevControl": { + "x": 8.066490727532095, + "y": 7.198595651250666 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3601659751037194, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.658921161825727, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.219851116625306, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path index 2d259744..d7526e8b 100644 --- a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 + "x": 3.282, + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 5.565820256776034, - "y": 0.5997860199714706 + "x": 5.2357375178316685, + "y": 0.571938659058489 }, "isLocked": false, - "linkedName": "Right Jiggle" + "linkedName": "Right Corner" }, { "anchor": { @@ -57,19 +57,19 @@ }, "nextControl": { "x": 6.070427960057061, - "y": 0.15987161198288136 + "y": 0.15987161198288025 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 + "x": 3.612, + "y": 0.559 }, "prevControl": { - "x": 6.1480599144079875, - "y": 0.5739087018544944 + "x": 6.147977175463622, + "y": 0.5460613409415132 }, "nextControl": null, "isLocked": false, @@ -106,7 +106,21 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0353753235547816, + "maxWaypointRelativePos": 3.1406384814495194, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -114,7 +128,7 @@ "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path new file mode 100644 index 00000000..9e9109a1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite Anti Collision.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 7.584251069900143, + "y": 0.48333808844507875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.631, + "y": 3.16 + }, + "prevControl": { + "x": 7.669977777777778, + "y": 1.8542444444444457 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "R-Anti-Collision" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17621776504297812, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.41791044776119385, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5077650236326793, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Anti Collision", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path index b8e70915..c6fec780 100644 --- a/src/main/deploy/pathplanner/paths/Right Corner Bite.path +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.0667047075606275, - "y": 0.4315834522111266 + "x": 7.584251069900143, + "y": 0.48333808844507875 }, "isLocked": false, "linkedName": "Right Trench Start" @@ -20,8 +20,8 @@ "y": 3.4271055753262156 }, "prevControl": { - "x": 7.687760342368046, - "y": 2.1265477888730384 + "x": 8.276700197706603, + "y": 2.121350019770661 }, "nextControl": null, "isLocked": false, @@ -30,16 +30,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2665245202558647, + "waypointRelativePos": 0.17621776504297812, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.41791044776119385, "rotationDegrees": 55.0 }, { - "waypointRelativePos": 0.8102345415778156, - "rotationDegrees": 55.0 + "waypointRelativePos": 0.7782515991471214, + "rotationDegrees": 90.0 } ], "constraintZones": [ diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path new file mode 100644 index 00000000..a68ee711 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Center Dot.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.282, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.282, + "y": 0.559 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.837566666666667, + "y": 0.8193333333333328 + }, + "prevControl": { + "x": 6.731912101231492, + "y": 0.5927563865741706 + }, + "nextControl": { + "x": 7.209057523541707, + "y": 1.6159980468078845 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.289, + "y": 4.064233333333333 + }, + "prevControl": { + "x": 8.164, + "y": 3.8477269823872233 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot Straight.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot Straight.path new file mode 100644 index 00000000..2a3aea96 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot Straight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.28, + "y": 0.56 + }, + "prevControl": null, + "nextControl": { + "x": 4.440424253599775, + "y": 0.56 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.559 + }, + "prevControl": { + "x": 8.003999999999998, + "y": 0.559 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot Turn.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot Turn.path new file mode 100644 index 00000000..cbfe0b65 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot Turn.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.28, + "y": 0.56 + }, + "prevControl": null, + "nextControl": { + "x": 4.440424253599775, + "y": 0.56 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.559 + }, + "prevControl": { + "x": 8.003999999999998, + "y": 0.559 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.58, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner To Dot.path b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path new file mode 100644 index 00000000..723eaf99 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner To Dot.path @@ -0,0 +1,97 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.308, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 6.911445078005017, + "y": -0.11712412737826428 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 1.927 + }, + "prevControl": { + "x": 6.868618724968162, + "y": 2.827330417029173 + }, + "nextControl": { + "x": 8.63228091821551, + "y": 1.6811631152454245 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.254, + "y": 0.759 + }, + "prevControl": { + "x": 8.254, + "y": 1.0090000000000008 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3837953091684457, + "rotationDegrees": 35.0 + }, + { + "waypointRelativePos": 0.6652452025586366, + "rotationDegrees": 33.0 + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "BC To Dot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Score.path b/src/main/deploy/pathplanner/paths/Right Follow To Score.path index 90adc8cf..08cf189b 100644 --- a/src/main/deploy/pathplanner/paths/Right Follow To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.7509666666666668, + "x": 1.1067175572519086, "y": 0.6256633380884444 }, "prevControl": { - "x": 3.7688065844293814, + "x": 3.1245574750146234, "y": 0.6251768362147326 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path new file mode 100644 index 00000000..01f7654f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score Anti Collision.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.631, + "y": 3.16 + }, + "prevControl": null, + "nextControl": { + "x": 5.925781760803848, + "y": 3.0625589577602206 + }, + "isLocked": false, + "linkedName": "R-Anti-Collision" + }, + { + "anchor": { + "x": 6.419771754636234, + "y": 1.4925534950071324 + }, + "prevControl": { + "x": 6.437298642104417, + "y": 2.392382816720799 + }, + "nextControl": { + "x": 6.399066666666666, + "y": 0.42955555555555525 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.511813503834114, + "y": 0.5261116588846113 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23445825932504563, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Anti Collision", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index c569aff8..27bdc626 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -8,36 +8,36 @@ }, "prevControl": null, "nextControl": { - "x": 5.763107947805457, - "y": 3.5131791221826814 + "x": 6.532444642151049, + "y": 3.329661130881772 }, "isLocked": false, "linkedName": "Right NZ" }, { "anchor": { - "x": 6.053851272542522, - "y": 1.6151808747904899 + "x": 6.419771754636234, + "y": 1.4925534950071324 }, "prevControl": { - "x": 6.040480921648686, - "y": 2.6846376869648685 + "x": 6.44766178400938, + "y": 2.924425883014991 }, "nextControl": { - "x": 6.070427960057061, - "y": 0.289258202567761 + "x": 6.399066666666666, + "y": 0.42955555555555525 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 + "x": 3.612, + "y": 0.559 }, "prevControl": { - "x": 6.290385164051354, - "y": 0.6127246790299581 + "x": 6.311283927722303, + "y": 0.5283859724203506 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score Jiggle.path b/src/main/deploy/pathplanner/paths/Right Score Jiggle.path deleted file mode 100644 index 29c167c7..00000000 --- a/src/main/deploy/pathplanner/paths/Right Score Jiggle.path +++ /dev/null @@ -1,134 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": null, - "nextControl": { - "x": 3.3131806164768474, - "y": 0.5777032745480257 - }, - "isLocked": false, - "linkedName": "Right Trench Score" - }, - { - "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 - }, - "prevControl": { - "x": 3.858050216469342, - "y": 0.6315687204629786 - }, - "nextControl": { - "x": 3.3651621367923017, - "y": 0.541952705976242 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.470799257649026, - "y": 0.5880453279537765 - }, - "prevControl": { - "x": 3.220842004837689, - "y": 0.5834223671050037 - }, - "nextControl": { - "x": 3.720756510460363, - "y": 0.5926682888025493 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.330297914985203, - "y": 0.585259293085392 - }, - "prevControl": { - "x": 3.0833481036127086, - "y": 0.54632613673904 - }, - "nextControl": { - "x": 3.5772477263576974, - "y": 0.6241924494317441 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.424220644336901, - "y": 0.5868906824364817 - }, - "prevControl": { - "x": 3.17424461599113, - "y": 0.5834287100977454 - }, - "nextControl": { - "x": 3.6741966726826725, - "y": 0.5903526547752179 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5181433736885994, - "y": 0.5885220717875714 - }, - "prevControl": { - "x": 3.768126174889212, - "y": 0.5914544946586795 - }, - "nextControl": { - "x": 3.2681605724879867, - "y": 0.5855896489164634 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2711, - "y": 0.5868473609129818 - }, - "prevControl": { - "x": 3.021101790448806, - "y": 0.5877935222084847 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Right Corner" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.7, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Score To Corner.path index 7697becc..206480da 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Corner.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Corner.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 + "x": 3.612, + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 3.36543714002239, - "y": 0.5460313281265844 + "x": 3.199970867125222, + "y": 0.6068337301834343 }, "isLocked": false, "linkedName": "Right Trench Score" }, { "anchor": { - "x": 3.2711, - "y": 0.5868473609129818 + "x": 3.282, + "y": 0.559 }, "prevControl": { - "x": 3.717247367182559, - "y": 0.5630880724749401 + "x": 3.5164317899184905, + "y": 0.47215683172745937 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 71ca80d6..a7c55caf 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 + "x": 3.282, + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 7.183152639087018, - "y": 0.5739087018544944 + "x": 6.8530699001426525, + "y": 0.5460613409415132 }, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": "Right Corner" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 22f7f845..9263a4ea 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 + "x": 3.282, + "y": 0.559 }, "prevControl": null, "nextControl": { - "x": 7.0796433666191145, - "y": 0.5868473609129818 + "x": 6.885082564711262, + "y": 0.5201840228245365 }, "isLocked": false, - "linkedName": "Right Jiggle" + "linkedName": "Right Corner" }, { "anchor": { @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 3.6120827389443653, - "y": 0.5868473609129818 + "x": 3.612, + "y": 0.559 }, "prevControl": { - "x": 7.673089129599263, - "y": 0.5487960706826538 + "x": 7.673006390654893, + "y": 0.5209487097696721 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Shallow To Score.path b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path new file mode 100644 index 00000000..f57f83cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Shallow To Score.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.569, + "y": 2.7346647646219684 + }, + "prevControl": null, + "nextControl": { + "x": 6.1198701854493605, + "y": 0.34101283880171307 + }, + "isLocked": false, + "linkedName": "Right Shallow NZ" + }, + { + "anchor": { + "x": 3.612, + "y": 0.559 + }, + "prevControl": { + "x": 6.018590584878744, + "y": 0.36492011412268166 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.37526652452025683, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right To Shallow.path b/src/main/deploy/pathplanner/paths/Right To Shallow.path new file mode 100644 index 00000000..9719aeb3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right To Shallow.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 6.939437022900763, + "y": 0.36130725190839597 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 7.569, + "y": 2.7346647646219684 + }, + "prevControl": { + "x": 7.602473282442749, + "y": 1.5045216348509767 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Shallow NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.20916905444126394, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5200573065902583, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.7736389684813755, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6143226919758464, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Shallow", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 13b73e23..e5cb1843 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 9.06721902017291, - "y": 0.41484149855907726 + "x": 8.684037089871612, + "y": 0.37982881597717666 }, "isLocked": false, "linkedName": "Right Trench Start" diff --git a/src/main/deploy/pathplanner/paths/Straight One.path b/src/main/deploy/pathplanner/paths/Straight One.path new file mode 100644 index 00000000..f3f18442 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight One.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.440805008944544, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 5.440805008944544, + "y": 0.5589999999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.441, + "y": 0.559 + }, + "prevControl": { + "x": 4.441, + "y": 0.5589999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "PathFinder Test", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Two.path b/src/main/deploy/pathplanner/paths/Straight Two.path new file mode 100644 index 00000000..0c65a299 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Two.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.0, + "y": 0.559 + }, + "prevControl": null, + "nextControl": { + "x": 7.0, + "y": 0.5590000000000002 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 0.559 + }, + "prevControl": { + "x": 6.0, + "y": 0.5590000000000002 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "PathFinder Test", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 551c8ead..f52289fc 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,13 +3,22 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ + "Anti Collision", + "BC To Dot", + "BC modified", "Bump Stuff", "Follow", + "Shallow", + "PathFinder Test", "To Depot", "To NZ", "To Score" ], - "autoFolders": [], + "autoFolders": [ + "BC Extras", + "BC Main", + "Duel" + ], "defaultMaxVel": 4.19, "defaultMaxAccel": 10.0, "defaultMaxAngVel": 300.0, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 72ead732..3e7338db 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -9,12 +9,12 @@ import java.lang.management.ManagementFactory; import java.lang.reflect.Field; import java.util.List; -import java.util.function.BiConsumer; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.commands.PathfindingCommand; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.leds.LEDApplyState; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.swerve.SwerveAutonInit; @@ -23,6 +23,7 @@ import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -223,6 +224,7 @@ public void autonomousInit() { mode = RobotMode.AUTON; CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); + // CommandScheduler.getInstance().schedule(new InstantCommand(() -> Intake.getInstance().autonInit(), Intake.getInstance())); auto = robot.getAutonomousCommand(); @@ -257,6 +259,9 @@ public void teleopInit() { CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); CommandScheduler.getInstance().schedule(new IntakeDeploy()); + CommandScheduler.getInstance().schedule(new HandoffStop().alongWith(new SpindexerStop())); + //Reset LEDs from auton. InstantCommand to be removed + CommandScheduler.getInstance().schedule(new LEDApplyState(LedState.RESET).withTimeout(1.0)); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f3c29829..4d33cb8c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,42 +7,30 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.Depot; -import com.stuypulse.robot.commands.auton.regular.LeftBump; -import com.stuypulse.robot.commands.auton.regular.LeftFollow; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; -import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; -import com.stuypulse.robot.commands.auton.regular.RightBump; -import com.stuypulse.robot.commands.auton.regular.RightFollow; -import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; -import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; -import com.stuypulse.robot.commands.auton.test.BoxTest; -import com.stuypulse.robot.commands.auton.test.EmptyTest; -import com.stuypulse.robot.commands.handoff.HandoffReverse; +import com.stuypulse.robot.commands.auton.regular.TwoCorner; +import com.stuypulse.robot.commands.auton.regular.TwoCornerShallow; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.HomingRoutineLower; import com.stuypulse.robot.commands.hood.HomingRoutineUpper; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtLowerHardstop; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtUpperHardstop; -import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; -import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.intake.IntakeTeleopDigest; import com.stuypulse.robot.commands.intake.SeedPivotDeployed; import com.stuypulse.robot.commands.intake.SeedPivotStowed; -import com.stuypulse.robot.commands.leds.LEDApplyPattern; -import com.stuypulse.robot.commands.spindexer.SpindexerReverse; +import com.stuypulse.robot.commands.leds.LEDApplyState; +import com.stuypulse.robot.commands.leds.LEDDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureCacheState; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; -import com.stuypulse.robot.commands.superstructure.SuperstructureManualOverride; import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; @@ -50,7 +38,6 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; -import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.commands.swerve.SwerveResetPoseKBShot; import com.stuypulse.robot.commands.swerve.SwerveResetPoseLeftCorner; import com.stuypulse.robot.commands.swerve.SwerveResetPoseRightCorner; @@ -67,11 +54,11 @@ import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.intake.Intake.RollerState; +import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -83,23 +70,22 @@ import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.PathUtil.AutonConfig; -import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RepeatCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { @@ -111,7 +97,7 @@ public interface EnabledSubsystems { SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); - SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", false); + SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", true); SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); @@ -134,7 +120,7 @@ public interface EnabledSubsystems { private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); - // private final LEDController leds = LEDController.getInstance(); + private final LEDController leds = LEDController.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -163,7 +149,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // leds.setDefaultCommand(new LEDDefaultCommand()); + leds.setDefaultCommand(new LEDDefaultCommand()); } /***************/ @@ -194,8 +180,26 @@ private void configureButtonBindings() { // ) // ); - // Digest (TR) + // Shoot in place (TR) driver.getTopButton() + .whileTrue(new SuperstructureCacheState(driver) + .andThen(new WaitUntilCommand(superstructure::isReadyToShoot)) + .andThen( + Commands.parallel( + new IntakeDeploy(), + new RunCommand( + () -> handoff.setState(HandoffState.FORWARD), + handoff), + new RunCommand( + () -> spindexer.setState(SpindexerState.FORWARD), + spindexer) + ) + )) + .onFalse( + new SpindexerStop().alongWith(new HandoffStop()) + ); + + driver.getDPadLeft() .whileTrue(new IntakeTeleopDigest().repeatedly()) .onFalse(new IntakeDeploy()); @@ -205,31 +209,31 @@ private void configureButtonBindings() { // Intake Deploy driver.getRightTriggerButton() - .onTrue(new LEDApplyPattern(Settings.LED.INTAKE_DEPLOYED)) + // .onTrue(new LEDApplyPattern(Settings.LED.INTAKE_DEPLOYED)) .onTrue(new IntakeDeploy()); // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) .onTrue(new ResetLimelightIMU()) - .onTrue(new LEDApplyPattern(Settings.LED.RESET_HEADING)) + // .onTrue(new LEDApplyPattern(Settings.LED.RESET_HEADING)) .onFalse(new SetIMUMode(0)); // Stop Rollers driver.getLeftBumper() - .onTrue(new LEDApplyPattern(Settings.LED.STOP_ROLLERS)) + // .onTrue(new LEDApplyPattern(Settings.LED.STOP_ROLLERS)) .onTrue(new IntakeDeploy() .andThen(new IntakeStopRollers())); // Outtake driver.getRightBumper() - .whileTrue(new LEDApplyPattern(Settings.LED.REVERSE)) + // .whileTrue(new LEDApplyPattern(Settings.LED.REVERSE)) .whileTrue(new IntakeOuttake()) .onFalse(new IntakeRunRollers()); // SOTM (BR) driver.getRightMenuButton() - .onTrue(new LEDApplyPattern(Settings.LED.SOTM_ON)) + // .onTrue(new LEDApplyPattern(Settings.LED.SOTM_ON)) .onTrue(new WaitUntilCommand(() -> spindexer.getState() == SpindexerState.FORWARD) .andThen(new WaitCommand(0.75).andThen(new IntakeDeploy()))) .whileTrue(new RepeatCommand(new BuzzController(driver).onlyWhile(() -> !vision.hasData() && superstructure.getState() == SuperstructureState.SOTM))) @@ -250,7 +254,7 @@ private void configureButtonBindings() { // FOTM (BL) driver.getLeftMenuButton() - .onTrue(new LEDApplyPattern(Settings.LED.FOTM_ON)) + // .onTrue(new LEDApplyPattern(Settings.LED.FOTM_ON)) .onTrue(new IntakeRunRollers()) .onTrue(new ConditionalCommand( new ParallelCommandGroup( @@ -268,14 +272,15 @@ private void configureButtonBindings() { )); driver.getDPadDown() - .whileTrue(new SwerveXMode()) - .onTrue(new LEDApplyPattern(Settings.LED.X_WHEELS)); + .whileTrue(new SwerveXMode()); + // .onTrue(new LEDApplyPattern(Settings.LED.X_WHEELS)); // Reset (TL) driver.getDPadRight() .onTrue(new SuperstructureStow() .alongWith(new HandoffStop()) - .alongWith(new SpindexerStop())); + .alongWith(new SpindexerStop())) + .onTrue(new LEDApplyState(LedState.RESET).repeatedly().withTimeout(2.0)); // Manual Left Corner Scoring driver.getLeftButton() @@ -285,7 +290,7 @@ private void configureButtonBindings() { new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()) .andThen(new SpindexerRun()), - new SwerveResetPoseLeftCorner(), + // new SwerveResetPoseLeftCorner(), new SwerveXMode() ) ) @@ -293,10 +298,10 @@ private void configureButtonBindings() { // Manual Right Corner Scoring driver.getRightButton() - .whileTrue(new LEDApplyPattern(Settings.LED.RIGHT_CORNER)) + // .whileTrue(new LEDApplyPattern(Settings.LED.RIGHT_CORNER)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) - .onTrue(new SwerveResetPoseRightCorner()) + // .onTrue(new SwerveResetPoseRightCorner()) .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD) .andThen(new SpindexerRun()))) @@ -304,7 +309,7 @@ private void configureButtonBindings() { // Manual KB Distance Scoring driver.getBottomButton() - .whileTrue(new LEDApplyPattern(Settings.LED.KB_DISTANCE)) + // .whileTrue(new LEDApplyPattern(Settings.LED.KB_DISTANCE)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .onTrue(new SwerveResetPoseKBShot()) @@ -352,19 +357,21 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/Set Pipeline High Sun", new SetPipeline(Pipeline.HIGH_SUN)); // Unjamming - SmartDashboard.putData("Robot/Handoff Reverse", - new ConditionalCommand( - new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), - new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), - () -> handoff.getState() == HandoffState.FORWARD).alongWith(new LEDApplyPattern(Settings.LED.REVERSE))); - - SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE).alongWith(new LEDApplyPattern(Settings.LED.REVERSE))); - - SmartDashboard.putData("Robot/Spindexer Reverse", - new ConditionalCommand( - new SpindexerReverse().andThen(new WaitCommand(1)).andThen(new SpindexerRun()), - new SpindexerReverse().andThen(new WaitCommand(1).andThen(new SpindexerStop())), - () -> spindexer.getState() == SpindexerState.FORWARD).alongWith(new LEDApplyPattern(Settings.LED.REVERSE))); + // SmartDashboard.putData("Robot/Handoff Reverse", + // //IMPORTANT this will not work with LEDS + // new ConditionalCommand( + // new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), + // new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), + // () -> handoff.getState() == HandoffState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); + + // SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); + + // SmartDashboard.putData("Robot/Spindexer Reverse", + // //IMPORTANT this will not work with LEDS + // new ConditionalCommand( + // new SpindexerReverse().andThen(new WaitCommand(1)).andThen(new SpindexerRun()), + // new SpindexerReverse().andThen(new WaitCommand(1).andThen(new SpindexerStop())), + // () -> spindexer.getState() == SpindexerState.FORWARD).alongWith(new LEDApplyState(LEDSTATE.REVERSE))); } @@ -379,57 +386,173 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT - AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, prevWaitTimeOne, prevWaitTimeTwo, - "Center Hub To Depot"); - DEPOT_ONLY.register(autonChooser); + // AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Center Hub To Depot"); + // DEPOT_ONLY.register(autonChooser); + + // AutonConfig LEFT_BUMP = new AutonConfig("Left Bump", Bump::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); + // LEFT_BUMP.register(autonChooser); + + // AutonConfig RIGHT_BUMP = new AutonConfig("Right Bump", RightBump::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Bump To Score (Start)", "Right Bump To Score", "Right Bump Score To Depot"); + // RIGHT_BUMP.register(autonChooser); + + // // TWO CYCLES (TRENCH) + // AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // LEFT_TWO_CYCLE.register(autonChooser); + + // AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // RIGHT_TWO_CYCLE.register(autonChooser); + + // AutonConfig BC_TEST = new AutonConfig("BC Test", TestBC::new, prevWaitTimeOne, prevWaitTimeTwo, "Right Corner to Dot"); + // BC_TEST.register(autonChooser); + + // // TWO CYCLES (CORNER) + // AutonConfig L_CN_FN = new AutonConfig("Left Corner-Near Far-Near", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // L_CN_FN.register(autonChooser); + + // AutonConfig R_CN_FN = new AutonConfig("Right Corner-Near Far-Near", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // R_CN_FN.register(autonChooser); + + // AutonConfig L_FNS_FN = new AutonConfig("Left Far-Near Shallow Far-Near", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left To Shallow", "Left Shallow To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // L_FNS_FN.register(autonChooser); + + // AutonConfig R_FNS_FN = new AutonConfig("Right Far-Near Shallow Far-Near", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right To Shallow", "Right Shallow To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // R_FNS_FN.register(autonChooser); + + // AutonConfig L_CN_NF = new AutonConfig("Left Corner-Near Near-Far", LeftTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + // L_CN_NF.register(autonChooser); + + // AutonConfig R_CN_NF = new AutonConfig("Right Corner-Near Near-Far", RightTwoCornerVariant::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + // R_CN_NF.register(autonChooser); + + // AutonConfig L_CNL_D = new AutonConfig("Left Corner-Near Long Dot", ShallowSwipeDot::new, + // "BC Left To Shallow", "BC Left Shallow To Score", "Left Score To Corner", "Left Corner To Dot Straight" + // ); + // L_CNL_D.register(autonChooser); + + // AutonConfig R_CNL_D = new AutonConfig("Right Corner-Near Long Dot", ShallowSwipeDot::new, + // "BC Right To Shallow", "BC Right Shallow To Score", "Right Score To Corner", "Right Corner To Dot Straight" + // ); + // R_CNL_D.register(autonChooser); + + //RAN AT BATTLE CRY + // AutonConfig R_CN_NFS = new AutonConfig("Right Corner-Near Near-Far-Short", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "BC Right Score To Score NY", "Right Score To Corner", "Right Score To NZ (F)"); + // R_CN_NFS.register(autonChooser); + + // AutonConfig L_CN_NFS = new AutonConfig("Left Corner-Near Near-Far-Short", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "BC Left Score To Score NY", "Left Score To Corner", "Left Score To NZ (F)"); + // L_CN_NFS.register(autonChooser); + + //might be a duplicate of Right Far Near Shallow Far Near - if no changes to that were made + AutonConfig Right_Champs = new AutonConfig("Right Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Champs Right To Shallow", "Champs Right Shallow To Score Wide", "Champs Right Bite Score To Score", "Champs Right Score To Corner"); + Right_Champs.register(autonChooser); + + AutonConfig Left_Champs = new AutonConfig("Left Champs", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Champs Left To Shallow", "Champs Left Shallow To Score Wide", "Champs Left Bite Score To Score", "Champs Left Score To Corner"); + Left_Champs.register(autonChooser); + + AutonConfig Right_NY = new AutonConfig("Right NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "NY Right Trench To NZ", "NY Right NZ To Score", "NY Right Score To Score", "Right Score To Corner"); + Right_NY.register(autonChooser); + + AutonConfig Left_NY = new AutonConfig("Left NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "NY Left Trench To NZ", "NY Left NZ To Score", "NY Left Score To Score", "Left Score To Corner"); + Left_NY.register(autonChooser); + + AutonConfig Right_Champs_NY = new AutonConfig("Right Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "316 Champs Right To Shallow", "316 Champs Right Shallow To Score Wide", "316 NY Right Score To Score", "Right Score To Corner"); + Right_Champs_NY.register(autonChooser); + + AutonConfig Left_Champs_NY = new AutonConfig("Left Champs NY", TwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "316 Champs Left To Shallow", "316 Champs Left Shallow To Score Wide", "316 NY Left Score To Score", "Left Score To Corner"); + Left_Champs_NY.register(autonChooser); + + //BC Score To Score NY is a shorened version of NY and w the slow down + AutonConfig Right_BC = new AutonConfig("Right BC", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite Anti Collision", "Right NZ To Score Anti Collision", "BC Right Score To Score NY", "Right Score To Corner"); + Right_BC.register(autonChooser); + + AutonConfig Left_BC = new AutonConfig("Left BC", TwoCornerShallow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite Anti Collision", "Left NZ To Score Anti Collision", "BC Left Score To Score NY", "Left Score To Corner"); + Left_BC.register(autonChooser); + + // AutonConfig Exp_Right_Champs = new AutonConfig("Exp Right Champs", MasterAuton::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Champs Right To Shallow", "Champs Right Shallow To Score", "Champs Right Score To Corner", "Champs Right Bite Score To Score"); + // Right_Champs.register(autonChooser); + + // AutonConfig Exp_Left_Champs = new AutonConfig("Exp Left Champs", MasterAuton::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Champs Left To Shallow", "Champs Left Shallow To Score", "Champs Left Score To Corner", "Champs Left Bite Score To Score"); + // Left_Champs.register(autonChooser); + + + //BC RIGHT + // AutonConfig R_CN_FN_D = new AutonConfig("Right Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Dot"); + // R_CN_FN_D.register(autonChooser); - AutonConfig LEFT_BUMP = new AutonConfig("Left Bump", LeftBump::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); - LEFT_BUMP.register(autonChooser); + // AutonConfig R_CN_NF_D = new AutonConfig("Right Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Dot"); + // R_CN_NF_D.register(autonChooser); - AutonConfig RIGHT_BUMP = new AutonConfig("Right Bump", RightBump::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Bump To Score (Start)", "Right Bump To Score", "Right Bump Score To Depot"); - RIGHT_BUMP.register(autonChooser); + // AutonConfig R_CN_FN_CD = new AutonConfig("Right Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Bite Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + // R_CN_FN_CD.register(autonChooser); - // TWO CYCLES (TRENCH) - AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_TWO_CYCLE.register(autonChooser); + // AutonConfig R_CN_NF_CD = new AutonConfig("Right Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Right Corner Bite", "BC Right NZ To Score", "Right Score To Corner", "BC Right Score To Score", "Right Corner To Center Dot pt1", "Right Corner To Center Dot pt2"); + // R_CN_NF_CD.register(autonChooser); - AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_TWO_CYCLE.register(autonChooser); + //BC LEFT + // AutonConfig L_CN_FN_D = new AutonConfig("Left Corner-Near Far-Near Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Dot"); + // L_CN_FN_D.register(autonChooser); - // TWO CYCLES (CORNER) - AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); - LEFT_TWO_CORNER.register(autonChooser); + // AutonConfig L_CN_NF_D = new AutonConfig("Left Corner-Near Near-Far Dot", TwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Dot"); + // L_CN_NF_D.register(autonChooser); - AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); - RIGHT_TWO_CORNER.register(autonChooser); + // AutonConfig L_CN_FN_CD = new AutonConfig("Left Corner-Near Far-Near Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Bite Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + // L_CN_FN_CD.register(autonChooser); + // AutonConfig L_CN_NF_CD = new AutonConfig("Left Corner-Near Near-Far Center Dot", CenterTwoCornerBC::new, prevWaitTimeOne, prevWaitTimeTwo, + // "BC Left Corner Bite", "BC Left NZ To Score", "Left Score To Corner", "BC Left Score To Score", "Left Corner To Center Dot pt1", "Left Corner To Center Dot pt2"); + // L_CN_NF_CD.register(autonChooser); + // FOLLOWS - AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Left Follow To Bump", "Left Follow To Score", "Left Corner To Depot"); - LEFT_FOLLOW.register(autonChooser); + // AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Left Follow To Bump", "Left Follow To Score", "Left Corner To Depot"); + // LEFT_FOLLOW.register(autonChooser); - AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Follow To Bump", "Right Follow To Score"); - RIGHT_FOLLOW.register(autonChooser); + // AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Follow To Bump", "Right Follow To Score"); + // RIGHT_FOLLOW.register(autonChooser); + // AutonConfig EMPTY_TEST = new AutonConfig("Empty Test", EmptyTest::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Right Trench Score To Corner"); + // EMPTY_TEST.register(autonChooser); - AutonConfig EMPTY_TEST = new AutonConfig("Empty Test", EmptyTest::new, prevWaitTimeOne, prevWaitTimeTwo, - "Right Trench Score To Corner"); - EMPTY_TEST.register(autonChooser); + // AutonConfig PATH_FIND_TEST = new AutonConfig("Path Find Test", PathfindTest::new, prevWaitTimeOne, prevWaitTimeTwo, + // "Straight One", "Straight Two"); + // PATH_FIND_TEST.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); - } public boolean hasWaitTimeOneChanged() { hasWaitTimeOneChanged = prevWaitTimeOne != getWaitTimeOne(); prevWaitTimeOne = getWaitTimeOne(); - prevWaitTimeTwo = getWaitTimeTwo(); return hasWaitTimeOneChanged; } @@ -440,10 +563,10 @@ public boolean hasWaitTimeTwoChanged() { } public void configureSysids() { - autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); - autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); @@ -495,12 +618,15 @@ public void configureSysids() { } public Command getAutonomousCommand() { - if (autonChooser.getSelected() == null) { - return new DoNothingAuton(); - } - else { - return autonChooser.getSelected(); + Command autonCommand = autonChooser.getSelected(); + + if (autonCommand == null) { + autonCommand = new DoNothingAuton(); } + + DogLog.log("Auton/Selected", autonCommand.getName()); + + return autonCommand; } public static double getWaitTimeOne() { @@ -519,11 +645,11 @@ public void periodicAfterScheduler() { superstructure.getCurrentDraw() + swerve.getTotalDriveSupplyCurrent() + swerve.getTotalSteerSupplyCurrent(); - DogLog.log("Robot/Total Current Draw", totalCurrentDraw); + DogLog.log("Robot/Total Current Draw", totalCurrentDraw, "Amps"); handoff.periodicAfterScheduler(); intake.periodicAfterScheduler(); - // leds.periodicAfterScheduler(); TODO: ADD THESE BACK TY + leds.periodicAfterScheduler(); spindexer.periodicAfterScheduler(); hood.periodicAfterScheduler(); shooter.periodicAfterScheduler(); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/deprecated/BCAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/BCAuton.java new file mode 100644 index 00000000..6dfe97b1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/BCAuton.java @@ -0,0 +1,87 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.deprecated; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.leds.LEDApplyState; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class BCAuton extends SequentialCommandGroup { + + public BCAuton(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).deadlineFor( + new WaitCommand(0.2).andThen(new IntakeDeploy()), + new LEDApplyState(LedState.AUTON_COLOR_ONE) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).deadlineFor( + new SuperstructureAutoInterpolation(), + new LEDApplyState(LedState.AUTON_COLOR_TWO) + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).withTimeout(1.5).deadlineFor( + new LEDApplyState(LedState.AUTON_COLOR_ONE), + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest() + ),//.withTimeout(1.5), moved to the path up top, for the deadline for + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).deadlineFor( + new LEDApplyState(LedState.AUTON_COLOR_TWO), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new WaitCommand(5).deadlineFor( //deadline for accounts for LED Apply States + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new LEDApplyState(LedState.AUTON_COLOR_ONE), + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest() + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).deadlineFor(new LEDApplyState(LedState.AUTON_COLOR_TWO)), + + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/LeftFollow.java similarity index 92% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java rename to src/main/java/com/stuypulse/robot/commands/auton/deprecated/LeftFollow.java index 822385c7..a094c5d9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/LeftFollow.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.regular; +package com.stuypulse.robot.commands.auton.deprecated; import java.util.Set; @@ -10,11 +10,8 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.constants.Gains.Spindexer; -import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/deprecated/ShallowSwipeDot.java b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/ShallowSwipeDot.java new file mode 100644 index 00000000..01378e9a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/deprecated/ShallowSwipeDot.java @@ -0,0 +1,66 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.deprecated; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +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.WaitUntilCommand; + +public class ShallowSwipeDot extends SequentialCommandGroup { + //BC AUTON - don't know where to put + public ShallowSwipeDot(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest(), + new WaitCommand(6.5) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), //ensure SOTM is over + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + + new SwerveXMode() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/Bump.java similarity index 91% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/Bump.java index e62711b5..93759309 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/Bump.java @@ -10,10 +10,8 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; -import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -23,9 +21,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class LeftBump extends SequentialCommandGroup { +public class Bump extends SequentialCommandGroup { - public LeftBump(PathPlannerPath... paths) { + public Bump(PathPlannerPath... paths) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerVariant.java similarity index 85% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerVariant.java index c4e05f49..0628093a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCornerVariant.java @@ -31,9 +31,9 @@ import com.pathplanner.lib.path.PathPlannerPath; -public class LeftTwoCorner extends SequentialCommandGroup { +public class LeftTwoCornerVariant extends SequentialCommandGroup { - public LeftTwoCorner(PathPlannerPath... paths) { + public LeftTwoCornerVariant(PathPlannerPath... paths) { addCommands( @@ -53,12 +53,13 @@ public LeftTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -76,11 +77,9 @@ public LeftTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index a6586094..8faab6a6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -53,12 +53,13 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -76,11 +77,9 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java index d8aed42f..a386e83f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java @@ -53,12 +53,13 @@ public RightTwoCorner(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -76,11 +77,9 @@ public RightTwoCorner(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), - - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerVariant.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerVariant.java new file mode 100644 index 00000000..57668d09 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCornerVariant.java @@ -0,0 +1,88 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +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.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class RightTwoCornerVariant extends SequentialCommandGroup { + + public RightTwoCornerVariant(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 2e860ec2..59fe6f3b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -53,12 +53,13 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), new WaitCommand(1.0).andThen( - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), @@ -76,12 +77,10 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun(), new SpindexerRun(), new WaitCommand(0.5) - .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), - new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) - ), + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) - ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCorner.java new file mode 100644 index 00000000..b171a196 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCorner.java @@ -0,0 +1,88 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +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.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class TwoCorner extends SequentialCommandGroup { + //Champs sequence + public TwoCorner(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(5.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerShallow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerShallow.java new file mode 100644 index 00000000..3e359c7a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TwoCornerShallow.java @@ -0,0 +1,90 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeOuttake; +import com.stuypulse.robot.commands.intake.IntakeSetState; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +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.WaitUntilCommand; + +public class TwoCornerShallow extends SequentialCommandGroup { + //champs sequence but uses different timings with shallow + public TwoCornerShallow(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(3.5)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeSetState(PivotState.DEPLOY)), + new IntakeOuttake(), + new WaitCommand(0.2), + new IntakeDeploy(), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().withTimeout(15.0)), + new WaitCommand(15.0) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java new file mode 100644 index 00000000..cb2726b9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/PathfindTest.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.auton.test; + +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class PathfindTest extends SequentialCommandGroup { + public PathfindTest(PathPlannerPath... paths) { + addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(10), + CommandSwerveDrivetrain.getInstance().pathfindThenFollowPath(paths[1], PathConstraints.unlimitedConstraints(12.0)) + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/TestBC.java b/src/main/java/com/stuypulse/robot/commands/auton/test/TestBC.java new file mode 100644 index 00000000..ab9dc102 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/TestBC.java @@ -0,0 +1,27 @@ +package com.stuypulse.robot.commands.auton.test; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class TestBC extends SequentialCommandGroup { + + public TestBC(PathPlannerPath... paths) { + + addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0].name).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy())) + ); + + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/Right Score To Score NY.path b/src/main/java/com/stuypulse/robot/commands/handoff/Right Score To Score NY.path new file mode 100644 index 00000000..d0f23ac2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/handoff/Right Score To Score NY.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 6.575035663338087, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 5.850470756062768, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 5.850470756062768, + "y": 0.3280741797432247 + }, + "nextControl": { + "x": 5.850470756062768, + "y": 4.150659142168011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.302881844380403, + "y": 2.851112696148359 + }, + "prevControl": { + "x": 7.215816731986725, + "y": 3.83143356936277 + }, + "nextControl": { + "x": 7.525057636887608, + "y": 0.34949567723342856 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 7.673089129599265, + "y": 0.5487960706826538 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.289978678038381, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.6703967446591785, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java rename to src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java index 01a4ae3d..871dc98d 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyState.java @@ -8,32 +8,32 @@ package com.stuypulse.robot.commands.leds; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj2.command.Command; - import java.util.function.Supplier; import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; -public class LEDApplyPattern extends Command { +import edu.wpi.first.wpilibj2.command.Command; +public class LEDApplyState extends Command { + //This will not work as default command will override it. Either make the cached class the same as this OR (better solution) have a boolean that tells you if it is manually applied or not and if it is then default command dont change protected final LEDController leds; - protected final Supplier pattern; + protected final Supplier state; - public LEDApplyPattern(Supplier pattern) { + public LEDApplyState(Supplier state) { leds = LEDController.getInstance(); - this.pattern = pattern; + this.state = state; addRequirements(leds); } - public LEDApplyPattern(LEDPattern pattern) { - this(() -> pattern); + public LEDApplyState(LedState state) { + this(() -> state); } @Override public void execute() { - leds.applyPattern(pattern.get()); + leds.changeState(state.get()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index 08192002..db551aa1 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -7,6 +7,8 @@ package com.stuypulse.robot.commands.leds; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.SolidColor; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.constants.Settings; @@ -16,6 +18,7 @@ import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController.LedState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -27,11 +30,10 @@ import com.stuypulse.robot.subsystems.vision.LimelightVision; import dev.doglog.DogLog; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class LEDDefaultCommand extends Command{ +public class LEDDefaultCommand extends InstantCommand{ private final LEDController leds; private final CommandSwerveDrivetrain swerve; private final Handoff handoff; @@ -59,69 +61,50 @@ public LEDDefaultCommand() { } @Override - public void execute() { - String state = "NONE"; - + public void initialize() { if (Robot.getMode() == RobotMode.DISABLED) { if (LimelightVision.getInstance().getMaxTagCount() >= Settings.LED.DESIRED_TAGS_WHEN_DISABLED) { - leds.applyPattern(Settings.LED.DISABLED_ALIGNED); - state = "DISABLED_ALLOWED"; + leds.changeState(LedState.DISABLED_ALIGNED); } else { - leds.applyPattern(LEDPattern.solid(Color.kRed)); - state = "DISABLED_DISALLOWED"; + leds.changeState(LedState.DISABLED); } } - + else { if (swerve.isUnderTrench()) { - leds.applyPattern(Settings.LED.PASSING_TRENCH); - state = "UNDER_TRENCH"; + leds.changeState(LedState.PASSING_TRENCH); } else if (turret.isWrapping()) { - leds.applyPattern(Settings.LED.TURRET_WRAPPING); - state = "TURRET_WRAPPING"; + leds.changeState(LedState.TURRET_WRAPPING); } else if (superstructure.getState() == SuperstructureState.LEFT_CORNER) { - leds.applyPattern(Settings.LED.LEFT_CORNER); - state = "LEFT_CORNER"; + leds.changeState(LedState.LEFT_CORNER); } else if (superstructure.getState() == SuperstructureState.RIGHT_CORNER) { - leds.applyPattern(Settings.LED.RIGHT_CORNER); - state = "RIGHT_CORNER"; + leds.changeState(LedState.RIGHT_CORNER); } else if (superstructure.getState() == SuperstructureState.KB) { - leds.applyPattern(Settings.LED.KB_DISTANCE); - state = "KITBOT"; + leds.changeState(LedState.KB_DISTANCE); } else if (superstructure.getState() == SuperstructureState.SOTM) { - leds.applyPattern(Settings.LED.SOTM_ON); - state = "SOTM"; + leds.changeState(LedState.SOTM_ON); } else if (superstructure.getState() == SuperstructureState.FOTM) { - leds.applyPattern(Settings.LED.FOTM_ON); - state = "FOTM"; + leds.changeState(LedState.FOTM_ON); } - else if (spindexer.getState() == SpindexerState.REVERSE || - handoff.getState() == HandoffState.REVERSE || - intake.getRollerState() == RollerState.OUTTAKE) { - leds.applyPattern(Settings.LED.REVERSE); - state = "REVERSE"; - } + // else if (superstructure.getState() == SuperstructureState.STOW) { + // leds.changeState(LedState.RESET); + // } else if (intake.getPivotState() == PivotState.STOW) { - leds.applyPattern(Settings.LED.INTAKE_STOW); - state = "INTAKE_STOW"; + leds.changeState(LedState.INTAKE_STOW); } else if (intake.getPivotState() == PivotState.DEPLOY) { - leds.applyPattern(Settings.LED.INTAKE_DEPLOYED); - state = "INTAKE_DEPLOYED"; - } - else { - leds.applyPattern(LEDPattern.solid(Color.kRed)); + leds.changeState(LedState.INTAKE_DEPLOYED); } } - DogLog.log("Leds/State", state); + // DogLog.log("Leds/State", state); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureCacheState.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureCacheState.java new file mode 100644 index 00000000..0219d989 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureCacheState.java @@ -0,0 +1,60 @@ +package com.stuypulse.robot.commands.superstructure; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + +import edu.wpi.first.wpilibj2.command.Command; + + +public class SuperstructureCacheState extends Command { + SuperstructureState cachedState; + Superstructure superstructure; + CommandSwerveDrivetrain swerve; + Gamepad driver; + BStream isIdle; + + public SuperstructureCacheState(Gamepad driver) { + superstructure = Superstructure.getInstance(); + swerve = CommandSwerveDrivetrain.getInstance(); + this.driver = driver; + + isIdle = BStream.create( + () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) + .filtered(new BDebounce.Both(0.1)); + + this.cachedState = superstructure.getState(); + + addRequirements(superstructure, swerve); + } + + @Override + public void initialize() { + this.cachedState = superstructure.getState(); + superstructure.setState(SuperstructureState.INTERPOLATION); + SwerveRequest request = new SwerveRequest.SwerveDriveBrake(); + swerve.setControl(request); + } + + @Override + public boolean isFinished() { + return !isIdle.get(); + } + + @Override + public void end(boolean interrupted) { + // this presumes that you already overrode the state in the following part of the command chain + superstructure.setState(cachedState); + } + + private Vector2D getDriverInputAsVelocity() { + return new Vector2D(driver.getLeftStick().y, -driver.getLeftStick().x); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 33fd1e47..6a89b035 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -8,13 +8,15 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; - +import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; import com.stuypulse.robot.constants.Settings.Swerve; @@ -31,6 +33,9 @@ public class SwerveDriveDrive extends Command { private final VStream speed; private final IStream turn; + private final BStream isIdle; + private boolean isIdleInit; + public SwerveDriveDrive(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); @@ -53,7 +58,10 @@ public SwerveDriveDrive(Gamepad driver) { ); this.driver = driver; - + isIdle = BStream.create( + () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) + .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); + isIdleInit = false; addRequirements(swerve); } @@ -63,10 +71,19 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(speed.get().x) - .withVelocityY(speed.get().y) - .withRotationalRate(-turn.getAsDouble()) - ); + if (isIdle.get()) { + // if (!isIdleInit) { + // CommandScheduler.getInstance().schedule(new IntakeAutoDigest().repeatedly().onlyWhile(() -> isIdle.get()).andThen(new IntakeDeploy())); + swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + // } + isIdleInit = true; + } else { + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-turn.getAsDouble()) + ); + isIdleInit = false; + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java index e8ea6c32..56427a79 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve; +import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; import com.stuypulse.robot.constants.Settings; @@ -15,6 +16,8 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; @@ -35,7 +38,8 @@ public class SwerveDriveFOTM extends Command{ private final VStream speed; private final IStream turn; - + private final BStream isIdle; + public SwerveDriveFOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); @@ -58,6 +62,10 @@ public SwerveDriveFOTM(Gamepad driver) { new LowPassFilter(Turn.RC) ); + isIdle = BStream.create( + () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) + .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); + this.driver = driver; addRequirements(swerve); @@ -70,11 +78,14 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(speed.get().x) - .withVelocityY(speed.get().y) - .withRotationalRate(-turn.get())); - + if (isIdle.get()) { + swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + } else { + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-turn.get())); + } DogLog.log("Swerve/Speed x", speed.get().x); DogLog.log("Swerve/Speed y", speed.get().y); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java index 89303369..d772b251 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -46,7 +46,6 @@ public class SwerveDriveSOTM extends Command { private final IStream turn; private final BStream isIdle; - private boolean isIdleInit; public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); @@ -75,7 +74,6 @@ public SwerveDriveSOTM(Gamepad driver) { .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); this.driver = driver; - isIdleInit = false; addRequirements(swerve); } @@ -93,14 +91,12 @@ public void execute() { // CommandScheduler.getInstance().schedule(new IntakeAutoDigest().repeatedly().onlyWhile(() -> isIdle.get()).andThen(new IntakeDeploy())); swerve.setControl(new SwerveRequest.SwerveDriveBrake()); // } - isIdleInit = true; } else { Vector2D velocity = speed.get(); swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(velocity.x) .withVelocityY(velocity.y) .withRotationalRate(-turn.get())); - isIdleInit = false; } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java index 818e36c1..ef83f3f3 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java @@ -4,6 +4,7 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; public class SwerveResetPoseKBShot extends SwerveResetPose { @@ -11,6 +12,6 @@ public SwerveResetPoseKBShot() { super(new Pose2d( Field.HUB_CENTER.getX() - Field.HUB_RADIUS - Units.inchesToMeters(23.5), Field.WIDTH / 2 - Units.inchesToMeters(6.5), - CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + Rotation2d.kZero)); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index fc9e2cfb..a8177566 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -7,9 +7,13 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.robot.util.vision.LimelightHelpers.LimelightResults; import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; import com.stuypulse.stuylib.network.SmartBoolean; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose3d; @@ -44,10 +48,17 @@ public static class Camera { private SmartBoolean isEnabled; private String keyName; + private double LLHeartbeat = -1; + private double LLLatency = -1; + private int loopCounter = 0; + private int rejectedCounterNotNull; private int rejectedCounterAngularVelocity; private int rejectedCounterInvalidPosition; private int rejectedCounterTargetArea; + private final BStream isdead; + + private LimelightResults result; private Pipeline currentPipeline; @@ -56,6 +67,8 @@ public Camera(String name, Pose3d location, SmartBoolean isEnabled) { this.location = location; this.isEnabled = isEnabled; this.keyName = "Vision/" + name + "/"; + this.result = LimelightHelpers.getLatestResults(name); + this.isdead = BStream.create(() -> !isAlive()).filtered( new BDebounce.Rising(1), new BDebounce.Falling(0.2)); } public enum Pipeline { @@ -113,6 +126,70 @@ public void incrementRejection(RejectionValue rejectionValue) { } } + public int getNumberOfTagsSeen() { + return LimelightHelpers.getRawFiducials(this.getName()).length; + } + + public void updateHeartBeat() { + LLHeartbeat = LimelightHelpers.getHeartbeat(this.getName()); + } + + public void updateLatency() { + LLLatency = LimelightHelpers.getLatency_Pipeline(this.name); + } + + public void incrementLoopCounter() { + loopCounter += 1; + } + + public boolean isAlive() { + //latest - old heartbeat + boolean isDeadLatency = (LLLatency == LimelightHelpers.getLatency_Pipeline(this.name) && LLLatency != -1); + DogLog.log(keyName + "isDeadLatency", isDeadLatency); + //TODO: double check that latency cannot be negative + boolean isDeadHeartbeat = LimelightHelpers.getHeartbeat(this.getName()) - LLHeartbeat < Settings.Vision.MIN_CYCLE_LL_HB && LLHeartbeat != -1; + if (isDeadHeartbeat || isDeadLatency) { + return false; + } + else { + return true; + } + + } + + public void updateLEDs() { + if (this.loopCounter == 50) { + boolean iscurrentlyDead = isdead.get(); + switch (this.getName()) { + case "limelight-right" -> { + LEDController.isRightLLDead = iscurrentlyDead; + this.loopCounter = 0; + } + case "limelight-left" -> { + LEDController.isLeftLLDead = iscurrentlyDead; + this.loopCounter = 0; + } + case "limelight-back" -> { + LEDController.isBackLLDead = iscurrentlyDead; + } + + } + this.loopCounter = 0; + } + } + + // public boolean seesTag() { + // return getNumberOfTagsSeen() == 0; + // } + + // public void checkHeartBeats() { + + // } + + // public boolean isDead(){ + // return this.isDead; + // } + public void log() { DogLog.log(keyName + "# Rejected Not Null", rejectedCounterNotNull); DogLog.log(keyName + "# Rejected Target Area", rejectedCounterTargetArea); @@ -120,7 +197,8 @@ public void log() { DogLog.log(keyName + "# Rejected Invalid Position", rejectedCounterInvalidPosition); DogLog.log(keyName + "Heartbeat", LimelightHelpers.getHeartbeat(name)); - DogLog.log(keyName + "Temp (C)", LimelightHelpers.getLimelightDoubleArrayEntry(name, "hw").get()); + DogLog.log(keyName + "Temp", LimelightHelpers.getLimelightDoubleArrayEntry(name, "hw").get(), "Celsius"); + DogLog.log(keyName + "Cpu Usage", LimelightHelpers.getLimelightDoubleArrayEntry(name, "cpu").get(), "Percent"); DogLog.log(keyName + "Pose MT1", (Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed(name).pose)); @@ -128,6 +206,15 @@ public void log() { ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).pose : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name).pose)); DogLog.log(keyName + "Pipeline", LimelightHelpers.getCurrentPipelineIndex(name)); + + // this yaw is seems to be the robot yaw passed into the LL + DogLog.log(keyName + "Robot Yaw Passed In", LimelightHelpers.getIMUData(name).robotYaw); + // this is just the yaw of the internal imu + DogLog.log(keyName + "IMU Yaw", LimelightHelpers.getIMUData(name).Yaw); + DogLog.log(keyName + "Pipeline Latency", LimelightHelpers.getLatency_Pipeline(name)); + + result = LimelightHelpers.getLatestResults(name); + DogLog.log(keyName + "Time since last boot", result.timestamp_LIMELIGHT_publish / 1000.0, "Seconds"); RawFiducial[] rawFiducials = LimelightHelpers.getRawFiducials(name); for(Integer i = 0; i < rawFiducials.length; i++) { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 5a1c76b9..907b02e9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -43,6 +43,8 @@ public interface Field { public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); + public static final double OPPONENT_HUB_DS_X = LENGTH - HUB_FAR_LEFT_CORNER.getX() + 2.0 * HUB_RADIUS; + public static final double BEHIND_HUB_TOLERANCE_X = Units.inchesToMeters(144); // To extend the triangle vertex public static final double BEHIND_HUB_TOLERANCE_Y = Units.inchesToMeters(12) + Units.inchesToMeters(2); // To extend base of triangle (colinear with back hub) diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 4d4e11c9..9bdcad21 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -18,6 +18,7 @@ public interface Gamepad { public interface LED { int LED_PORT = 1; + int CANDLE_PORT = 61; } public interface ClimberHopper { @@ -54,6 +55,7 @@ public interface Intake { } public interface Spindexer { - int MOTOR = 30; + int LEADER = 30; + int FOLLOWER = 31; // TODO: follower port } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7a504b27..d1f8d607 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -6,6 +6,9 @@ package com.stuypulse.robot.constants; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.controls.RainbowAnimation; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.signals.RGBWColor; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; @@ -18,9 +21,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; /*- @@ -62,7 +62,7 @@ public interface Handoff { } public interface Intake { - Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(71.0); + Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(71.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(-10.0); Rotation2d PIVOT_DIGEST_ANGLE = Rotation2d.fromDegrees(30); @@ -77,7 +77,7 @@ public interface Intake { double HOMING_VOLTAGE = 3.0; double PUSHDOWN_VOLTAGE = -3.0; - double PUSHDOWN_CURRENT_TELEOP = -75.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY + double PUSHDOWN_CURRENT_TELEOP = -55.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY double PUSHDOWN_CURRENT_AUTON = -80.0; double GEAR_RATIO = 32.0/20.0 * 64.0/18.0 * 60.0/8.0; @@ -142,7 +142,8 @@ public interface RPMInterpolation{ {3.38, 3075}, {4.43, 3350.0}, {5.66, 3650.0}, - {6.44, 3800} + {6.44, 3800.0}, + {8.23, 4500.0} // THIS POINT IS AN EXTRAPOLATION }; } @@ -154,7 +155,9 @@ public interface TOFInterpolation{ {3.38, 1.02}, {4.43, 1.165}, {5.50, 1.21}, - {6.44, 1.255} + {6.44, 1.255}, + {6.6, 1.41}, + {8.23, 1.71} // THIS POINT IS AN EXTRAPOLATION }; } @@ -165,12 +168,9 @@ public interface FerryRPMInterpolation { {5.16, 3300.0}, {6.94, 3600.0}, {7.87, 3800.0}, - {9.77, 4300.0}, - {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! - {11.516, 4900.0}, - {12.416, 5200.0}, - {13.316, 5500.0}, - {14.216, 5600.0} + {9.77, 4300.0}, //TODO: ADD DATA BACK IN COMP + {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! + {11.516, 4900.0} }; } @@ -198,7 +198,7 @@ public interface Shooter { public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2.0); public interface RPM { - public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3500.0); + public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3863.0); public final double REVERSE = 0.0; public final double KB = 2675.0; @@ -237,10 +237,10 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { - public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 20.0); - public final Rotation2d FERRY_ANGLE = Rotation2d.fromDegrees(44.0); + public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 44.0); public final Rotation2d MAX = FORWARD_SOFT_LIMIT; public final Rotation2d MIN = REVERSE_SOFT_LIMIT; + public final Rotation2d FERRY_ANGLE = MAX;//Rotation2d.fromDegrees(44.0); public final Rotation2d STOW = Rotation2d.fromDegrees(21.0); public final Rotation2d KB = Rotation2d.fromDegrees(20.0); @@ -361,43 +361,81 @@ public interface Tolerances { } public interface LED { + //TODO: + //add back states (reverse and etc (low priority)) - LEDPattern PASSING_TRENCH = LEDPattern.solid(Color.kRed); - LEDPattern IS_BEHIND_HUB = LEDPattern.solid(Color.kRed); + //(DONE) remove stuff we dont use + //(DONE) space out dead limelight indicators - // LEDPattern CLIMB_ALIGNING = LEDPattern.solid(Color.kYellow); - // LEDPattern CLIMB_ALIGNED = LEDPattern.solid(Color.kGreen); - // LEDPattern CLIMBING = LEDPattern.solid(Color.kRed); + //FIX rainbow (flickering) + //make dead limelight colors flash (GIVEN THEY WORK) + //TUNE constant for heart beat - LEDPattern TURRET_WRAPPING = LEDPattern.solid(Color.kRed); - LEDPattern LEFT_WARNING = LEDPattern.solid(Color.kBlack); // TBD - LEDPattern RIGHT_WARNING = LEDPattern.solid(Color.kBlack); // TBD + //(DONE BUT CONFIRM WITH BLAY IF HE WANTS TIME (like 2 seconds outside of zone)) + //make debounce for the distance away from last pose with april tags in sight - LEDPattern SHOOT_IN_PLACE = LEDPattern.solid(Color.kPurple); + //(DONE) make the getter for the last pose with april tags in sight + // (PARTIAL IMPLEMENTATION) add the variables to the Camera objects as fields (heartbeats) - LEDPattern SOTM_ON = LEDPattern.solid(Color.kCyan); - LEDPattern FOTM_ON = LEDPattern.rainbow(255, 128).scrollAtAbsoluteSpeed(MetersPerSecond.of(1), Meters.of(1 / 120.0)); + //Add flashing based on the distance thing. - LEDPattern LEFT_CORNER = LEDPattern.solid(Color.kPurple); - LEDPattern RIGHT_CORNER = LEDPattern.solid(Color.kBlue); + //SEPERATE THING: ADD JSONS TO LL from the PractiCAL + + public SolidColor solidColorRequest = new SolidColor(0, Settings.LED.LED_LENGTH - 1).withColor(new RGBWColor(Color.kRed)); + public RainbowAnimation rainbowRequest = new RainbowAnimation(0, Settings.LED.LED_LENGTH - 1).withFrameRate(60).withSlot(0); + + public static RGBWColor rgbwConverter(Color color) { + return new RGBWColor(color); + } + + public final int LED_LENGTH = 8 + 21; //CANdle already has 8 + RGBWColor PASSING_TRENCH = rgbwConverter(Color.kRed); + RGBWColor IS_BEHIND_HUB = rgbwConverter(Color.kRed); + + // RGBWColor CLIMB_ALIGNING = rgbwConverter(Color.kYellow); + // RGBWColor CLIMB_ALIGNED = rgbwConverter(Color.kGreen); + // RGBWColor CLIMBING = rgbwConverter(Color.kRed); + + RGBWColor TURRET_WRAPPING = rgbwConverter(Color.kRed); + // RGBWColor LEFT_WARNING = rgbwConverter(Color.kBlack); // TBD + // RGBWColor RIGHT_WARNING = rgbwConverter(Color.kBlack); // TBD + + RGBWColor SHOOT_IN_PLACE = rgbwConverter(Color.kPurple); + + RGBWColor SOTM_ON = rgbwConverter(Color.kGreen); + RGBWColor FOTM_ON = rgbwConverter(Color.kDarkBlue); + RGBWColor LEFT_CORNER = rgbwConverter(Color.kPurple); + RGBWColor RIGHT_CORNER = rgbwConverter(Color.kBlue); - LEDPattern KB_DISTANCE = LEDPattern.solid(Color.kPink); + RGBWColor KB_DISTANCE = rgbwConverter(Color.kPink); + + // RGBWColor REVERSE = rgbwConverter(Color.kWhite); + RGBWColor STOP_ROLLERS = rgbwConverter(Color.kYellow); + + RGBWColor RESET_HEADING = rgbwConverter(Color.kYellow); + RGBWColor X_WHEELS = rgbwConverter(Color.kRed); + + RGBWColor INTAKE_STOW = rgbwConverter(Color.kBrown); //broken + RGBWColor INTAKE_DEPLOYED = rgbwConverter(Color.kPurple); //broken + + RGBWColor DISABLED_ALIGNED = rgbwConverter(Color.kGreen); + RGBWColor DISABLED = rgbwConverter(Color.kRed); - LEDPattern REVERSE = LEDPattern.solid(Color.kWhite); - LEDPattern STOP_ROLLERS = LEDPattern.solid(Color.kYellow); + RGBWColor AUTON_ONE = rgbwConverter(Color.kBlue); + RGBWColor AUTON_TWO = rgbwConverter(Color.kOrange); - LEDPattern RESET_HEADING = LEDPattern.solid(Color.kYellow); - LEDPattern X_WHEELS = LEDPattern.solid(Color.kRed); + RGBWColor LLDEAD = rgbwConverter(Color.kWhite); - LEDPattern INTAKE_STOW = LEDPattern.solid(Color.kBrown); //broken - LEDPattern INTAKE_DEPLOYED = LEDPattern.solid(Color.kOrange); //broken + SolidColor RIGHT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 6, Settings.LED.LED_LENGTH - 2); + SolidColor BACK_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 13, Settings.LED.LED_LENGTH - 9); + SolidColor LEFT_DEAD_STRIP = new SolidColor(Settings.LED.LED_LENGTH - 20, Settings.LED.LED_LENGTH - 16); + SolidColor CANDLE_DEAD_STRIP = new SolidColor(0, 7); - LEDPattern DISABLED_ALIGNED = LEDPattern.solid(Color.kGreen); - // LEDPattern.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); + // RGBWColor.gradient(GradientType.kDiscontinuous, Color.kRed, Color.kWhite).scrollAtRelativeSpeed(Percent.per(Second).of(25)); public final int DESIRED_TAGS_WHEN_DISABLED = 2; - public final int LED_LENGTH = 9; // TBA + public double APRIL_TAG_DISTANCE_THRESHOLD = Units.feetToMeters(2); //TODO: update because comparing Translation2d, so make sure it is 2 feet } public interface Vision { @@ -409,6 +447,7 @@ public interface Vision { public final double INVALID_POSITION_TOLERANCE_M = 0.05; public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI; double MIN_TAG_AREA = 5; //TODO: MAKE SURE THIS IS A GOOD VALUE!!! + public final double MIN_CYCLE_LL_HB = 1; // TODO: tune SmartBoolean HDR_ENABLED = new SmartBoolean("Vision/HDR Enabled?", false); double HDR_TIMEOUT_SEC = 0.25; diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index 0a5da6c1..a69dbcfb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -115,8 +115,6 @@ public double getFollowerRPM() { public void periodicAfterScheduler() { super.periodicAfterScheduler(); - // removed shouldNotShootIntoHub logic (no longer used) - if (EnabledSubsystems.HANDOFF.get() && getState() != HandoffState.STOP) { if (voltageOverride.isPresent()) { motorLead.setVoltage(voltageOverride.get()); @@ -132,18 +130,18 @@ public void periodicAfterScheduler() { motorFollow.stopMotor(); } - DogLog.log("Handoff/Lead Velocity", getLeaderRPM()); - DogLog.log("Handoff/Follow Velocity", getLeaderRPM()); + DogLog.log("Handoff/Lead Velocity", getLeaderRPM(), "RPM"); + DogLog.log("Handoff/Follow Velocity", getLeaderRPM(), "RPM"); if (Settings.DEBUG_MODE.get()) { - DogLog.log("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble()); - DogLog.log("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - DogLog.log("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble(), "Amps"); - DogLog.log("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble()); - DogLog.log("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - DogLog.log("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble(), "Amps"); if(Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log("Robot/CAN/Main/Handoff Lead Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_LEAD) + ")", motorLead.isConnected()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index cb0c1496..68a66f1a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -93,6 +93,8 @@ public void setRollerState(RollerState state) { public abstract void setPivotVoltageOverride(Optional voltage); public abstract SysIdRoutine getPivotSysIdRoutine(); public abstract boolean pivotStalling(); + public abstract void teleopInit(); + public abstract void autonInit(); public abstract void seedPivotDeployed(); public abstract void seedPivotStowed(); @@ -103,7 +105,7 @@ public void periodicAfterScheduler() { DogLog.log("Intake/Pivot State", getPivotState().toString()); DogLog.log("Intake/Roller State", getRollerState().toString()); - DogLog.log("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); + DogLog.forceNt.log("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); DogLog.log("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); DogLog.log("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 330d5fa1..5e48ddff 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -8,6 +8,8 @@ import java.util.Optional; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; @@ -59,6 +61,7 @@ public class IntakeImpl extends Intake { private final BStream isPivotBelowPushDownThreshold; StatusSignal pivotSupplyCurrent; + StatusSignal pivotTorqueCurrent; StatusSignal pivotStatorCurrent; StatusSignal rollerLeaderSupplyCurrent; StatusSignal rollerLeaderStatorCurrent; @@ -77,7 +80,7 @@ public IntakeImpl() { .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(10.0) // was 60 on practice day + .withSupplyCurrentLimitAmps(20.0) // was 60 on practice day .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) @@ -121,6 +124,7 @@ public IntakeImpl() { pivotSupplyCurrent = pivot.getSupplyCurrent(); pivotStatorCurrent = pivot.getStatorCurrent(); + pivotTorqueCurrent = pivot.getTorqueCurrent(); pivotMotorPosition = pivot.getPosition(); rollerLeaderSupplyCurrent = rollerLeader.getSupplyCurrent(); rollerLeaderStatorCurrent = rollerLeader.getStatorCurrent(); @@ -135,7 +139,7 @@ public IntakeImpl() { PhoenixUtil.registerToRio(pivotSupplyCurrent, pivotStatorCurrent, pivotMotorPosition, rollerLeaderSupplyCurrent, rollerLeaderStatorCurrent, rollerFollowerSupplyCurrent, rollerFollowerStatorCurrent, rollerLeaderTemperature, rollerFollowerTemperature, pivotTemperature, pivotMotorVoltage, - rollerLeaderVoltage, rollerFollowerVoltage); + rollerLeaderVoltage, rollerFollowerVoltage, pivotTorqueCurrent); pivotStalling = BStream.create( () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.PIVOT_STALL_CURRENT) @@ -145,6 +149,22 @@ public IntakeImpl() { .filtered(new BDebounce.Falling(0.1)); } + @Override + public void teleopInit() { + TalonFXConfiguration newConfiguration = new TalonFXConfiguration(); + pivot.getConfigurator().refresh(newConfiguration); + newConfiguration.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(10)); + pivot.getConfigurator().apply(newConfiguration); + } + + @Override + public void autonInit() { + TalonFXConfiguration newConfiguration = new TalonFXConfiguration(); + pivot.getConfigurator().refresh(newConfiguration); + newConfiguration.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20)); + pivot.getConfigurator().apply(newConfiguration); + } + @Override public boolean pivotStalling() { return pivotStalling.get(); @@ -240,38 +260,39 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge // PIVOT DogLog.log("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); - DogLog.log("Intake/Pivot Closed Loop Error (deg)", - pivot.getClosedLoopError().getValueAsDouble() * 360.0); + DogLog.log("Intake/Pivot Closed Loop Error", + pivot.getClosedLoopError().getValueAsDouble() * 360.0, "Degrees"); if (Settings.DEBUG_MODE.get()) { DogLog.log("Intake/Voltage Override", pivotVoltageOverride.isPresent()); - DogLog.log("Intake/Pivot Temperature (C)", pivotTemperature.getValueAsDouble()); - DogLog.log("Intake/Leader Temperature (C)", - rollerLeaderTemperature.getValueAsDouble()); - DogLog.log("Intake/Follower Temperature (C)", - rollerFollowerTemperature.getValueAsDouble()); + DogLog.log("Intake/Pivot Temperature", pivotTemperature.getValueAsDouble(), "Celsius"); + DogLog.log("Intake/Leader Temperature", + rollerLeaderTemperature.getValueAsDouble(), "Celsius"); + DogLog.log("Intake/Follower Temperature", + rollerFollowerTemperature.getValueAsDouble(), "Celsius"); // Rolers - DogLog.log("Intake/Roller Leader Voltage (volts)", - rollerLeaderVoltage.getValueAsDouble()); - DogLog.log("Intake/Roller Leader Current (amps)", - rollerLeaderSupplyCurrent.getValueAsDouble()); - DogLog.log("Intake/Roller Leader Stator Current (amps)", - rollerLeaderStatorCurrent.getValueAsDouble()); - DogLog.log("Intake/Roller Follower Voltage (volts)", - rollerFollowerVoltage.getValueAsDouble()); - DogLog.log("Intake/Roller Follower Current (amps)", - rollerFollowerSupplyCurrent.getValueAsDouble()); - DogLog.log("Intake/Roller Follower Stator Current (amps)", - rollerFollowerStatorCurrent.getValueAsDouble()); - + DogLog.log("Intake/Roller Leader Voltage", + rollerLeaderVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Intake/Roller Leader Current", + rollerLeaderSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Roller Leader Stator Current", + rollerLeaderStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Roller Follower Voltage", + rollerFollowerVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Intake/Roller Follower Current", + rollerFollowerSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Roller Follower Stator Current", + rollerFollowerStatorCurrent.getValueAsDouble(), "Amps"); + // Pivot - DogLog.log("Intake/Pivot Voltage (volts)", pivotMotorVoltage.getValueAsDouble()); - DogLog.log("Intake/Pivot Supply Current (amps)", - pivotSupplyCurrent.getValueAsDouble()); - DogLog.log("Intake/Pivot Stator Current (amps)", - pivotStatorCurrent.getValueAsDouble()); + DogLog.log("Intake/Pivot Voltage", pivotMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Intake/Pivot Supply Current", + pivotSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Intake/Pivot Stator Current", + pivotStatorCurrent.getValueAsDouble(), "Amps"); DogLog.log("Intake/Pivot is below pushdown Threshold", isPivotBelowPushDownThreshold.get()); + DogLog.log("Intake/Pivot Torque Current", pivotTorqueCurrent.getValueAsDouble(), "Amps"); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " @@ -280,9 +301,9 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge + String.valueOf(Ports.Intake.ROLLER_LEADER) + ")", rollerLeader.isConnected()); DogLog.log("Robot/CAN/Main/Intake Roller Follower Motor Connected? (ID " + String.valueOf(Ports.Intake.ROLLER_FOLLOWER) + ")", rollerFollower.isConnected()); + } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); - } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java index ca3dbffb..7c4485b0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java @@ -216,4 +216,10 @@ public SysIdRoutine getPivotSysIdRoutine() { public double getCurrentDraw() { return 0; } + + @Override + public void teleopInit() {} + + @Override + public void autonInit() {} } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 54318f91..4896183a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -1,4 +1,3 @@ - /************************ PROJECT MARY *************************/ /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ @@ -7,59 +6,188 @@ package com.stuypulse.robot.subsystems.leds; +import com.ctre.phoenix6.configs.CANdleConfiguration; +import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; +import com.ctre.phoenix6.configs.LEDConfigs; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.hardware.CANdle; +import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; +import com.ctre.phoenix6.signals.RGBWColor; +import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; +import com.ctre.phoenix6.signals.StripTypeValue; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDController extends SubsystemBase { - private final static LEDController instance; + public static boolean isLeftLLDead; + public static boolean isBackLLDead; + public static boolean isRightLLDead; +; + public boolean leftDeadAnimationCleared; + public boolean backDeadAnimationCleared; + public boolean rightDeadAnimationCleared; + + // private Pose2d lastPoseOnAprilTag; + // private boolean initialPoseUpdated = false; + static { - instance = new LEDController(Ports.LED.LED_PORT, Settings.LED.LED_LENGTH); + instance = new LEDController(); } public static LEDController getInstance() { return instance; } - private AddressableLED leds; - private AddressableLEDBuffer ledsBuffer; + private final CANdle leds; + private CANdleConfiguration candleConfigs; + private ControlRequest ledPattern = Settings.LED.solidColorRequest.withColor(Settings.LED.DISABLED); - private final LEDPattern defaultPattern = LEDPattern.kOff; + private LEDController() { + leftDeadAnimationCleared = false; + backDeadAnimationCleared = false; + rightDeadAnimationCleared = false; - protected LEDController(int port, int length) { - leds = new AddressableLED(port); - ledsBuffer = new AddressableLEDBuffer(length); + isLeftLLDead = false; + isBackLLDead = false; + isRightLLDead = false; - leds.setLength(length); - leds.setData(ledsBuffer); - leds.start(); + leds = new CANdle(Ports.LED.CANDLE_PORT, Ports.CANIVORE); + // lastPoseOnAprilTag = new Pose2d(); - applyPattern(defaultPattern); + candleConfigs = new CANdleConfiguration() + .withLED( + new LEDConfigs() + .withBrightnessScalar(1.0) + .withStripType(StripTypeValue.GRB) + .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)) - SmartDashboard.putData(instance); + .withCANdleFeatures( + new CANdleFeaturesConfigs().withStatusLedWhenActive(StatusLedWhenActiveValue.Enabled)); + + leds.getConfigurator().apply(candleConfigs); + + leds.setControl(ledPattern); } - public void applyPattern(LEDPattern pattern) { - pattern.applyTo(ledsBuffer); + // add the flashing aspect based on if we don't see a tag (with debounce or by distance) + + public enum LedState { + PASSING_TRENCH(Settings.LED.PASSING_TRENCH), + IS_BEHIND_HUB(Settings.LED.IS_BEHIND_HUB), + TURRET_WRAPPING(Settings.LED.TURRET_WRAPPING), + SHOOT_IN_PLACE(Settings.LED.SHOOT_IN_PLACE), + SOTM_ON(Settings.LED.SOTM_ON), + FOTM_ON(Settings.LED.FOTM_ON), + LEFT_CORNER(Settings.LED.LEFT_CORNER), + RIGHT_CORNER(Settings.LED.RIGHT_CORNER), + KB_DISTANCE(Settings.LED.KB_DISTANCE), + STOP_ROLLERS(Settings.LED.STOP_ROLLERS), + RESET(Settings.LED.RESET_HEADING), + X_WHEELS(Settings.LED.X_WHEELS), + INTAKE_STOW(Settings.LED.INTAKE_STOW), + INTAKE_DEPLOYED(Settings.LED.INTAKE_DEPLOYED), + DISABLED_ALIGNED(Settings.LED.DISABLED_ALIGNED), + DISABLED(Settings.LED.DISABLED), + AUTON_COLOR_ONE(Settings.LED.AUTON_ONE), + AUTON_COLOR_TWO(Settings.LED.AUTON_TWO); + + private RGBWColor color; + + private LedState(RGBWColor color) { + this.color = color; + } + + private RGBWColor getColor() { + return this.color; + } + + public ControlRequest getAnimation() { + return Settings.LED.solidColorRequest.withColor(this.color); + } } + private LedState state = LedState.DISABLED; + private LedState cachedState = LedState.DISABLED; + + //TODO: make branch for the distance flashing thing + public void applyPattern() { + // if (initialPoseUpdated && + // lastPoseOnAprilTag.getTranslation() + // .getDistance(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()) > Settings.LED.APRIL_TAG_DISTANCE_THRESHOLD) { + // + // } + + if (cachedState != state) { + SolidColor solidColor = (SolidColor) ledPattern; + solidColor.withColor(state.getColor()); + + cachedState = state; + } + } + + public void changeState(LedState state) { + this.state = state; + } + + public void periodicAfterScheduler() { + // if (Cameras.LimelightCameras[0].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[1].getNumberOfTagsSeen() > 0 || + // Cameras.LimelightCameras[2].getNumberOfTagsSeen() > 0) { + // lastPoseOnAprilTag = CommandSwerveDrivetrain.getInstance().getPose(); + // initialPoseUpdated = true; + // } + if (RobotContainer.EnabledSubsystems.LEDS.get()) { - // leds.start(); - leds.setData(ledsBuffer); + applyPattern(); + leds.setControl(ledPattern); + } else { + leds.clearAllAnimations(); } - else { - LEDPattern.kOff.applyTo(ledsBuffer); - leds.setData(ledsBuffer); + + //deadAnimationClear booleans ensure we aren't clearing animations 3 times per loop. + if (isRightLLDead) { + leds.setControl(Settings.LED.RIGHT_DEAD_STRIP + .withColor(Settings.LED.LLDEAD)); + rightDeadAnimationCleared = false; + } else if (!isRightLLDead && !rightDeadAnimationCleared) { + leds.clearAllAnimations(); + rightDeadAnimationCleared = true; + } + if (isLeftLLDead) { + leds.setControl(Settings.LED.LEFT_DEAD_STRIP + .withColor(Settings.LED.LLDEAD)); + leftDeadAnimationCleared = false; + } else if (!isLeftLLDead && !leftDeadAnimationCleared) { + leds.clearAllAnimations(); + leftDeadAnimationCleared = true; + } + if (isBackLLDead) { + leds.setControl(Settings.LED.BACK_DEAD_STRIP + .withColor(Settings.LED.LLDEAD)); + backDeadAnimationCleared = false; + } else if (!isBackLLDead && !backDeadAnimationCleared) { + leds.clearAllAnimations(); + backDeadAnimationCleared = true; } - // SmartDashboard.putString("Leds/Color", ledsBuffer.getLED(1).toString()); + + DogLog.log("LED/Applied Pattern Name", ledPattern.getName()); + + DogLog.log("LED/State Pattern Name", state.getAnimation().getName()); + DogLog.log("LED/Cached State Pattern Name", state.getAnimation().getName()); + + DogLog.log("LED/State", state.toString()); + DogLog.log("LED/Cached State", state.toString()); + + DogLog.log("LED/Is Back LL dead", isBackLLDead); + DogLog.log("LED/Is Right LL dead", isRightLLDead); + DogLog.log("LED/Is Left LL dead", isLeftLLDead); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 88f93592..8c4e2a7b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -9,8 +9,10 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; @@ -35,10 +37,13 @@ public class SpindexerImpl extends Spindexer { private final Motors.TalonFXConfig spindexerLeadConfig; + private final Motors.TalonFXConfig spindexerFollowConfig; private final TalonFX leaderMotor; + private final TalonFX followerMotor; private final DutyCycleOut controller; + private final Follower followerControl; private final BStream isStalling; private boolean hasStartedStallTimer; private final Timer unjamTimer; @@ -49,6 +54,10 @@ public class SpindexerImpl extends Spindexer { private StatusSignal leaderStatorCurrent; private StatusSignal leaderVelocity; private StatusSignal leaderMotorVoltage; + private StatusSignal followerSupplyCurrent; + private StatusSignal followerStatorCurrent; + private StatusSignal followerVelocity; + private StatusSignal followerMotorVoltage; public SpindexerImpl() { spindexerLeadConfig = new Motors.TalonFXConfig() @@ -64,19 +73,40 @@ public SpindexerImpl() { .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); - leaderMotor = new TalonFX(Ports.Spindexer.MOTOR, Ports.CANIVORE); + spindexerFollowConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(45) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) + + .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); + + leaderMotor = new TalonFX(Ports.Spindexer.LEADER, Ports.CANIVORE); + followerMotor = new TalonFX(Ports.Spindexer.FOLLOWER, Ports.CANIVORE); spindexerLeadConfig.configure(leaderMotor); + spindexerFollowConfig.configure(followerMotor); controller = new DutyCycleOut(getTargetDutyCycle()).withEnableFOC(true); + followerControl = new Follower(Ports.Spindexer.LEADER, MotorAlignmentValue.Aligned); leaderSupplyCurrent = leaderMotor.getSupplyCurrent(); leaderStatorCurrent = leaderMotor.getStatorCurrent(); leaderVelocity = leaderMotor.getVelocity(); leaderMotorVoltage = leaderMotor.getMotorVoltage(); - PhoenixUtil.registerToCanivore(leaderSupplyCurrent, leaderStatorCurrent, leaderVelocity, leaderMotorVoltage); - - isStalling = BStream.create( () -> leaderSupplyCurrent.getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) + followerSupplyCurrent = followerMotor.getSupplyCurrent(); + followerStatorCurrent = followerMotor.getStatorCurrent(); + followerVelocity = followerMotor.getVelocity(); + followerMotorVoltage = followerMotor.getMotorVoltage(); + PhoenixUtil.registerToCanivore(leaderSupplyCurrent, leaderStatorCurrent, leaderVelocity, leaderMotorVoltage, + followerSupplyCurrent, followerStatorCurrent, followerVelocity, followerMotorVoltage); + + isStalling = BStream.create(() -> (leaderSupplyCurrent.getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) || followerSupplyCurrent.getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); voltageOverride = Optional.empty(); @@ -84,10 +114,13 @@ public SpindexerImpl() { unjamTimer = new Timer(); } - private double getMotorRPM() { + private double getLeaderRPM() { return leaderVelocity.getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } + private double getFollowerRPM() { + return followerVelocity.getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; + } private boolean spindexerUnjam() { if (!hasStartedStallTimer && Handoff.getInstance().isHandoffStalling()) { @@ -115,24 +148,32 @@ public void periodicAfterScheduler() { if (EnabledSubsystems.SPINDEXER.get()) { if (voltageOverride.isPresent()) { leaderMotor.setVoltage(voltageOverride.get()); + followerMotor.setVoltage(voltageOverride.get()); } else { if (Superstructure.getInstance().shouldStop()) { leaderMotor.stopMotor(); + followerMotor.stopMotor(); } else { leaderMotor.setControl(controller.withOutput(getTargetDutyCycle())); + followerMotor.setControl(followerControl); } } } else { leaderMotor.stopMotor(); + followerMotor.stopMotor(); } - DogLog.log("Spindexer/Leader Motor RPM", getMotorRPM()); + DogLog.log("Spindexer/Leader RPM", getLeaderRPM(), "RPM"); + DogLog.log("Spindexer/Follower RPM", getFollowerRPM(), "RPM"); // SmartDashboard.putBoolean("Spindexer/Unjamming", unJamming); - DogLog.log("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); - DogLog.log("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); - DogLog.log("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Voltage", leaderMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Spindexer/Leader Supply Current", leaderSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Spindexer/Leader Stator Current", leaderStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Spindexer/Follower Voltage", followerMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Spindexer/Follower Supply Current", followerSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Spindexer/Follower Stator Current", followerStatorCurrent.getValueAsDouble(), "Amps"); // SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); @@ -140,8 +181,12 @@ public void periodicAfterScheduler() { if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log( "Robot/CAN/Canivore/Spindexer Leader Motor Connected? (ID " - + String.valueOf(Ports.Spindexer.MOTOR) + ")", + + String.valueOf(Ports.Spindexer.LEADER) + ")", leaderMotor.isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Spindexer Follower Motor Connected? (ID " + + String.valueOf(Ports.Spindexer.FOLLOWER) + ")", + followerMotor.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index c2c07c70..c3058adb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; @@ -143,6 +144,11 @@ public boolean isTurretAtTolerance(){ return turret.atTolerance(); } + public boolean isTurretLaggingFOTM() { + double error = Math.abs(turret.getAngle().minus(turret.getTargetAngle()).getRotations()); + return error >= Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_START.getRotations() && getState() == SuperstructureState.FOTM; + } + public double getTargetRPM() { return shooter.getTargetRPM(); } @@ -189,21 +195,27 @@ public boolean shouldStop() { getState() == SuperstructureState.RIGHT_CORNER && getState() == SuperstructureState.KB; boolean isBehindTower = swerve.isBehindTower() && getState() == SuperstructureState.SOTM; + boolean isBtwnOppHubAndWall = swerve.isBtwnOppHubAndWall() && getState() == SuperstructureState.FOTM; boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; + boolean turretLaggingFOTM = isTurretLaggingFOTM(); + DogLog.log("Spindexer/Should Stop/Is Behind Tower?", isBehindTower); DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + DogLog.log("Spindexer/Should Stop/Is behind Opponent's Hub While Ferrying?", isBtwnOppHubAndWall); DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); DogLog.log("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); + DogLog.log("Spindexer/Should Stop/Turret Lagging FOTM", turretLaggingFOTM); DogLog.log("Spindexer/Should Stop/In Manual State", inManualState); boolean shouldStop = isSpindexerStopState || isHandOffStopState || - isTurretWrapping || (isBehindHubWhileFerrying && !inManualState) || + isBtwnOppHubAndWall || turretLaggingSOTM || + turretLaggingFOTM || (isOutsideAllianceZone && !inManualState) || (isUnderTrench && !inManualState) || isBehindTower; @@ -241,7 +253,7 @@ else if (getState() == SuperstructureState.FOTM && shouldStop() && DriverStation if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && state == SuperstructureState.SOTM && Robot.getMode() != RobotMode.AUTON) { // allows us to start SOTM earlier in auto, but currently not desired in teleop - setState(SuperstructureState.FOTM); + setState(SuperstructureState.STOW); Spindexer.getInstance().setState(SpindexerState.STOP); Handoff.getInstance().setState(HandoffState.STOP); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 30dc518f..00a29c42 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -141,7 +141,7 @@ public void periodicAfterScheduler() { DogLog.log("Superstructure/Hood/State", state.name()); DogLog.log("Superstructure/Hood/Target Angle (deg)", getTargetAngle().getDegrees()); - DogLog.log("Superstructure/Hood/Current Angle (deg)", getAngle().getDegrees()); + DogLog.forceNt.log("Superstructure/Hood/Current Angle (deg)", getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { if (EnabledSubsystems.HOOD.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 21fc5de5..9f730fa1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -182,15 +182,15 @@ public void periodicAfterScheduler() { // SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); DogLog.log("Prematch Checks/Hood at Bottom?", getAngle().getDegrees() < Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getDegrees()); - DogLog.log("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); - DogLog.log("Superstructure/Hood/Closed Loop Error (deg)", hoodMotorClosedLoopError.getValueAsDouble() * 360.0); - DogLog.log("Superstructure/Hood/Implemented Error (Degrees)", getTargetAngle().getDegrees() - getAngle().getDegrees()); + DogLog.log("Superstructure/Hood/Correct Hood Angle", getAbsoluteHoodAngleDeg(), "Degrees"); + DogLog.log("Superstructure/Hood/Closed Loop Error", hoodMotorClosedLoopError.getValueAsDouble() * 360.0, "Degrees"); + DogLog.log("Superstructure/Hood/Implemented Error", getTargetAngle().getDegrees() - getAngle().getDegrees(), "Degrees"); if (Settings.DEBUG_MODE.get()) { - DogLog.log("Superstructure/Hood/Applied Voltage (amps)", hoodMotorVoltage.getValueAsDouble()); - DogLog.log("Superstructure/Hood/Supply Current (amps)", hoodMotorSupplyCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Hood/Stator Current (amps)", hoodMotorStatorCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Hood/Raw Motor Encoder Value",hoodMotorStatorCurrent.getValueAsDouble()); + DogLog.log("Superstructure/Hood/Applied Voltage", hoodMotorVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Superstructure/Hood/Supply Current", hoodMotorSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Hood/Stator Current", hoodMotorStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Hood/Raw Motor Encoder Value",hoodMotorStatorCurrent.getValueAsDouble(), "Degrees"); DogLog.log("Superstructure/Hood/is stalling", isStalling()); Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 30bdae5b..532bd9f0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -111,7 +111,7 @@ public ShooterImpl() { shooterFollowerTemperature = shooterFollower.getDeviceTemp(); currentlyShooting = BStream.create(() -> (shooterLeadStatorCurrent.getValueAsDouble() > Settings.Superstructure.Shooter.IS_SHOOTING_CURRENT)) - .filtered(new BDebounce.Falling(1.0)); + .filtered(new BDebounce.Falling(2.0)); PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent, @@ -153,34 +153,32 @@ public void periodicAfterScheduler() { shooterLeader.stopMotor(); } - DogLog.log("Superstructure/Shooter/Leader RPM", getLeaderRPM()); - DogLog.log("Superstructure/Shooter/Follower RPM", getFollowerRPM()); + DogLog.log("Superstructure/Shooter/Leader RPM", getLeaderRPM(), "RPM"); + DogLog.log("Superstructure/Shooter/Follower RPM", getFollowerRPM(), "RPM"); if (Settings.DEBUG_MODE.get()) { DogLog.log("InterpolationTesting/Shooter Applied Voltage", - shooterLeaderVoltage.getValueAsDouble()); - - DogLog.log("Superstructure/Shooter/Leader Voltage (volts)", - shooterLeaderVoltage.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Leader Supply Current (amps)", - shooterLeadSupplyCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Leader Stator Current (amps)", - shooterLeadStatorCurrent.getValueAsDouble()); - - DogLog.log("Superstructure/Shooter/Leader Motor Temp (C)", - shooterLeaderTemperature.getValueAsDouble()); - - DogLog.log("Superstructure/Shooter/Follower Voltage (volts)", - shooterFollowerVoltage.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Follower Supply Current (amps)", - shooterFollowSupplyCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Shooter/Follower Stator Current (amps)", - shooterFollowStatorCurrent.getValueAsDouble()); + shooterLeaderVoltage.getValueAsDouble(), "Volts"); + + DogLog.log("Superstructure/Shooter/Leader Voltage", + shooterLeaderVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Superstructure/Shooter/Leader Supply Current", + shooterLeadSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Shooter/Leader Stator Current", + shooterLeadStatorCurrent.getValueAsDouble(), "Amps"); + + DogLog.log("Superstructure/Shooter/Leader Motor Temp", + shooterLeaderTemperature.getValueAsDouble(), "Celsius"); + + DogLog.log("Superstructure/Shooter/Follower Voltage", + shooterFollowerVoltage.getValueAsDouble(), "Volts"); + DogLog.log("Superstructure/Shooter/Follower Supply Current", + shooterFollowSupplyCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Shooter/Follower Stator Current", + shooterFollowStatorCurrent.getValueAsDouble(), "Amps"); - DogLog.log("Superstructure/Shooter/Follower Motor Temp (C)", - shooterFollowerTemperature.getValueAsDouble()); - - + DogLog.log("Superstructure/Shooter/Follower Motor Temp", + shooterFollowerTemperature.getValueAsDouble(), "Celsius"); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log( @@ -195,10 +193,10 @@ public void periodicAfterScheduler() { Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } - DogLog.log("InterpolationTesting/Shooter Closed Loop Error (RPM)", - shooterLeaderClosedLoopError.getValueAsDouble() * 60.0); + DogLog.log("InterpolationTesting/Shooter Closed Loop Error", + shooterLeaderClosedLoopError.getValueAsDouble() * 60.0, "RPM"); - DogLog.log("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); + DogLog.log("Superstructure/Shooter/Implemented Error", getTargetRPM() - getLeaderRPM(), "RPM"); } private void setVoltageOverride(Optional voltageOverride) { @@ -222,9 +220,9 @@ public SysIdRoutine getShooterSysIdRoutine() { public double getCurrentDraw() { return Double.max(0, shooterLeadSupplyCurrent.getValueAsDouble()) + Double.max(0, shooterFollowSupplyCurrent.getValueAsDouble()); - } + } - @Override + @Override public boolean isShooting() { return currentlyShooting.get(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index b9866e4d..7f212c16 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -146,7 +146,7 @@ public void periodicAfterScheduler() { DogLog.log("Superstructure/Turret/State", state.name()); DogLog.log("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); - DogLog.log("Superstructure/Turret/Current Angle", getAngle().getDegrees()); + DogLog.forceNt.log("Superstructure/Turret/Current Angle", getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { if (EnabledSubsystems.TURRET.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 05119635..56fba4db 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -254,9 +254,6 @@ public void periodicAfterScheduler() { if (isWrapping) { slot = 1; } - // else if (!deltaIsSignificant) { - // slot = 2; - // } if (EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { @@ -281,26 +278,26 @@ public void periodicAfterScheduler() { turretMotor.stopMotor(); } - DogLog.log("Superstructure/Turret/Relative Encoder Position (deg)", - turretMotorPos.getValueAsDouble() * 360.0); - DogLog.log("Superstructure/Turret/Closed Loop Error (deg)", - turretMotorClosedLoopError.getValueAsDouble() * 360.0); + DogLog.log("Superstructure/Turret/Relative Encoder Position", + turretMotorPos.getValueAsDouble() * 360.0, "Degrees"); + DogLog.log("Superstructure/Turret/Closed Loop Error", + turretMotorClosedLoopError.getValueAsDouble() * 360.0, "Degrees"); - DogLog.log("Superstructure/Turret/Encoder18t Abs Position (Rot)", - encoder18tPos.getValueAsDouble()); - DogLog.log("Superstructure/Turret/Encoder17t Abs Position (Rot)", - encoder17tPos.getValueAsDouble()); + DogLog.log("Superstructure/Turret/Encoder18t Abs Position", + encoder18tPos.getValueAsDouble(), "Rotations"); + DogLog.log("Superstructure/Turret/Encoder17t Abs Position", + encoder17tPos.getValueAsDouble(), "Rotations"); - DogLog.log("Superstructure/Turret/Voltage (volts)", - turretMotorVoltage.getValueAsDouble()); + DogLog.log("Superstructure/Turret/Voltage", + turretMotorVoltage.getValueAsDouble(), "Volts"); - DogLog.log("Superstructure/Turret/Wrapped Target Angle (deg)", prevActualTargetAngle); + DogLog.log("Superstructure/Turret/Wrapped Target Angle", prevActualTargetAngle, "Degrees"); if (Settings.DEBUG_MODE.get()) { - DogLog.log("Superstructure/Turret/Stator Current (amps)", - turretMotorStatorCurrent.getValueAsDouble()); - DogLog.log("Superstructure/Turret/Supply Current (amps)", - turretMotorSupplyCurrent.getValueAsDouble()); + DogLog.log("Superstructure/Turret/Stator Current", + turretMotorStatorCurrent.getValueAsDouble(), "Amps"); + DogLog.log("Superstructure/Turret/Supply Current", + turretMotorSupplyCurrent.getValueAsDouble(), "Amps"); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { DogLog.log( @@ -312,7 +309,6 @@ public void periodicAfterScheduler() { + String.valueOf(Ports.Superstructure.Turret.ENCODER18T) + ")", encoder18t.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); - } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 1368e99a..b9d437e7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.PathPlannerLogging; import com.stuypulse.robot.Robot; @@ -86,6 +87,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private Optional isInOpponentZone = Optional.empty(); private Optional isUnderTrench = Optional.empty(); private Optional isBehindTower = Optional.empty(); + private Optional isBtwnOppHubAndWall = Optional.empty(); // private StructPublisher robotPose = NetworkTableInstance.getDefault() // .getStructTopic("Robot Pose", Pose2d.struct).publish(); @@ -476,10 +478,22 @@ public Command followPathCommand(String pathName) { } } + public Command pathfindThenFollowPath(String pathName, PathConstraints pathFindingConstraints) { + try { + return pathfindThenFollowPath(PathPlannerPath.fromPathFile(pathName), pathFindingConstraints); + } catch (Exception e) { + throw new IllegalArgumentException(pathName + " does not exist"); + } + } + public Command followPathCommand(PathPlannerPath path) { return AutoBuilder.followPath(path); } + public Command pathfindThenFollowPath(PathPlannerPath path, PathConstraints pathFindingConstraints) { + return AutoBuilder.pathfindThenFollowPath(path, pathFindingConstraints); + } + public SwerveModuleState[] getModuleStates() { return getState().ModuleStates; } @@ -650,12 +664,28 @@ public boolean isOutsideAllianceZone() { return isOutsideAllianceZone.get(); } + public boolean isBtwnOppHubAndWall() { + if (!isBtwnOppHubAndWall.isEmpty()) { + return isBtwnOppHubAndWall.get(); + } + + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean btwnOppHubAndWallX = turretTranslation.getX() < Field.LENGTH && turretTranslation.getX() > Field.OPPONENT_HUB_DS_X; + boolean btwnOppHubAndWallY = turretTranslation.getY() < Field.HUB_FAR_LEFT_CORNER.getY() && turretTranslation.getY() > Field.HUB_FAR_RIGHT_CORNER.getY(); + + isBtwnOppHubAndWall = Optional.of(btwnOppHubAndWallX && btwnOppHubAndWallY); + + return isBtwnOppHubAndWall.get(); + } + public void clearMemoized() { isBehindHub = Optional.empty(); isOutsideAllianceZone = Optional.empty(); isInOpponentZone = Optional.empty(); isUnderTrench = Optional.empty(); isBehindTower = Optional.empty(); + isBtwnOppHubAndWall = Optional.empty(); } public void teleopInit() { @@ -723,15 +753,15 @@ public void periodicAfterScheduler() { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); Vector2D fieldRelativeSpeeds = getFieldRelativeSpeeds(); - DogLog.log("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); - DogLog.log("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); + DogLog.log("Swerve/Velocity Robot Relative X", chassisSpeeds.vxMetersPerSecond, "Meters/Second"); + DogLog.log("Swerve/Velocity Robot Relative Y", chassisSpeeds.vyMetersPerSecond, "Meters/Second"); - DogLog.log("Swerve/Velocity Field Relative X (m per s)", fieldRelativeSpeeds.x); - DogLog.log("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); - DogLog.log("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); + DogLog.log("Swerve/Velocity Field Relative X", fieldRelativeSpeeds.x, "Meters/Second"); + DogLog.log("Swerve/Field Relative Rotation", pose.getRotation().getDegrees(), "Degrees"); + DogLog.log("Swerve/Velocity Field Relative Y", getFieldRelativeSpeeds().y, "Meters/Second"); - DogLog.log("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); - DogLog.log("Swerve/Distance From Hub (meters)", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); + DogLog.log("Swerve/Angular Velocity", chassisSpeeds.omegaRadiansPerSecond, "Radians/Second"); + DogLog.log("Swerve/Distance From Hub", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation()), "Meters"); DogLog.log("Swerve/Pose", pose); @@ -749,27 +779,27 @@ public void periodicAfterScheduler() { // will confirm whether we are even getting data DogLog.log("Superstructure/Turret/Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation()), "Meters"); DogLog.log("InterpolationTesting/Turret Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation()), "Meters"); DogLog.log("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() - .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); + .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation()), "Meters"); for (int i = 0; i < 4; i++) { String prefix = "Swerve/Modules/Module " + i; SwerveModuleState current = getModule(i).getCurrentState(); SwerveModuleState target = getModule(i).getTargetState(); - DogLog.log(prefix + "/Speed (m per s)", current.speedMetersPerSecond); - DogLog.log(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); - DogLog.log(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); - DogLog.log(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); + DogLog.log(prefix + "/Speed", current.speedMetersPerSecond, "Meters/Second"); + DogLog.log(prefix + "/Target Speed", target.speedMetersPerSecond, "Meters/Second"); + DogLog.log(prefix + "/Angle", current.angle.getDegrees() % 360, "Degrees"); + DogLog.log(prefix + "/Target Angle", target.angle.getDegrees() % 360, "Degrees"); if (Settings.DEBUG_MODE.get()) { DogLog.log(prefix + "/Stator Current", - getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); + getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble(), "Amps"); DogLog.log(prefix + "/Supply Current", - getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); + getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble(), "Amps"); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index c7b02853..a6b89ffd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -1,7 +1,7 @@ /** ********************** PROJECT TRIBECBOT ************************ */ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ - /* Use of this source code is governed by an MIT-style license */ - /* that can be found in the repository LICENSE file. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ /** ************************************************************ */ package com.stuypulse.robot.subsystems.vision; @@ -46,6 +46,10 @@ public static LimelightVision getInstance() { private int maxTagCount; private MegaTagMode megaTagMode; + private double prevLeftLLLatency = -1; // change to -1 is we need to switch the way we do this + private double prevRightLLLatency = -1; + private double prevBackLLLatency = -1; + private Pose2d[] limelightPoseArray; // private StructPublisher leftLimelightPosePublisher; @@ -64,16 +68,22 @@ public enum MegaTagMode { } public void setPipeline(Pipeline pipeline) { - for(Camera camera: Cameras.LimelightCameras) { + for (Camera camera : Cameras.LimelightCameras) { camera.setPipeline(pipeline); } } public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; - // leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); - // rightLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", Pose2d.struct).publish(); - // backLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", Pose2d.struct).publish(); + // leftLimelightPosePublisher = + // NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", + // Pose2d.struct).publish(); + // rightLimelightPosePublisher = + // NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", + // Pose2d.struct).publish(); + // backLimelightPosePublisher = + // NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", + // Pose2d.struct).publish(); names = new String[Cameras.LimelightCameras.length]; @@ -89,8 +99,7 @@ public LimelightVision() { robotRelativePose.getZ(), Units.radiansToDegrees(robotRelativePose.getRotation().getX()), Units.radiansToDegrees(robotRelativePose.getRotation().getY()), - Units.radiansToDegrees(robotRelativePose.getRotation().getZ()) - ); + Units.radiansToDegrees(robotRelativePose.getRotation().getZ())); limelightPoseArray[i] = new Pose2d(); } @@ -142,10 +151,13 @@ public void setIMUMode(int mode) { * gyro. * * @param assistValue, an double that sets the correction speed of the - * complementary filter for the IMU. IMU Mode 4 uses the fusing of the - * internal IMU (1khz) with the external gyro reading as well. Higher values - * ranging towards 1 indicate a faster convergence of internal IMU to the - * robot IMU mode. Defaults to 0.001. + * complementary filter for the IMU. IMU Mode 4 uses the + * fusing of the + * internal IMU (1khz) with the external gyro reading as + * well. Higher values + * ranging towards 1 indicate a faster convergence of + * internal IMU to the + * robot IMU mode. Defaults to 0.001. */ public void setIMUAssistValue(double assistValue) { for (String name : names) { @@ -158,17 +170,17 @@ public void setIMUAssistValue(double assistValue) { * indexes to -1 in the full tag list before applying the new list. * * @param tagsToBlacklist array of tag IDs to exclude from detection - * @param limelight the name of the Limelight camera to configure + * @param limelight the name of the Limelight camera to configure */ public void setTagBlacklist(int[] tagsToBlacklist, String limelight) { int[] allTags = Field.ALL_TAGS.clone(); - + for (int i = 0; i < tagsToBlacklist.length; i++) { allTags[tagsToBlacklist[i] - 1] = -1; } int[] validTags = new int[allTags.length - tagsToBlacklist.length]; - + int counter = 0; for (int i = 0; i < allTags.length; i++) { if (allTags[i] != -1) { @@ -188,7 +200,7 @@ public void setTagBlacklist(int[] tagsToBlacklist, String limelight) { * as the whitelist. * * @param tagsToWhitelist array of tag IDs to allow for detection - * @param limelight the name of the Limelight camera to configure + * @param limelight the name of the Limelight camera to configure */ public void setTagWhitelist(int[] tagsToWhitelist, String limelight) { LimelightHelpers.SetFiducialIDFiltersOverride(limelight, tagsToWhitelist); @@ -218,8 +230,15 @@ public void periodicAfterScheduler() { if (Cameras.LimelightCameras[i].isEnabled()) { String limelightName = names[i]; - // Seed robot heading (used by MT2) - LimelightHelpers.SetRobotOrientation( + Cameras.LimelightCameras[i].incrementLoopCounter(); + Cameras.LimelightCameras[i].updateLEDs(); + + //Ensure this is below updateLEDs(), otherwise the cameras will never appear as dead + Cameras.LimelightCameras[i].updateHeartBeat(); + Cameras.LimelightCameras[i].updateLatency(); + + // Seed robot heading (used by MT2) + LimelightHelpers.SetRobotOrientation( limelightName, (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360, 0, @@ -233,7 +252,7 @@ public void periodicAfterScheduler() { // MegaTag switching if (megaTagMode == MegaTagMode.MEGATAG1) { - poseEstimate = Robot.isBlue() + poseEstimate = Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName) : LimelightHelpers.getBotPoseEstimate_wpiRed(limelightName); } else { @@ -263,11 +282,6 @@ public void periodicAfterScheduler() { Cameras.LimelightCameras[i].incrementRejection(RejectionValue.ANGULAR_VELOCITY); } - // if (poseEstimate.avgTagArea >= Settings.Vision.MIN_TAG_AREA) { - // withinTargetAreaTolerance = true; - // } else { - // Cameras.LimelightCameras[i].incrementRejection(RejectionValue.TARGET_AREA); - // } Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; @@ -286,21 +300,10 @@ public void periodicAfterScheduler() { DogLog.log("Vision/Within Angular Velocity Tolerance", withinAngularVelocityTolerance); DogLog.log("Vision/Not Null", notNull); - DogLog.log("Vision/Pose X Component", robotPose.getX()); - DogLog.log("Vision/Pose Y Component", robotPose.getY()); - DogLog.log("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); - DogLog.log("Vision/Pose Estimate X " + limelightName, poseEstimate.pose.getX()); - DogLog.log("Vision/Pose Estimate Y " + limelightName, poseEstimate.pose.getY()); - DogLog.log("Vision/Pose Estimate Theta " + limelightName, poseEstimate.pose.getRotation().getDegrees()); - - // switch (limelightName) { - // case "limelight-right" -> - // rightLimelightPosePublisher.set(robotPose); - // case "limelight-left" -> - // leftLimelightPosePublisher.set(robotPose); - // case "limelight-back" -> - // backLimelightPosePublisher.set(robotPose); - // } + DogLog.log("Vision/Pose", robotPose); + DogLog.log("Vision/Pose X Component", robotPose.getX(), "Meters"); + DogLog.log("Vision/Pose Y Component", robotPose.getY(), "Meters"); + DogLog.log("Vision/Pose Theta", robotPose.getRotation().getDegrees(), "Degrees"); DogLog.log("Vision/" + names[i] + " Has Data", true); @@ -310,12 +313,7 @@ public void periodicAfterScheduler() { } DogLog.log("Vision/MegaTag Mode", megaTagMode.toString()); - // this yaw is seems to be the robot yaw passed into the LL - DogLog.log("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); - // this is just the yaw of the internal imu - DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); - - //Rejection counters + Cameras.LimelightCameras[i].log(); } } diff --git a/src/main/java/com/stuypulse/robot/util/FMSUtil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java index d28a6535..e4459ba3 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSUtil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSUtil.java @@ -19,6 +19,7 @@ public class FMSUtil { private final Timer timer = new Timer(); private boolean autoMode; private boolean autoOverride = false; + private int maxErrorPrint = 0; public enum FieldState { AUTO(0.0, 20.0), @@ -115,9 +116,10 @@ public boolean didWinAuto() { String winner = DriverStation.getGameSpecificMessage(); Optional allianceOpt = DriverStation.getAlliance(); - if (winner == null || winner.isEmpty() || allianceOpt.isEmpty()) { + if (winner == null || winner.isEmpty() || allianceOpt.isEmpty() && maxErrorPrint < 5) { DriverStation.reportWarning("No FMS auto winner data available", false); DogLog.log("FMSUtil/No Auto Winner Data", true); + maxErrorPrint += 1; return autoOverride; } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index debae0cc..de1c359c 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -18,6 +18,7 @@ import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; import com.stuypulse.stuylib.network.SmartBoolean; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -338,7 +339,9 @@ public static void updateSOTMSolution() { // SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", hubSol.targetTurretAngle().getDegrees()); // SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", hubSol.targetHoodAngle().getDegrees()); // SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", hubSol.flightTime()); - // SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); + + DogLog.log("Superstructure/SOTM/virtual pose", virtualHubPose2d.getPose()); + DogLog.log("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); } public static void updateFOTMSolution() {