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

auto align setpoints

Browse Source
This commit is contained in:
Tylr-J42 2025-02-24 12:02:45 -05:00
parent d934cdf35b
commit 2990b917e7
5 changed files with 55 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 3.4559075342465753,
"y": 3.123458904109589
"x": 12.272,
"y": 2.975
},
"prevControl": {
"x": 2.4559075342465753,
"y": 3.123458904109589
"x": 10.940715672291898,
"y": 2.975
},
"nextControl": null,
"isLocked": false,
@ -30,7 +30,18 @@
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 0.4,
"y": 5.5
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.15,
"maxWaypointRelativePos": 0.4,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
@ -42,7 +53,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 57.80426606528677
"rotation": 59.99999999999999
},
"reversed": false,
"folder": null,

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

@ -9,7 +9,7 @@
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 48.35,
"robotMass": 53.0,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.038,

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

@ -65,12 +65,6 @@ public class RobotContainer {
manipulatorPivot = new ManipulatorPivot();
//commands for pathplanner
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
@ -147,7 +141,7 @@ public class RobotContainer {
);
driver.leftTrigger().whileTrue(
manipulator.runUntilCollected(() -> 0.35)
manipulator.runUntilCollected(() -> 0.35).andThen(manipulator.retractCommand(() -> 0.35))
);
driver.start().and(driver.back()).onTrue(
@ -210,6 +204,10 @@ public class RobotContainer {
private void configureNamedCommands() {
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
}
//creates tabs and transforms them on the shuffleboard

25
src/main/java/frc/robot/constants/VisionConstants.java
View File

@ -1,6 +1,9 @@
package frc.robot.constants;
import java.util.HashMap;
public class VisionConstants {
// global coordinate map of all tags. index is the tag id.
// Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
// This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
@ -28,4 +31,26 @@ public class VisionConstants {
{209.49, 158.3, 12.13, 0, 0},
{193.1, 129.97, 12.13, 300, 0},
};
//map of coral placing setpoints based on the tag that is on the same reef face
// and the on the left or right branch of that side of the reef
// <tag_number, {left_x, left_y, right_x, right_y}>
public HashMap<Integer, double[]> blueReefMap = new HashMap<>();
public HashMap<Integer, double[]> redReefMap = new HashMap<>();
VisionConstants(){
blueReefMap.put(17, new double[] {3.703, 3.975, 3.982, 2.806});
blueReefMap.put(18, new double[] {3.183, 4.191, 3.183, 3.857});
blueReefMap.put(19, new double[] {3.986, 5.24, 3.701, 5.076});
blueReefMap.put(20, new double[] {5.275, 5.075, 4.991, 5.246});
blueReefMap.put(21, new double[] {5.789, 3.862, 5.789, 4.194});
blueReefMap.put(22, new double[] {4.993, 2.816, 5.272, 2.996});
blueReefMap.put(11, new double[] {3.703+12.272, 3.975, 3.982+12.272, 2.806});
blueReefMap.put(10, new double[] {3.183+12.272, 4.191, 3.183, 3.857});
blueReefMap.put(9, new double[] {3.986+12.272, 5.24, 3.701+12.272, 5.076});
blueReefMap.put(8, new double[] {5.275+12.272, 5.075, 4.991+12.272, 5.246});
blueReefMap.put(7, new double[] {5.789+12.272, 3.862, 5.789+12.272, 4.194});
blueReefMap.put(6, new double[] {4.993+12.272, 2.816, 5.272+12.272, 2.996});
}
}

7
src/main/java/frc/robot/subsystems/Manipulator.java
View File

@ -74,6 +74,13 @@ public class Manipulator extends SubsystemBase {
}).until(() -> !coralBeamBreak.get());
}
public Command retractCommand(DoubleSupplier speed){
return run(() -> {
manipulatorMotor.set(-speed.getAsDouble());
}
).until(() -> coralBeamBreak.get());
}
/**
* Runs the manipulator in a way that will bring the coral to a reliable holding position
*