two coral auto path good, beak break bad
This commit is contained in:
parent
649660ade6
commit
a089dddae3
@ -2,6 +2,9 @@
|
||||
"Clients": {
|
||||
"open": true
|
||||
},
|
||||
"Connections": {
|
||||
"open": true
|
||||
},
|
||||
"NetworkTables Settings": {
|
||||
"mode": "Client (NT4)"
|
||||
},
|
||||
@ -14,6 +17,12 @@
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"client@5": {
|
||||
"Publishers": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"outlineviewer@2": {
|
||||
"Publishers": {
|
||||
"open": true
|
||||
|
||||
@ -10,6 +10,18 @@
|
||||
"pathName": "Start to 30 Right"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "J Approach"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
@ -21,6 +33,12 @@
|
||||
"data": {
|
||||
"pathName": "J Backup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@ -13,13 +13,38 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
"name": "Lift L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "30 Right to HP"
|
||||
"pathName": "J Approach"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "30 Right to HP"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
@ -34,6 +59,18 @@
|
||||
"pathName": "HP to 330 Left"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "K Approach"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
|
||||
@ -34,7 +34,7 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -16,12 +16,12 @@
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.3072345890410957,
|
||||
"y": 7.135316780821918
|
||||
"x": 1.174795081967213,
|
||||
"y": 7.321618852459016
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.1636986301369863,
|
||||
"y": 6.188698630136986
|
||||
"x": 2.031259123063103,
|
||||
"y": 6.375000701774084
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@ -31,17 +31,10 @@
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "HP Pickup",
|
||||
"waypointRelativePos": 0.20476190476190542,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -46,7 +46,7 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -34,7 +34,7 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -3,45 +3,38 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.3072345890410957,
|
||||
"y": 7.135316780821918
|
||||
"x": 1.174795081967213,
|
||||
"y": 7.321618852459016
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.5994434931506847,
|
||||
"y": 6.909931506849315
|
||||
"x": 2.4670039860768025,
|
||||
"y": 7.096233578486413
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Left Position"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.973766447368421,
|
||||
"y": 5.246957236842105
|
||||
"x": 3.6202868852459016,
|
||||
"y": 5.859118852459017
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.6582270638067773,
|
||||
"y": 5.772856209444845
|
||||
"x": 3.304747501684258,
|
||||
"y": 6.385017825061756
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "Before K"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.5,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
54
src/main/deploy/pathplanner/paths/J Approach.path
Normal file
54
src/main/deploy/pathplanner/paths/J Approach.path
Normal file
@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.286577868852459,
|
||||
"y": 5.955020491803278
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.166700819672132,
|
||||
"y": 5.679303278688525
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Before J"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.974897540983607,
|
||||
"y": 5.115881147540984
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.058811475409836,
|
||||
"y": 5.403586065573771
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "J"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -119.71497744813712
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -119.71497744813712
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@ -3,13 +3,13 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.988527397260274,
|
||||
"y": 5.227054794520548
|
||||
"x": 4.974897540983607,
|
||||
"y": 5.115881147540984
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.1321837955756875,
|
||||
"y": 5.49468698033176
|
||||
"x": 5.11855393929902,
|
||||
"y": 5.383513333352196
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "J"
|
||||
@ -34,7 +34,7 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@ -48,7 +48,7 @@
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -121.60750224624898
|
||||
"rotation": -119.71497744813712
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/K Approach.path
Normal file
54
src/main/deploy/pathplanner/paths/K Approach.path
Normal file
@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6202868852459016,
|
||||
"y": 5.859118852459017
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.7970635805425386,
|
||||
"y": 5.68234215716238
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Before K"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.8720286885245896,
|
||||
"y": 5.043954918032787
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.836065573770491,
|
||||
"y": 5.295696721311475
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -60.94539590092286
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.69923999693802
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@ -45,7 +45,7 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -3,45 +3,38 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.587970890410959,
|
||||
"y": 6.143621575342466
|
||||
"x": 7.588217213114754,
|
||||
"y": 7.537397540983607
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.385916095890411,
|
||||
"y": 6.158647260273973
|
||||
"x": 6.3534836065573765,
|
||||
"y": 6.51844262295082
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.988527397260274,
|
||||
"y": 5.227054794520548
|
||||
"x": 5.286577868852459,
|
||||
"y": 5.955020491803278
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.574529109589041,
|
||||
"y": 6.098544520547945
|
||||
"x": 5.872579581181226,
|
||||
"y": 6.826510217830675
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "J"
|
||||
"linkedName": "Before J"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.4547619047619047,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@ -49,7 +42,7 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -121.60750224624898
|
||||
"rotation": -119.71497744813712
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -50,7 +50,7 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
"pathFolders": [],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 4.0,
|
||||
"defaultMaxAccel": 1.0,
|
||||
"defaultMaxAccel": 2.5,
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 400.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
|
||||
@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.NamedCommands;
|
||||
import com.pathplanner.lib.events.EventTrigger;
|
||||
import com.pathplanner.lib.path.EventMarker;
|
||||
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
@ -154,12 +155,11 @@ public class RobotContainer {
|
||||
|
||||
driver.start().whileTrue(drivetrain.resetToVision());
|
||||
|
||||
/*
|
||||
driver.rightBumper().whileTrue(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
|
||||
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
|
||||
() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
|
||||
)
|
||||
);
|
||||
|
||||
@ -167,10 +167,9 @@ public class RobotContainer {
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
|
||||
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
|
||||
() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
|
||||
)
|
||||
);
|
||||
*/
|
||||
|
||||
//Operator inputs
|
||||
operator.povUp().onTrue(
|
||||
@ -227,16 +226,15 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
private void configureNamedCommands() {
|
||||
new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
new EventTrigger("Lift L4").whileTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
|
||||
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
||||
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
|
||||
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
|
||||
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
|
||||
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
|
||||
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
|
||||
}
|
||||
|
||||
//creates tabs and transforms them on the shuffleboard
|
||||
@ -325,7 +323,7 @@ public class RobotContainer {
|
||||
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist());
|
||||
apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
|
||||
apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
|
||||
|
||||
apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
|
||||
|
||||
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
|
||||
|
||||
|
||||
@ -12,7 +12,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
public class AutoConstants {
|
||||
public static final double kMaxSpeedMetersPerSecond = 4;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 1;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 2;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
||||
|
||||
|
||||
@ -57,10 +57,10 @@ public class DrivetrainConstants {
|
||||
|
||||
public static final boolean kGyroReversed = true;
|
||||
|
||||
public static final double kHeadingP = 0.0;
|
||||
public static final double kHeadingP = 0.01;
|
||||
|
||||
public static final double kXTranslationP = 0.0;
|
||||
public static final double kYTranslationP = 0.0;
|
||||
public static final double kXTranslationP = 0.5;
|
||||
public static final double kYTranslationP = 0.5;
|
||||
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
@ -40,12 +40,12 @@ public class VisionConstants {
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{4.993+12.272, 2.816, 5.272+12.272, 2.996},//6
|
||||
{5.789+12.272, 3.862, 5.789+12.272, 4.194},
|
||||
{5.275+12.272, 5.075, 4.991+12.272, 5.246},
|
||||
{3.986+12.272, 5.24, 3.701+12.272, 5.076},
|
||||
{3.183+12.272, 4.191, 3.183, 3.857},
|
||||
{3.703+12.272, 3.975, 3.982+12.272, 2.806},//11
|
||||
{13.570, 2.816, 13.858, 2.970},//6
|
||||
{14.373, 3.862, 14.385, 4.194},
|
||||
{13.858, 5.032, 13.558, 5.227},
|
||||
{12.575, 5.227, 12.287, 5.056},
|
||||
{11.772, 4.169, 11.772, 3.845},
|
||||
{12.287, 2.982, 12.587, 2.826},//11
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
@ -59,4 +59,6 @@ public class VisionConstants {
|
||||
{4.993, 2.816, 5.272, 2.996}
|
||||
};
|
||||
|
||||
public static final double latencyFudge = 0.080;
|
||||
|
||||
}
|
||||
|
||||
@ -22,6 +22,7 @@ import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
@ -112,7 +113,10 @@ public class Drivetrain extends SubsystemBase {
|
||||
pidHeading.enableContinuousInput(-180, 180);
|
||||
|
||||
pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
|
||||
pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
|
||||
pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
|
||||
pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
|
||||
|
||||
|
||||
AutoBuilder.configure(
|
||||
this::getPose,
|
||||
@ -164,6 +168,11 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
|
||||
|
||||
// if(vision.getOrangeDist() < 72){
|
||||
// m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
|
||||
//}
|
||||
|
||||
/*
|
||||
if(vision.getOrangeTagDetected()){
|
||||
// if the detected tags match your alliances reef tags use their pose estimates
|
||||
if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
|
||||
@ -194,6 +203,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose));
|
||||
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
|
||||
}
|
||||
*/
|
||||
|
||||
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
|
||||
|
||||
@ -300,23 +310,27 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
|
||||
return run(() -> {
|
||||
drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()),
|
||||
pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()),
|
||||
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
|
||||
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1),
|
||||
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
|
||||
true);
|
||||
|
||||
Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
|
||||
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
|
||||
new Rotation2d(headingSetpoint.getAsDouble()))));
|
||||
});
|
||||
}
|
||||
|
||||
public int getClosestTag(){
|
||||
|
||||
if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
|
||||
if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
|
||||
int closestTag = 17;
|
||||
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
|
||||
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][0]), 2)
|
||||
+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][1]), 2));
|
||||
|
||||
for(int i = 17; i <= 22; ++i){
|
||||
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
|
||||
double distance = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][0]), 2)
|
||||
+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
|
||||
|
||||
if(distance < closestTagDist){
|
||||
closestTag = i;
|
||||
@ -326,12 +340,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
return closestTag;
|
||||
}else{
|
||||
int closestTag = 6;
|
||||
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
|
||||
double closestTagDist = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][0]), 2)
|
||||
+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][1]), 2));
|
||||
|
||||
for(int i = 6; i <= 11; ++i){
|
||||
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
|
||||
double distance = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters( VisionConstants.globalTagCoords[i][0]), 2)
|
||||
+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
|
||||
|
||||
if(distance < closestTagDist){
|
||||
closestTag = i;
|
||||
|
||||
@ -161,7 +161,7 @@ public class Vision{
|
||||
}
|
||||
|
||||
public double getOrangeTimeStamp(){
|
||||
return orange_tx.getLastChange();
|
||||
return orange_tx.getLastChange()-VisionConstants.latencyFudge;
|
||||
}
|
||||
|
||||
public boolean getOrangeTagDetected(){
|
||||
|
||||
Loading…
Reference in New Issue
Block a user