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

progress on 3 piece auto

Browse Source
This commit is contained in:
Team 2648 2025-03-22 20:47:47 -04:00
parent fdf837ab10
commit c9f6928806
9 changed files with 105 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

39
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"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "J Approach"
}
},
{
"type": "named",
"data": {
@ -62,19 +50,38 @@
{
"type": "named",
"data": {
"name": "Lift L4"
"name": "Shoot Coral L4"
}
},
{
"type": "path",
"type": "parallel",
"data": {
"pathName": "K Approach"
"commands": [
{
"type": "path",
"data": {
"pathName": "L Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
"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": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxVelocity": 5.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -50,5 +50,5 @@
"velocity": 0,
"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": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxVelocity": 5.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -62,5 +62,5 @@
"velocity": 0,
"rotation": -59.99999999999999
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

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
}

6
src/main/deploy/pathplanner/paths/L Backup.path
View File

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

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));
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("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.1)));
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).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.01)));
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));
}

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 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 kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 3;
public static final double kPYController = 3;
public static final double kPXController = 3.5;
public static final double kPYController = 5;
public static final double kPThetaController = 5.5;
// Constraint for the motion profiled robot angle controller

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

@ -174,7 +174,7 @@ public class Drivetrain extends SubsystemBase {
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.getOrangeDist() < 60){