Merge branch 'reveal-night' into shikhar-op-controls

This commit is contained in:
Shikhar
2026-02-28 12:07:15 -07:00
15 changed files with 514 additions and 72 deletions
@@ -19,7 +19,7 @@
{ {
"type": "path", "type": "path",
"data": { "data": {
"pathName": "Quick Shoot" "pathName": "Center-Hub-Shoot"
} }
}, },
{ {
@@ -0,0 +1,73 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "Intake Retracted"
}
},
{
"type": "path",
"data": {
"pathName": "Center-Hub-Shoot"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
},
{
"type": "path",
"data": {
"pathName": "Center-ToDepot"
}
},
{
"type": "named",
"data": {
"name": "Intake Extended"
}
},
{
"type": "path",
"data": {
"pathName": "Depot-Intake-Shoot (side)"
}
},
{
"type": "named",
"data": {
"name": "Intake Retracted"
}
},
{
"type": "named",
"data": {
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
@@ -0,0 +1,73 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "Intake Retracted"
}
},
{
"type": "path",
"data": {
"pathName": "Center-Hub-Shoot"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
},
{
"type": "path",
"data": {
"pathName": "Center-ToDepot"
}
},
{
"type": "named",
"data": {
"name": "Intake Extended"
}
},
{
"type": "path",
"data": {
"pathName": "Depot-Intake-Shoot (straight)"
}
},
{
"type": "named",
"data": {
"name": "Intake Retracted"
}
},
{
"type": "named",
"data": {
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
@@ -4,6 +4,18 @@
"type": "sequential", "type": "sequential",
"data": { "data": {
"commands": [ "commands": [
{
"type": "named",
"data": {
"name": "Intake Extended"
}
},
{
"type": "path",
"data": {
"pathName": "HubFarRight-NeutralZone"
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
@@ -13,13 +25,7 @@
{ {
"type": "path", "type": "path",
"data": { "data": {
"pathName": "Quick Shoot" "pathName": "HubFarLeft-Shoot"
}
},
{
"type": "path",
"data": {
"pathName": "Test - Robot Align"
} }
}, },
{ {
@@ -7,7 +7,13 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "Lidar Intake" "name": "Intake Extended"
}
},
{
"type": "path",
"data": {
"pathName": "HubFarRight-NeutralZone"
} }
} }
] ]
@@ -1,19 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Taxi"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
@@ -33,11 +33,11 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 0.5, "maxVelocity": 1.7,
"maxAcceleration": 3.0, "maxAcceleration": 2.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 500.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 700.0,
"nominalVoltage": 12.0, "nominalVoltage": 10.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": 180.0
}, },
"useDefaultConstraints": false "useDefaultConstraints": true
} }
@@ -8,20 +8,20 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.526567580029336, "x": 2.490418422348576,
"y": 4.248584319077823 "y": 4.249816319262193
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 2.5, "x": 2.115,
"y": 4.7 "y": 6.0
}, },
"prevControl": { "prevControl": {
"x": 2.5175324328959037, "x": 2.1584120444167327,
"y": 4.450615530161258 "y": 5.753798061746948
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,16 +33,16 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 0.5, "maxVelocity": 1.7,
"maxAcceleration": 3.0, "maxAcceleration": 2.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 500.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 700.0,
"nominalVoltage": 12.0, "nominalVoltage": 10.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": -178.85423716182484
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": 180.0
}, },
"useDefaultConstraints": false "useDefaultConstraints": true
} }
@@ -0,0 +1,97 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.115,
"y": 6.0
},
"prevControl": null,
"nextControl": {
"x": 1.9425952380952385,
"y": 6.874773809523809
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.485,
"y": 6.972
},
"prevControl": {
"x": 0.4543797151601425,
"y": 7.437592646883011
},
"nextControl": {
"x": 0.5156202848398574,
"y": 6.50640735311699
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.485,
"y": 4.985190476190476
},
"prevControl": {
"x": 0.5545595169162219,
"y": 4.745062440329646
},
"nextControl": {
"x": 0.36614285714285816,
"y": 5.395499999999999
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.3960952380952394,
"y": 5.179547619047619
},
"prevControl": {
"x": 2.136952380952382,
"y": 4.898809523809524
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 0.5,
"y": 4.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.2,
"maxWaypointRelativePos": 2.3115107913668993,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.7,
"maxAcceleration": 2.0,
"maxAngularVelocity": 500.0,
"maxAngularAcceleration": 700.0,
"nominalVoltage": 10.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 147.26477372789242
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
}
@@ -0,0 +1,81 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.115,
"y": 6.0
},
"prevControl": null,
"nextControl": {
"x": 1.8650000000000004,
"y": 6.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.7,
"y": 5.95697619047619
},
"prevControl": {
"x": 0.9480694691784167,
"y": 5.987984874123494
},
"nextControl": {
"x": 0.4519305308215832,
"y": 5.925967506828887
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.5256666666666674,
"y": 5.093166666666666
},
"prevControl": {
"x": 2.2773032851162216,
"y": 5.064607417642443
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [
{
"fieldPosition": {
"x": -0.1,
"y": 6.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.08778625954198889,
"maxWaypointRelativePos": 0.8328244274809142,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.7,
"maxAcceleration": 2.0,
"maxAngularVelocity": 500.0,
"maxAngularAcceleration": 700.0,
"nominalVoltage": 10.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 154.59228189105156
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
}
@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.62702380952381, "x": 4.0,
"y": 6.0 "y": 7.4
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 4.627023809523808, "x": 3.054750000000001,
"y": 6.0 "y": 7.565821428571429
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 2.180142857142858, "x": 2.4500833333333345,
"y": 6.0 "y": 5.363107142857143
}, },
"prevControl": { "prevControl": {
"x": 1.180142857142858, "x": 2.6268600286299715,
"y": 6.0 "y": 5.53988383815378
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,22 +33,22 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 1.0, "maxVelocity": 1.7,
"maxAcceleration": 3.0, "maxAcceleration": 2.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 500.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 700.0,
"nominalVoltage": 12.0, "nominalVoltage": 10.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 178.89829388479367 "rotation": 146.30993247402023
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 179.95980050207663 "rotation": 180.0
}, },
"useDefaultConstraints": false "useDefaultConstraints": true
} }
@@ -0,0 +1,117 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.0,
"y": 0.6661428571428576
},
"prevControl": null,
"nextControl": {
"x": 4.999999999999995,
"y": 0.6661428571428576
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.663,
"y": 0.8712976190476196
},
"prevControl": {
"x": 7.713544609817508,
"y": 0.5725975891884018
},
"nextControl": {
"x": 7.578952380952382,
"y": 1.367988095238095
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.53,
"y": 7.4
},
"prevControl": {
"x": 7.779738811179202,
"y": 7.388575194058578
},
"nextControl": {
"x": 6.471233966716803,
"y": 7.448435388999068
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 7.4
},
"prevControl": {
"x": 4.246995139395343,
"y": 7.438644548322825
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 6.75,
"y": 8.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.9056310679611663,
"maxWaypointRelativePos": 1.9091262135922324,
"name": "Point Towards Zone"
},
{
"fieldPosition": {
"x": 0.4,
"y": 7.4
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 1.9728155339805904,
"maxWaypointRelativePos": 2.55,
"name": "Point Towards Zone"
},
{
"fieldPosition": {
"x": 9.0,
"y": 1.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.025631067961170227,
"maxWaypointRelativePos": 0.6920388349514595,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.7,
"maxAcceleration": 2.0,
"maxAngularVelocity": 500.0,
"maxAngularAcceleration": 700.0,
"nominalVoltage": 10.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
+8 -6
View File
@@ -2,13 +2,15 @@
"robotWidth": 0.9, "robotWidth": 0.9,
"robotLength": 0.9, "robotLength": 0.9,
"holonomicMode": true, "holonomicMode": true,
"pathFolders": [], "pathFolders": [
"New Folder"
],
"autoFolders": [], "autoFolders": [],
"defaultMaxVel": 3.0, "defaultMaxVel": 1.7,
"defaultMaxAccel": 3.0, "defaultMaxAccel": 2.0,
"defaultMaxAngVel": 540.0, "defaultMaxAngVel": 500.0,
"defaultMaxAngAccel": 720.0, "defaultMaxAngAccel": 700.0,
"defaultNominalVoltage": 12.0, "defaultNominalVoltage": 10.0,
"robotMass": 74.088, "robotMass": 74.088,
"robotMOI": 6.883, "robotMOI": 6.883,
"robotTrackwidth": 0.546, "robotTrackwidth": 0.546,
@@ -96,7 +96,7 @@ public class RobotContainer {
private Command autoCommand; private Command autoCommand;
private Command RobotIntakeExtended = new SequentialCommandGroup( private Command IntakeExtended = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
); );
@@ -162,7 +162,8 @@ public class RobotContainer {
NamedCommands.registerCommand("Robot Intake Retracted", RobotIntakeRetracted); NamedCommands.registerCommand("Robot Intake Retracted", RobotIntakeRetracted);
NamedCommands.registerCommand("Robot Shoot", RobotShoot); NamedCommands.registerCommand("Robot Shoot", RobotShoot);
// NamedCommands.registerCommand("Lidar Intake", LidarIntake); // NamedCommands.registerCommand("Lidar Intake", LidarIntake);
NamedCommands.registerCommand("Robot Intake Extended", RobotIntakeExtended); NamedCommands.registerCommand("Intake Extended", IntakeExtended);
DriverStation.silenceJoystickConnectionWarning(true); DriverStation.silenceJoystickConnectionWarning(true);
@@ -344,7 +344,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90));
heading = heading.rotateBy(Rotation2d.fromDegrees(-90)); if(TimesNegativeOne.isRed) {
heading = heading.rotateBy(Rotation2d.fromDegrees(-90));
} else {
heading = heading.rotateBy(Rotation2d.fromDegrees(90));
}
rotTarget = heading.getDegrees(); rotTarget = heading.getDegrees();
driveFieldAngle(leftStick, heading); driveFieldAngle(leftStick, heading);