Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: Team_2648:073b2ab7541e993084bbd15dffdde7cb43799a5d
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup
...
compare: Team_2648:c9f6928806fd67ec822e5166b757f6878f616d73
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup

3 Commits
073b2ab754 ... c9f6928806

Author SHA1 Message Date
Team 2648
c9f6928806 progress on 3 piece auto 2025-03-22 20:47:47 -04:00
Team 2648
fdf837ab10 working 2.5 piece 2025-03-22 19:23:19 -04:00
Team 2648
be961d286b 2 piece left works 2025-03-22 19:19:09 -04:00
22 changed files with 247 additions and 101 deletions
Show Stats Expand all files Collapse all files

33
src/main/deploy/pathplanner/autos/Two Coral Left Event.auto → src/main/deploy/pathplanner/autos/2.5 Coral Left.auto
View File

@ -44,19 +44,7 @@
{ {
"type": "path", "type": "path",
"data": { "data": {
"pathName": "HP to 330 Left" "pathName": "HP to 330 Right"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "K Approach"
} }
}, },
{ {
@ -64,6 +52,25 @@
"data": { "data": {
"name": "Shoot Coral L4" "name": "Shoot Coral L4"
} }
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "L Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
} }
] ]
} }

57
src/main/deploy/pathplanner/autos/Two Coral Left.auto → src/main/deploy/pathplanner/autos/3 Coral Left.auto
View File

@ -10,18 +10,6 @@
"pathName": "Start to 30 Right" "pathName": "Start to 30 Right"
} }
}, },
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "J Approach"
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
@ -56,19 +44,7 @@
{ {
"type": "path", "type": "path",
"data": { "data": {
"pathName": "HP to 330 Left" "pathName": "HP to 330 Right"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "K Approach"
} }
}, },
{ {
@ -76,6 +52,37 @@
"data": { "data": {
"name": "Shoot Coral L4" "name": "Shoot Coral L4"
} }
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "L Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Collect Coral"
}
},
{
"type": "path",
"data": {
"pathName": "HP to K"
}
} }
] ]
} }

6
src/main/deploy/pathplanner/paths/30 Right to HP.path
View File

@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 5.0,
"maxAcceleration": 1.0, "maxAcceleration": 2.5,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": -120.06858282186238 "rotation": -120.06858282186238
}, },
"useDefaultConstraints": true "useDefaultConstraints": false
} }

6
src/main/deploy/pathplanner/paths/330 Right to HP.path
View File

@ -45,8 +45,8 @@
} }
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 5.0,
"maxAcceleration": 1.0, "maxAcceleration": 2.5,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@ -62,5 +62,5 @@
"velocity": 0, "velocity": 0,
"rotation": -59.99999999999999 "rotation": -59.99999999999999
}, },
"useDefaultConstraints": true "useDefaultConstraints": false
} }

4
src/main/deploy/pathplanner/paths/E to HP.path
View File

@ -52,8 +52,8 @@
} }
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/H Backup.path
View File

@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

66
src/main/deploy/pathplanner/paths/HP to 330 Right.path Normal file
View File

@ -0,0 +1,66 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
},
"prevControl": null,
"nextControl": {
"x": 2.490979395912868,
"y": 6.964368824388053
},
"isLocked": false,
"linkedName": "HP Left Position"
},
{
"anchor": {
"x": 3.6202868852459016,
"y": 5.031967213114754
},
"prevControl": {
"x": 3.304747501684258,
"y": 5.557866185717494
},
"nextControl": null,
"isLocked": false,
"linkedName": "L"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.0,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -59.69923999693802
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -53.97262661489646
},
"useDefaultConstraints": true
}

4
src/main/deploy/pathplanner/paths/HP to D.path
View File

@ -52,8 +52,8 @@
} }
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

66
src/main/deploy/pathplanner/paths/HP to K.path Normal file
View File

@ -0,0 +1,66 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
},
"prevControl": null,
"nextControl": {
"x": 1.7142418032786886,
"y": 6.638319672131147
},
"isLocked": false,
"linkedName": "HP Left Position"
},
{
"anchor": {
"x": 3.9679303278688525,
"y": 5.211782786885246
},
"prevControl": {
"x": 3.2246926229508195,
"y": 6.230737704918033
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.0,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -60.49491285058726
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -53.97262661489646
},
"useDefaultConstraints": true
}

4
src/main/deploy/pathplanner/paths/J Approach.path
View File

@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/J Backup.path
View File

@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

22
src/main/deploy/pathplanner/paths/K Approach.path
View File

@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.6202868852459016, "x": 3.35655737704918,
"y": 5.859118852459017 "y": 5.78719262295082
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 3.6663590190101236, "x": 3.402629510813402,
"y": 5.6134008057257505 "y": 5.541474576217554
}, },
"isLocked": false, "isLocked": false,
"linkedName": "Before K" "linkedName": "Before K"
}, },
{ {
"anchor": { "anchor": {
"x": 3.7290765411991873, "x": 3.6322745901639335,
"y": 5.091647695859483 "y": 5.019979508196721
}, },
"prevControl": { "prevControl": {
"x": 3.693113426445089, "x": 3.596311475409835,
"y": 5.343389499138171 "y": 5.271721311475409
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 1.0,
"maxAcceleration": 1.0, "maxAcceleration": 0.5,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": -59.69923999693802 "rotation": -59.69923999693802
}, },
"useDefaultConstraints": true "useDefaultConstraints": false
} }

30
src/main/deploy/pathplanner/paths/HP to 330 Left.path → src/main/deploy/pathplanner/paths/L Backup.path
View File

@ -3,29 +3,29 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.1987704918032787, "x": 3.6202868852459016,
"y": 7.189754098360656 "y": 5.031967213114754
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.490979395912868, "x": 3.4877871502451065,
"y": 6.964368824388053 "y": 5.243966789116026
}, },
"isLocked": false, "isLocked": false,
"linkedName": "HP Left Position" "linkedName": "L"
}, },
{ {
"anchor": { "anchor": {
"x": 3.6202868852459016, "x": 1.1987704918032787,
"y": 5.859118852459017 "y": 7.189754098360656
}, },
"prevControl": { "prevControl": {
"x": 3.304747501684258, "x": 1.4025614754098363,
"y": 6.385017825061756 "y": 6.914036885245903
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
"linkedName": "Before K" "linkedName": "HP Left Position"
} }
], ],
"rotationTargets": [], "rotationTargets": [],
@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 5.0,
"maxAcceleration": 1.0, "maxAcceleration": 2.5,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -59.69923999693802 "rotation": -53.97262661489646
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -53.97262661489646 "rotation": -59.69923999693802
}, },
"useDefaultConstraints": true "useDefaultConstraints": false
} }

4
src/main/deploy/pathplanner/paths/New Path.path
View File

@ -44,8 +44,8 @@
], ],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/Right Start to E.path
View File

@ -52,8 +52,8 @@
} }
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/Start to 30 Right.path
View File

@ -45,8 +45,8 @@
} }
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/Start to H.path
View File

@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/fein.path
View File

@ -49,8 +49,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 2.0, "maxVelocity": 3.5,
"maxAcceleration": 1.0, "maxAcceleration": 1.75,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/settings.json
View File

@ -4,8 +4,8 @@
"holonomicMode": true, "holonomicMode": true,
"pathFolders": [], "pathFolders": [],
"autoFolders": [], "autoFolders": [],
"defaultMaxVel": 2.0, "defaultMaxVel": 3.5,
"defaultMaxAccel": 1.0, "defaultMaxAccel": 1.75,
"defaultMaxAngVel": 540.0, "defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 400.0, "defaultMaxAngAccel": 400.0,
"defaultNominalVoltage": 12.0, "defaultNominalVoltage": 12.0,

6
src/main/java/frc/robot/RobotContainer.java
View File

@ -238,10 +238,10 @@ public class RobotContainer {
//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); //new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(0.5).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.01)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).until(() -> manipulator.getCoralBeamBreak() == false).andThen(manipulator.runManipulator(() -> 0, false)).withTimeout(0.1)); NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.01)));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position) NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
.andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.1))); .andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.01)));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)); NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
} }

6
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -12,12 +12,12 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants { public class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 5.5; public static final double kMaxSpeedMetersPerSecond = 5.5;
public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 4;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 3; public static final double kPXController = 3.5;
public static final double kPYController = 3; public static final double kPYController = 5;
public static final double kPThetaController = 5.5; public static final double kPThetaController = 5.5;
// Constraint for the motion profiled robot angle controller // Constraint for the motion profiled robot angle controller

6
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -174,11 +174,11 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees()); gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected()){ if(vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){ if(vision.getOrangeDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
} }
// if the detected tags match your alliances reef tags use their pose estimates // if the detected tags match your alliances reef tags use their pose estimates
@ -196,7 +196,7 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected()){ if(vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){ if(vision.getBlackDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
} }
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){