Compare commits
12 Commits
maybe_fix_
...
dd50663b9e
| Author | SHA1 | Date | |
|---|---|---|---|
| dd50663b9e | |||
| a089dddae3 | |||
| 66a9608006 | |||
| 649660ade6 | |||
| 445ce9bf6f | |||
| 7c446fd874 | |||
| 2ae2beddfa | |||
| 80b5908206 | |||
| aecc342dc4 | |||
| 05e9202592 | |||
| 868e096c02 | |||
| 87c0772982 |
@@ -1,13 +1,49 @@
|
||||
{
|
||||
"Clients": {
|
||||
"open": true
|
||||
},
|
||||
"Connections": {
|
||||
"open": true
|
||||
},
|
||||
"NetworkTables Settings": {
|
||||
"mode": "Client (NT4)"
|
||||
},
|
||||
"client@2": {
|
||||
"open": true
|
||||
},
|
||||
"client@4": {
|
||||
"Publishers": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"client@5": {
|
||||
"Publishers": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"outlineviewer@2": {
|
||||
"Publishers": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"outlineviewer@3": {
|
||||
"open": true
|
||||
},
|
||||
"shuffleboard@1": {
|
||||
"open": true
|
||||
},
|
||||
"transitory": {
|
||||
"Shuffleboard": {
|
||||
"Sensors Tab": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"orange_Fiducial": {
|
||||
"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;
|
||||
@@ -52,8 +53,6 @@ public class RobotContainer {
|
||||
|
||||
private SendableChooser<Command> autoChooser;
|
||||
|
||||
private Vision vision;
|
||||
|
||||
private IntSupplier closestTag;
|
||||
|
||||
public RobotContainer() {
|
||||
@@ -63,8 +62,6 @@ public class RobotContainer {
|
||||
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
vision = new Vision(drivetrain::getGyroValue);
|
||||
|
||||
elevator = new Elevator();
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
@@ -109,8 +106,8 @@ public class RobotContainer {
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
() -> Math.pow(driver.getLeftY(), 3),
|
||||
() -> Math.pow(driver.getLeftX(), 3),
|
||||
() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
|
||||
() -> true
|
||||
)
|
||||
@@ -143,7 +140,7 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
driver.leftTrigger().whileTrue(
|
||||
manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
|
||||
manipulator.runUntilCollected(() -> 0.75)
|
||||
);
|
||||
|
||||
driver.start().and(driver.back()).onTrue(
|
||||
@@ -153,13 +150,16 @@ public class RobotContainer {
|
||||
driver.y().whileTrue(drivetrain.zeroHeading());
|
||||
|
||||
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
|
||||
|
||||
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
|
||||
|
||||
/*
|
||||
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(
|
||||
@@ -201,46 +200,48 @@ public class RobotContainer {
|
||||
)
|
||||
);
|
||||
|
||||
operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
|
||||
operator.back().onTrue(elevator.homeCommand());
|
||||
|
||||
operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
|
||||
|
||||
operator.a().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
|
||||
safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
|
||||
);
|
||||
|
||||
operator.x().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
);
|
||||
|
||||
operator.b().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
);
|
||||
|
||||
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
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
|
||||
private void configureShuffleboard() {
|
||||
ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
|
||||
ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
|
||||
ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
|
||||
|
||||
Shuffleboard.selectTab(OIConstants.kAutoTab);
|
||||
|
||||
@@ -316,6 +317,17 @@ public class RobotContainer {
|
||||
|
||||
//sensorTab.add("odometry", drivetrain::getPose);
|
||||
|
||||
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag());
|
||||
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX());
|
||||
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY());
|
||||
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);
|
||||
|
||||
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -29,10 +29,10 @@ public class DrivetrainConstants {
|
||||
public static final double kBackRightChassisAngularOffset = 0;
|
||||
*/
|
||||
|
||||
public static final double kFrontLeftChassisAngularOffset = 0;
|
||||
public static final double kFrontRightChassisAngularOffset = Math.PI / 2;
|
||||
public static final double kBackLeftChassisAngularOffset = -Math.PI / 2;
|
||||
public static final double kBackRightChassisAngularOffset = Math.PI;
|
||||
public static final double kFrontLeftChassisAngularOffset = Math.PI;
|
||||
public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
|
||||
public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
|
||||
public static final double kBackRightChassisAngularOffset = 0;
|
||||
|
||||
// 1, 7, 10 is the default for these three values
|
||||
public static final double kSysIDDrivingRampRate = 1;
|
||||
@@ -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
|
||||
|
||||
@@ -43,8 +43,8 @@ public class ElevatorConstants {
|
||||
|
||||
public static final double kCoralIntakePosition = 0;
|
||||
public static final double kL1Position = 0;
|
||||
public static final double kL2Position = 9;
|
||||
public static final double kL3Position = 23.0;
|
||||
public static final double kL2Position = 11;
|
||||
public static final double kL3Position = 27;
|
||||
public static final double kL4Position = 50.5;
|
||||
public static final double kL4TransitionPosition = 40.0;
|
||||
public static final double kL2AlgaePosition = 23.0;
|
||||
|
||||
@@ -29,26 +29,27 @@ public class ManipulatorPivotConstants {
|
||||
public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
|
||||
public static final double kFeedForwardV = 0.68; //calculated value 0.68
|
||||
|
||||
public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
|
||||
public static final double kFFGravityOffset = Units.degreesToRadians(135.0+90);
|
||||
|
||||
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
|
||||
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
|
||||
|
||||
public static final double kEncoderOffset = 0.780;
|
||||
public static final double kEncoderOffset = 0.78-0.25;
|
||||
|
||||
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kL1Position = Units.degreesToRadians(0.0);
|
||||
public static final double kL2Position = Units.degreesToRadians(22.0);
|
||||
public static final double kL3Position = Units.degreesToRadians(22.0);
|
||||
public static final double kL4Position = Units.degreesToRadians(45.0);
|
||||
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kProcessorPosition = Units.degreesToRadians(175.0);
|
||||
public static final double kNetPosition = Units.degreesToRadians(175.0);
|
||||
public static final double kStartingPosition = Units.degreesToRadians(90);
|
||||
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
|
||||
public static final double kL1Position = Units.degreesToRadians(0.0+90);
|
||||
public static final double kL2Position = Units.degreesToRadians(22.0+90);
|
||||
public static final double kL3Position = Units.degreesToRadians(22.0+90);
|
||||
public static final double kL4Position = Units.degreesToRadians(45.0+90);
|
||||
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90);
|
||||
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90);
|
||||
public static final double kProcessorPosition = Units.degreesToRadians(175.0+90);
|
||||
public static final double kNetPosition = Units.degreesToRadians(175.0+90);
|
||||
/**The closest position to the elevator brace without hitting it */
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
|
||||
/**The forward rotation limit of the pivot */
|
||||
public static final double kRotationLimit = Units.degreesToRadians(175.0);
|
||||
public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
|
||||
|
||||
public static final double kSysIDRampRate = 1;
|
||||
public static final double kSysIDStepVolts = 7;
|
||||
|
||||
@@ -48,7 +48,7 @@ public class ModuleConstants {
|
||||
|
||||
public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
|
||||
|
||||
public static final InvertedValue kDriveInversionState = InvertedValue.CounterClockwise_Positive;
|
||||
public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
|
||||
public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
@@ -4,8 +4,9 @@ public class OIConstants {
|
||||
public static final int kDriverControllerPort = 0;
|
||||
public static final int kOperatorControllerPort = 1;
|
||||
|
||||
public static final double kDriveDeadband = 0.05;
|
||||
public static final double kDriveDeadband = Math.pow(0.05, 3);
|
||||
|
||||
public static final String kAutoTab = "Auto Tab";
|
||||
public static final String kSensorsTab = "Sensors Tab";
|
||||
public static final String kApriltagTab = "Apriltag Tab";
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
|
||||
@@ -9,21 +9,28 @@ import java.util.Optional;
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.Orchestra;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.studica.frc.AHRS;
|
||||
import com.studica.frc.AHRS.NavXComType;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
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;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
@@ -32,6 +39,7 @@ import frc.robot.constants.AutoConstants;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
|
||||
public class Drivetrain extends SubsystemBase {
|
||||
// Create MAXSwerveModules
|
||||
@@ -45,6 +53,8 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
private SwerveDrivePoseEstimator m_estimator;
|
||||
|
||||
private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
|
||||
|
||||
public Orchestra m_orchestra = new Orchestra();
|
||||
private Timer musicTimer = new Timer();
|
||||
@@ -53,6 +63,11 @@ public class Drivetrain extends SubsystemBase {
|
||||
private PIDController pidTranslationX;
|
||||
private PIDController pidTranslationY;
|
||||
|
||||
public Vision vision;
|
||||
|
||||
public Pose2d orangePose2d;
|
||||
public Pose2d blackPose2d;
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public Drivetrain() {
|
||||
m_frontLeft = new MAXSwerveModule(
|
||||
@@ -80,6 +95,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
);
|
||||
|
||||
gyro = new AHRS(NavXComType.kMXP_SPI);
|
||||
gyro.reset();
|
||||
|
||||
m_estimator = new SwerveDrivePoseEstimator(
|
||||
DrivetrainConstants.kDriveKinematics,
|
||||
@@ -90,14 +106,19 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
new Pose2d()
|
||||
new Pose2d(),
|
||||
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
|
||||
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
|
||||
);
|
||||
|
||||
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
|
||||
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,
|
||||
@@ -130,6 +151,10 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_orchestra.play();
|
||||
musicTimer.reset();
|
||||
musicTimer.start();
|
||||
|
||||
vision = new Vision();
|
||||
orangePose2d = new Pose2d();
|
||||
blackPose2d = new Pose2d();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -144,24 +169,47 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
|
||||
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
|
||||
|
||||
// if the detected tags match your alliances reef tags use their pose estimates
|
||||
/*
|
||||
if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
|
||||
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
|
||||
|
||||
}else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
|
||||
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
|
||||
|
||||
if(vision.getOrangeTagDetected()){
|
||||
if(vision.getOrangeDist() < 60){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
|
||||
}
|
||||
|
||||
// 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()){
|
||||
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
|
||||
|
||||
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
|
||||
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
|
||||
}
|
||||
Logger.recordOutput("orange pose", new Pose3d());
|
||||
}
|
||||
|
||||
if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
|
||||
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
|
||||
}else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
|
||||
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
|
||||
if(vision.getBlackTagDetected()){
|
||||
if(vision.getBlackDist() < 60){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
|
||||
}
|
||||
|
||||
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
|
||||
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
|
||||
|
||||
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
|
||||
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
|
||||
}
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
|
||||
}
|
||||
*/
|
||||
|
||||
if(musicTimer.get()>20){
|
||||
|
||||
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
|
||||
|
||||
if(musicTimer.get()>10){
|
||||
if (m_orchestra.isPlaying()) {
|
||||
m_orchestra.stop();
|
||||
}
|
||||
@@ -211,12 +259,20 @@ public class Drivetrain extends SubsystemBase {
|
||||
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
|
||||
BooleanSupplier fieldRelative) {
|
||||
return run(() -> {
|
||||
drive(
|
||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
fieldRelative.getAsBoolean()
|
||||
);
|
||||
if(DriverStation.getAlliance().get().equals(Alliance.Blue)){
|
||||
drive(
|
||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
fieldRelative.getAsBoolean()
|
||||
);
|
||||
}else{
|
||||
drive(
|
||||
MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
fieldRelative.getAsBoolean()
|
||||
);}
|
||||
});
|
||||
}
|
||||
|
||||
@@ -256,23 +312,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;
|
||||
@@ -282,12 +342,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;
|
||||
@@ -335,6 +395,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
public Command zeroHeading() {
|
||||
return run(() -> {
|
||||
gyro.reset();
|
||||
m_estimator.resetRotation(new Rotation2d(0));
|
||||
});
|
||||
}
|
||||
|
||||
@@ -351,6 +412,10 @@ public class Drivetrain extends SubsystemBase {
|
||||
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
|
||||
}
|
||||
|
||||
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
|
||||
return gyroBuffer;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
@@ -360,10 +425,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
|
||||
public void addVisionMeasurement(Pose2d pose, double timestamp){
|
||||
m_estimator.addVisionMeasurement(pose, timestamp);
|
||||
}
|
||||
|
||||
public double driveDistance(){
|
||||
return m_frontLeft.getTotalDist();
|
||||
}
|
||||
@@ -371,4 +432,10 @@ public class Drivetrain extends SubsystemBase {
|
||||
public double getVelocity(){
|
||||
return m_frontLeft.getState().speedMetersPerSecond;
|
||||
}
|
||||
|
||||
public Command resetToVision(){
|
||||
return run(() -> {
|
||||
m_estimator.resetPose(orangePose2d);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
@@ -174,6 +174,14 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
}
|
||||
|
||||
public Command homeCommand(){
|
||||
return run(() -> {
|
||||
elevatorMotor1.setVoltage(0.5);
|
||||
})
|
||||
.until(() -> elevatorMotor1.getOutputCurrent() > 5)
|
||||
.andThen(run(() -> encoder.setPosition(0)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the elevator to a target destination (setpoint).
|
||||
*
|
||||
@@ -208,7 +216,7 @@ public class Elevator extends SubsystemBase {
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
|
||||
.andThen(runManualElevator(() -> -.1)
|
||||
.andThen(runManualElevator(() -> -.5)
|
||||
.until(() -> encoder.getPosition() == 0));
|
||||
|
||||
} else {
|
||||
|
||||
@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
);
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidController.enableContinuousInput(0, 180);
|
||||
pidController.enableContinuousInput(0, 280);
|
||||
|
||||
feedForward = new ArmFeedforward(
|
||||
ManipulatorPivotConstants.kFeedForwardS,
|
||||
|
||||
@@ -1,93 +1,125 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.networktables.BooleanSubscriber;
|
||||
import edu.wpi.first.networktables.DoubleSubscriber;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
|
||||
public class Vision{
|
||||
|
||||
private DoubleSubscriber blackRobotRelativeX;
|
||||
private DoubleSubscriber blackRobotRelativeY;
|
||||
private DoubleSubscriber blackRobotRelativeZ;
|
||||
private NetworkTable blackVisionTable;
|
||||
|
||||
private DoubleSubscriber black_tx;
|
||||
private DoubleSubscriber black_ty;
|
||||
private DoubleSubscriber black_dist;
|
||||
|
||||
private DoubleSubscriber blackClosestTag;
|
||||
private BooleanSubscriber blackTagDetected;
|
||||
|
||||
private DoubleSubscriber blackFramerate;
|
||||
|
||||
private DoubleSubscriber orangeRobotRelativeX;
|
||||
private DoubleSubscriber orangeRobotRelativeY;
|
||||
private DoubleSubscriber orangeRobotRelativeZ;
|
||||
private NetworkTable orangeVisionTable;
|
||||
|
||||
private DoubleSubscriber orange_tx;
|
||||
private DoubleSubscriber orange_ty;
|
||||
private DoubleSubscriber orange_dist;
|
||||
|
||||
private DoubleSubscriber orangeClosestTag;
|
||||
private BooleanSubscriber orangeTagDetected;
|
||||
|
||||
private DoubleSubscriber orangeFramerate;
|
||||
|
||||
private DoubleSupplier gyroAngle;
|
||||
private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 14.0-1.05, 7.308+2.75};
|
||||
private double[] blackCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
|
||||
|
||||
public Vision(DoubleSupplier gyroAngle){
|
||||
public Vision(){
|
||||
NetworkTableInstance inst = NetworkTableInstance.getDefault();
|
||||
|
||||
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
|
||||
NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
|
||||
blackVisionTable = inst.getTable("black_Fiducial");
|
||||
orangeVisionTable = inst.getTable("orange_Fiducial");
|
||||
|
||||
blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
|
||||
blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
|
||||
blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
|
||||
black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
|
||||
black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
|
||||
black_dist = blackVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
|
||||
|
||||
blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
|
||||
blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
|
||||
|
||||
blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
|
||||
|
||||
orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
|
||||
orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
|
||||
orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
|
||||
orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
|
||||
orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
|
||||
orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
|
||||
|
||||
orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
|
||||
orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
|
||||
|
||||
orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
|
||||
|
||||
}
|
||||
|
||||
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
|
||||
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
|
||||
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
|
||||
VisionConstants.globalTagCoords[tagID][1],
|
||||
new Rotation2d());
|
||||
|
||||
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
|
||||
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
|
||||
|
||||
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
|
||||
|
||||
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
|
||||
|
||||
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
|
||||
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
|
||||
}
|
||||
|
||||
public Pose2d getBlackGlobalPose(){
|
||||
return relativeToGlobalPose2d(getBlackClosestTag(),
|
||||
new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
|
||||
public Pose2d cameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
|
||||
|
||||
// System.out.println(gyroBuffer.getSample(timestamp));
|
||||
|
||||
double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-Units.degreesToRadians( camPose[1]) + Units.degreesToRadians(ty));
|
||||
|
||||
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(Units.degreesToRadians(camPose[2])).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
|
||||
|
||||
Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
|
||||
Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
|
||||
new Rotation2d());
|
||||
|
||||
Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
|
||||
.transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
|
||||
.getTranslation();
|
||||
Pose2d robotPose = new Pose2d(
|
||||
fieldToCameraTranslation,
|
||||
new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(Units.degreesToRadians( camPose[2]))))
|
||||
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(Units.degreesToRadians(camPose[2]))), Pose2d.kZero));
|
||||
|
||||
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
|
||||
|
||||
return robotPose;
|
||||
}
|
||||
|
||||
public double getBlackRelativeX(){
|
||||
return blackRobotRelativeX.get();
|
||||
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
|
||||
return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
|
||||
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
|
||||
}
|
||||
|
||||
public double getBlackRelativeY(){
|
||||
return blackRobotRelativeY.get();
|
||||
public double getBlackTX(){
|
||||
return black_tx.get();
|
||||
}
|
||||
|
||||
public double getBlackRelativeZ(){
|
||||
return blackRobotRelativeZ.get();
|
||||
public double getBlackTY(){
|
||||
return black_ty.get();
|
||||
}
|
||||
|
||||
public double getBlackDist(){
|
||||
return black_dist.get();
|
||||
}
|
||||
|
||||
public int getBlackClosestTag(){
|
||||
@@ -95,7 +127,7 @@ public class Vision{
|
||||
}
|
||||
|
||||
public double getBlackTimeStamp(){
|
||||
return blackRobotRelativeX.getLastChange();
|
||||
return black_tx.getLastChange()-VisionConstants.latencyFudge;
|
||||
}
|
||||
|
||||
public boolean getBlackTagDetected(){
|
||||
@@ -106,21 +138,26 @@ public class Vision{
|
||||
return blackFramerate.get();
|
||||
}
|
||||
|
||||
public Pose2d getOrangeGlobalPose(){
|
||||
return relativeToGlobalPose2d(getOrangeClosestTag(),
|
||||
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
|
||||
public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
|
||||
if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
|
||||
return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
|
||||
orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
|
||||
);
|
||||
}else{
|
||||
return new Pose2d();
|
||||
}
|
||||
}
|
||||
|
||||
public double getOrangeRelativeX(){
|
||||
return orangeRobotRelativeX.get();
|
||||
public double getOrangeTX(){
|
||||
return orange_tx.get();
|
||||
}
|
||||
|
||||
public double getOrangeRelativeY(){
|
||||
return orangeRobotRelativeY.get();
|
||||
public double getOrangeTY(){
|
||||
return orange_ty.get();
|
||||
}
|
||||
|
||||
public double getOrangeRelativeZ(){
|
||||
return orangeRobotRelativeZ.get();
|
||||
public double getOrangeDist(){
|
||||
return orange_dist.get();
|
||||
}
|
||||
|
||||
public int getOrangeClosestTag(){
|
||||
@@ -128,7 +165,7 @@ public class Vision{
|
||||
}
|
||||
|
||||
public double getOrangeTimeStamp(){
|
||||
return orangeRobotRelativeX.getLastChange();
|
||||
return orange_tx.getLastChange()-VisionConstants.latencyFudge;
|
||||
}
|
||||
|
||||
public boolean getOrangeTagDetected(){
|
||||
|
||||
Reference in New Issue
Block a user