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:maybe_fix_field_oriented
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

1 Commits
073b2ab754 ... maybe_fix_

Author SHA1 Message Date
Bradley Bickford
740b4a57b9 Something to try to fix field oriented control with 2025-03-04 12:13:30 -05:00
38 changed files with 293 additions and 821 deletions
Expand all files Collapse all files

45
.OutlineViewer/outlineviewer.json
View File

@@ -1,58 +1,13 @@
{
"Clients": {
"open": true
},
"Connections": {
"open": true
},
"NetworkTables Settings": {
"mode": "Client (NT4)"
},
"client@1": {
"Publishers": {
"open": true
},
"open": true
},
"client@2": {
"open": true
},
"client@3": {
"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
}
}
}

6
src/main/deploy/pathplanner/autos/1 L4 Center.auto
View File

@@ -4,12 +4,6 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {

19
src/main/deploy/pathplanner/autos/Field Oriented Test Auto.auto Normal file
View File

@@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Field Oriented Test Path"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

18
src/main/deploy/pathplanner/autos/One 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": {
@@ -33,12 +21,6 @@
"data": {
"pathName": "J Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}

74
src/main/deploy/pathplanner/autos/Two Coral Left Event.auto
View File

@@ -1,74 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Start to 30 Right"
}
},
{
"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"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Collect Coral"
}
},
{
"type": "path",
"data": {
"pathName": "HP to 330 Left"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "K Approach"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

41
src/main/deploy/pathplanner/autos/Two Coral Left.auto
View File

@@ -13,38 +13,13 @@
{
"type": "named",
"data": {
"name": "Lift L4"
"name": "Shoot Coral L4"
}
},
{
"type": "path",
"data": {
"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"
}
}
]
"pathName": "30 Right to HP"
}
},
{
@@ -59,18 +34,6 @@
"pathName": "HP to 330 Left"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "K Approach"
}
},
{
"type": "named",
"data": {

49
src/main/deploy/pathplanner/autos/Two Coral Right.auto
View File

@@ -1,49 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Right Start to E"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
},
{
"type": "path",
"data": {
"pathName": "E to HP"
}
},
{
"type": "named",
"data": {
"name": "Collect Coral"
}
},
{
"type": "path",
"data": {
"pathName": "HP to D"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

19
src/main/deploy/pathplanner/autos/fein.auto Normal file
View File

@@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "fein"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

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

File diff suppressed because one or more lines are too long

24
src/main/deploy/pathplanner/paths/K Approach.path → src/main/deploy/pathplanner/paths/150 Right to HP.path
View File

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 3.6202868852459016,
"y": 5.859118852459017
"x": 5.289041095890411,
"y": 2.9732020547945206
},
"prevControl": null,
"nextControl": {
"x": 3.6663590190101236,
"y": 5.6134008057257505
"x": 5.950171232876712,
"y": 1.6208904109589037
},
"isLocked": false,
"linkedName": "Before K"
"linkedName": null
},
{
"anchor": {
"x": 3.7290765411991873,
"y": 5.091647695859483
"x": 1.4424657534246574,
"y": 0.7944777397260271
},
"prevControl": {
"x": 3.693113426445089,
"y": 5.343389499138171
"x": 2.389083904109589,
"y": 2.5374571917808213
},
"nextControl": null,
"isLocked": false,
@@ -33,7 +33,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -60.94539590092286
"rotation": 54.162347045721745
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -59.69923999693802
"rotation": 120.96375653207336
},
"useDefaultConstraints": true
}

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

@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
"x": 1.3072345890410957,
"y": 7.135316780821918
},
"prevControl": {
"x": 2.055234532899169,
"y": 6.243135947675724
"x": 2.1636986301369863,
"y": 6.188698630136986
},
"nextControl": null,
"isLocked": false,
@@ -31,9 +31,16 @@
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"eventMarkers": [
{
"name": "HP Pickup",
"waypointRelativePos": 0.20476190476190542,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

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

@@ -45,7 +45,7 @@
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

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

@@ -1,73 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.958476027397261,
"y": 2.837970890410959
},
"prevControl": null,
"nextControl": {
"x": 5.619606164383562,
"y": 1.455607876712329
},
"isLocked": false,
"linkedName": "E"
},
{
"anchor": {
"x": 1.1569777397260272,
"y": 1.0198630136986298
},
"prevControl": {
"x": 2.7647260273972605,
"y": 1.485659246575342
},
"nextControl": null,
"isLocked": false,
"linkedName": "Right HP"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "HP Pickup",
"waypointRelativePos": 0.22857142857142831,
"endWaypointRelativePos": null,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 54.162347045721745
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 120.25643716352937
},
"useDefaultConstraints": true
}

26
src/main/deploy/pathplanner/paths/J Approach.path → src/main/deploy/pathplanner/paths/Field Oriented Test Path.path
View File

@@ -3,29 +3,29 @@
"waypoints": [
{
"anchor": {
"x": 5.286577868852459,
"y": 5.955020491803278
"x": 2.0,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 5.166700819672132,
"y": 5.679303278688525
"x": 3.0,
"y": 7.0
},
"isLocked": false,
"linkedName": "Before J"
"linkedName": null
},
{
"anchor": {
"x": 4.974897540983607,
"y": 5.235758196721312
"x": 4.495389344262295,
"y": 6.788165983606557
},
"prevControl": {
"x": 5.058811475409836,
"y": 5.523463114754099
"x": 3.4164959016393444,
"y": 6.80015368852459
},
"nextControl": null,
"isLocked": false,
"linkedName": "J"
"linkedName": null
}
],
"rotationTargets": [],
@@ -33,7 +33,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -119.71497744813712
"rotation": -90.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -119.71497744813712
"rotation": 0.0
},
"useDefaultConstraints": true
}

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

@@ -33,7 +33,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

29
src/main/deploy/pathplanner/paths/HP to 330 Left.path
View File

@@ -3,37 +3,44 @@
"waypoints": [
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
"x": 1.3072345890410957,
"y": 7.135316780821918
},
"prevControl": null,
"nextControl": {
"x": 2.490979395912868,
"y": 6.964368824388053
"x": 2.5994434931506847,
"y": 6.909931506849315
},
"isLocked": false,
"linkedName": "HP Left Position"
},
{
"anchor": {
"x": 3.6202868852459016,
"y": 5.859118852459017
"x": 3.973766447368421,
"y": 5.246957236842105
},
"prevControl": {
"x": 3.304747501684258,
"y": 6.385017825061756
"x": 3.6582270638067773,
"y": 5.772856209444845
},
"nextControl": null,
"isLocked": false,
"linkedName": "Before K"
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.5,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

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

@@ -1,73 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.187029109589041,
"y": 0.9747859589041092
},
"prevControl": null,
"nextControl": {
"x": 2.268878424657535,
"y": 1.0949914383561645
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.011857876712329,
"y": 2.837970890410959
},
"prevControl": {
"x": 3.3657534246575342,
"y": 1.9364297945205475
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.27142857142857096,
"endWaypointRelativePos": null,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lift L4"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 58.57043438516136
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 54.24611274556325
},
"useDefaultConstraints": true
}

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

@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 4.974897540983607,
"y": 5.235758196721312
"x": 4.988527397260274,
"y": 5.227054794520548
},
"prevControl": null,
"nextControl": {
"x": 5.11855393929902,
"y": 5.503390382532524
"x": 5.1321837955756875,
"y": 5.49468698033176
},
"isLocked": false,
"linkedName": "J"
@@ -33,7 +33,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
@@ -48,7 +48,7 @@
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -119.71497744813712
"rotation": -121.60750224624898
},
"useDefaultConstraints": true
}

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

@@ -44,7 +44,7 @@
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

36
src/main/deploy/pathplanner/paths/Right Start to E.path → src/main/deploy/pathplanner/paths/Right Start to 150 Right.path
View File

@@ -3,29 +3,29 @@
"waypoints": [
{
"anchor": {
"x": 7.572945205479453,
"y": 0.4939640410958907
"x": 7.602996575342465,
"y": 2.0115582191780823
},
"prevControl": null,
"nextControl": {
"x": 6.461044520547946,
"y": 0.4789383561643841
"x": 6.491095890410959,
"y": 1.9965325342465756
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.958476027397261,
"y": 2.837970890410959
"x": 5.289041095890411,
"y": 2.9882277397260277
},
"prevControl": {
"x": 5.799914383561644,
"y": 1.9664811643835614
"x": 6.130479452054795,
"y": 2.11673801369863
},
"nextControl": null,
"isLocked": false,
"linkedName": "E"
"linkedName": null
}
],
"rotationTargets": [],
@@ -34,25 +34,13 @@
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.4261904761904757,
"waypointRelativePos": 0.7142857142857124,
"endWaypointRelativePos": null,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lift L4"
}
}
]
}
}
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

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

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 7.1686475409836055,
"y": 7.573360655737705
"x": 7.587970890410959,
"y": 6.143621575342466
},
"prevControl": null,
"nextControl": {
"x": 5.933913934426228,
"y": 6.5544057377049185
"x": 6.385916095890411,
"y": 6.158647260273973
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.974897540983607,
"y": 5.235758196721312
"x": 4.988527397260274,
"y": 5.227054794520548
},
"prevControl": {
"x": 5.560899253312374,
"y": 6.107247922748709
"x": 5.574529109589041,
"y": 6.098544520547945
},
"nextControl": null,
"isLocked": false,
@@ -34,18 +34,13 @@
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0,
"waypointRelativePos": 0.4547619047619047,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
@@ -54,7 +49,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -119.71497744813712
"rotation": -121.60750224624898
},
"reversed": false,
"folder": null,

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

@@ -31,9 +31,16 @@
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.08214624881291864,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

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

@@ -49,7 +49,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,

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

@@ -4,7 +4,7 @@
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 2.0,
"defaultMaxVel": 4.0,
"defaultMaxAccel": 1.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 400.0,

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

@@ -25,17 +25,13 @@ import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.events.EventTrigger;
import com.pathplanner.lib.path.EventMarker;
import edu.wpi.first.math.geometry.Rotation2d;
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;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
public class RobotContainer {
private ClimberPivot climberPivot;
@@ -56,6 +52,8 @@ public class RobotContainer {
private SendableChooser<Command> autoChooser;
private Vision vision;
private IntSupplier closestTag;
public RobotContainer() {
@@ -65,6 +63,8 @@ public class RobotContainer {
drivetrain = new Drivetrain();
vision = new Vision(drivetrain::getGyroValue);
elevator = new Elevator();
//elevator = new ElevatorSysID();
@@ -109,8 +109,8 @@ public class RobotContainer {
drivetrain.setDefaultCommand(
drivetrain.drive(
() -> 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),
() -> Math.pow(driver.getLeftY(), 3),
() -> Math.pow(driver.getLeftX(), 3),
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
() -> true
)
@@ -127,7 +127,9 @@ public class RobotContainer {
manipulator.setDefaultCommand(
manipulator.runManipulator(() -> 0.0, false)
manipulator.runUntilCollected(
() -> 0.0
)
);
//Driver inputs
@@ -141,9 +143,7 @@ public class RobotContainer {
);
driver.leftTrigger().whileTrue(
manipulator.runUntilCollected(() -> 0.75)
.until(() -> manipulator.getCoralBeamBreak() == false)
.andThen(manipulator.retractCommand(() -> .1))
manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
);
driver.start().and(driver.back()).onTrue(
@@ -153,28 +153,24 @@ 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.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
))
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
)
);
driver.leftBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
))
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
)
);
*/
//Operator inputs
operator.povUp().onTrue(
@@ -205,51 +201,46 @@ public class RobotContainer {
)
);
operator.back().onTrue(elevator.homeCommand());
operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
operator.a().onTrue(
safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
);
operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
operator.b().onTrue(
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
operator.rightTrigger().onTrue(shootAlgae());
}
private void configureNamedCommands() {
//new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
new EventTrigger("Lift L4").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("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).until(() -> manipulator.getCoralBeamBreak() == false).andThen(manipulator.runManipulator(() -> 0, false)).withTimeout(0.1));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
.andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.1)));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
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
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);
@@ -322,40 +313,9 @@ public class RobotContainer {
sensorTab.addDouble("velocity", drivetrain::getVelocity);
sensorTab.addDouble("heading", drivetrain::getHeading);
//sensorTab.add("odometry", drivetrain::getPose);
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
.withSize(1,1).withPosition(1,1);
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
.withSize(2,1).withPosition(2,1);
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
.withSize(3,1).withPosition(3,1);
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
.withSize(4,1).withPosition(4,1);
apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
.withSize(6,1).withPosition(5,1);
apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
.withSize(7,1).withPosition(6,1);
apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
.withSize(1,1).withPosition(1,2);
apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
.withSize(1,1).withPosition(2,2);
apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
.withSize(1,1).withPosition(3,2);
apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
.withSize(1,1).withPosition(4,2);
apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
.withSize(1,1).withPosition(5,2);
apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
.withSize(1,1).withPosition(6,2);
apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
.withSize(1,1).withPosition(4,4);
}
public Command getAutonomousCommand() {
@@ -496,14 +456,6 @@ public class RobotContainer {
.raceWith(elevator.maintainPosition()));
}
private Command shootAlgae(){
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>44).andThen(manipulator.runManipulator(() -> -1, false),
elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition()));
}
@SuppressWarnings("unused")
private Command startingConfig() {
return moveManipulatorUtil(0, 0, false, true)

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

@@ -11,13 +11,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
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 kMaxSpeedMetersPerSecond = 4;
public static final double kMaxAccelerationMetersPerSecondSquared = 1;
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 = 6;
public static final double kPYController = 6;
public static final double kPThetaController = 5.5;
// Constraint for the motion profiled robot angle controller

14
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@@ -29,10 +29,10 @@ public class DrivetrainConstants {
public static final double kBackRightChassisAngularOffset = 0;
*/
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;
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;
// 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.1;
public static final double kHeadingP = 0.0;
public static final double kXTranslationP = 0.5;
public static final double kYTranslationP = 0.5;
public static final double kXTranslationP = 0.0;
public static final double kYTranslationP = 0.0;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM

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

@@ -42,9 +42,9 @@ public class ElevatorConstants {
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 14;
public static final double kL2Position = 11;
public static final double kL3Position = 27;
public static final double kL1Position = 0;
public static final double kL2Position = 9;
public static final double kL3Position = 23.0;
public static final double kL4Position = 50.5;
public static final double kL4TransitionPosition = 40.0;
public static final double kL2AlgaePosition = 23.0;

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

@@ -1,6 +1,5 @@
package frc.robot.constants;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
public class ManipulatorConstants {
@@ -8,9 +7,4 @@ public class ManipulatorConstants {
public static final int kCoralBeamBreakID = 2;
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static{
motorConfig.smartCurrentLimit(40)
.idleMode(IdleMode.kBrake);
};
}

31
src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@@ -20,7 +20,7 @@ public class ManipulatorPivotConstants {
public static final double kPivotMaxVelocity = 2 * Math.PI;
public static final double kPositionalP = 4.5;
public static final double kPositionalP = 4;
public static final double kPositionalI = 0;
public static final double kPositionalD = 0;
public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
@@ -29,29 +29,26 @@ 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+90);
public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
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.78-0.25;
public static final double kEncoderOffset = 0.780;
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(246);
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);
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);
/**The closest position to the elevator brace without hitting it */
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
public static final double kBargeShotPosition = Units.degreesToRadians(222);
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
/**The forward rotation limit of the pivot */
public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
public static final double kRotationLimit = Units.degreesToRadians(175.0);
public static final double kSysIDRampRate = 1;
public static final double kSysIDStepVolts = 7;

2
src/main/java/frc/robot/constants/ModuleConstants.java
View File

@@ -48,7 +48,7 @@ public class ModuleConstants {
public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
public static final InvertedValue kDriveInversionState = InvertedValue.CounterClockwise_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

3
src/main/java/frc/robot/constants/OIConstants.java
View File

@@ -4,9 +4,8 @@ public class OIConstants {
public static final int kDriverControllerPort = 0;
public static final int kOperatorControllerPort = 1;
public static final double kDriveDeadband = Math.pow(0.05, 3);
public static final double kDriveDeadband = 0.05;
public static final String kAutoTab = "Auto Tab";
public static final String kSensorsTab = "Sensors Tab";
public static final String kApriltagTab = "Apriltag Tab";
}

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

@@ -40,12 +40,12 @@ public class VisionConstants {
{},
{},
{},
{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
{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
{},
{},
{},
@@ -54,11 +54,9 @@ public class VisionConstants {
{3.703, 3.975, 3.982, 2.806},
{3.183, 4.191, 3.183, 3.857},
{3.986, 5.24, 3.701, 5.076},
{5.275, 5.075, 4.891, 5.284},//4.991, 5.246},
{5.275, 5.075, 4.991, 5.246},
{5.789, 3.862, 5.789, 4.194},
{4.993, 2.816, 5.272, 2.996}
};
public static final double latencyFudge = 0.0;
}

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

@@ -8,32 +8,22 @@ import java.io.File;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Orchestra;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.RotationTarget;
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.PoseEstimator;
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;
@@ -42,7 +32,6 @@ 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
@@ -56,8 +45,6 @@ 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();
@@ -66,11 +53,6 @@ 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(
@@ -108,20 +90,14 @@ public class Drivetrain extends SubsystemBase {
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
new Pose2d(),
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
new Pose2d()
);
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
pidHeading.setTolerance(Units.degreesToRadians(3));
pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
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,
@@ -154,10 +130,6 @@ public class Drivetrain extends SubsystemBase {
m_orchestra.play();
musicTimer.reset();
musicTimer.start();
vision = new Vision();
orangePose2d = new Pose2d();
blackPose2d = new Pose2d();
}
@Override
@@ -172,47 +144,24 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
});
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, 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());
}
// 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());
}
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
if(vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, 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());
}
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());
}
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
*/
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>10){
if(musicTimer.get()>20){
if (m_orchestra.isPlaying()) {
m_orchestra.stop();
}
@@ -254,15 +203,14 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_estimator.resetPosition(
Rotation2d.fromDegrees(getGyroValue()),
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
},
pose
);
}
@@ -270,20 +218,12 @@ public class Drivetrain extends SubsystemBase {
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
BooleanSupplier fieldRelative) {
return run(() -> {
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()
);}
drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean()
);
});
}
@@ -305,7 +245,7 @@ public class Drivetrain extends SubsystemBase {
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
Rotation2d.fromDegrees(getGyroValue()))
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -321,35 +261,25 @@ public class Drivetrain extends SubsystemBase {
});
}
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
return startRun(() -> {
pidHeading.reset();
pidTranslationX.reset();
pidTranslationY.reset();
},
() -> {
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.05, 0.05),
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.05, 0.05),
pidHeading.calculate(Units.degreesToRadians(getHeading()), headingSetpoint.get().getRadians()),
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()),
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
true);
Logger.recordOutput("reeeee", pidHeading.getError());
Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
headingSetpoint.get())));
});
}
public int getClosestTag(){
if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
int closestTag = 17;
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));
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
for(int i = 17; i <= 22; ++i){
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));
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
if(distance < closestTagDist){
closestTag = i;
@@ -359,12 +289,12 @@ public class Drivetrain extends SubsystemBase {
return closestTag;
}else{
int closestTag = 6;
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));
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
for(int i = 6; i <= 11; ++i){
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));
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
if(distance < closestTagDist){
closestTag = i;
@@ -412,7 +342,6 @@ public class Drivetrain extends SubsystemBase {
public Command zeroHeading() {
return run(() -> {
gyro.reset();
m_estimator.resetRotation(new Rotation2d(0));
});
}
@@ -426,11 +355,7 @@ public class Drivetrain extends SubsystemBase {
* @return the robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return m_estimator.getEstimatedPosition().getRotation().getDegrees();
}
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
return gyroBuffer;
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
}
/**
@@ -442,6 +367,10 @@ 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();
}
@@ -449,10 +378,4 @@ public class Drivetrain extends SubsystemBase {
public double getVelocity(){
return m_frontLeft.getState().speedMetersPerSecond;
}
public Command resetToVision(){
return runOnce(() -> {
m_estimator.resetPose(orangePose2d);
});
}
}

10
src/main/java/frc/robot/subsystems/Elevator.java
View File

@@ -174,14 +174,6 @@ 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).
*
@@ -216,7 +208,7 @@ public class Elevator extends SubsystemBase {
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
.andThen(runManualElevator(() -> -.5)
.andThen(runManualElevator(() -> -.1)
.until(() -> encoder.getPosition() == 0));
} else {

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

@@ -71,20 +71,7 @@ public class Manipulator extends SubsystemBase {
manipulatorMotor.set(
speed.getAsDouble()
);
}).unless(() -> !coralBeamBreak.get())
.until(() -> !coralBeamBreak.get());
/*
return run(() -> {
if(getCoralBeamBreak()) {
manipulatorMotor.set(
speed.getAsDouble()
);
} else {
manipulatorMotor.set(
speed.getAsDouble()
);
}
*/
}).until(() -> !coralBeamBreak.get());
}
public Command retractCommand(DoubleSupplier retractSpeed){

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

@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
);
pidController.setSetpoint(0);
pidController.enableContinuousInput(0, 280);
pidController.enableContinuousInput(0, 180);
feedForward = new ArmFeedforward(
ManipulatorPivotConstants.kFeedForwardS,

119
src/main/java/frc/robot/subsystems/Vision.java
View File

@@ -1,125 +1,93 @@
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 NetworkTable blackVisionTable;
private DoubleSubscriber black_tx;
private DoubleSubscriber black_ty;
private DoubleSubscriber black_dist;
private DoubleSubscriber blackRobotRelativeX;
private DoubleSubscriber blackRobotRelativeY;
private DoubleSubscriber blackRobotRelativeZ;
private DoubleSubscriber blackClosestTag;
private BooleanSubscriber blackTagDetected;
private DoubleSubscriber blackFramerate;
private NetworkTable orangeVisionTable;
private DoubleSubscriber orange_tx;
private DoubleSubscriber orange_ty;
private DoubleSubscriber orange_dist;
private DoubleSubscriber orangeRobotRelativeX;
private DoubleSubscriber orangeRobotRelativeY;
private DoubleSubscriber orangeRobotRelativeZ;
private DoubleSubscriber orangeClosestTag;
private BooleanSubscriber orangeTagDetected;
private DoubleSubscriber orangeFramerate;
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};
private DoubleSupplier gyroAngle;
public Vision(){
public Vision(DoubleSupplier gyroAngle){
NetworkTableInstance inst = NetworkTableInstance.getDefault();
blackVisionTable = inst.getTable("black_Fiducial");
orangeVisionTable = inst.getTable("orange_Fiducial");
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
black_dist = blackVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").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);
orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").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, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1],
new Rotation2d());
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
}
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(-camPose[1] + Units.degreesToRadians(ty));
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(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(camPose[2])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
return robotPose;
public Pose2d getBlackGlobalPose(){
return relativeToGlobalPose2d(getBlackClosestTag(),
new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
}
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
public double getBlackRelativeX(){
return blackRobotRelativeX.get();
}
public double getBlackTX(){
return black_tx.get();
public double getBlackRelativeY(){
return blackRobotRelativeY.get();
}
public double getBlackTY(){
return black_ty.get();
}
public double getBlackDist(){
return black_dist.get();
public double getBlackRelativeZ(){
return blackRobotRelativeZ.get();
}
public int getBlackClosestTag(){
@@ -127,7 +95,7 @@ public class Vision{
}
public double getBlackTimeStamp(){
return black_tx.getLastChange()-VisionConstants.latencyFudge;
return blackRobotRelativeX.getLastChange();
}
public boolean getBlackTagDetected(){
@@ -138,26 +106,21 @@ public class Vision{
return blackFramerate.get();
}
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 Pose2d getOrangeGlobalPose(){
return relativeToGlobalPose2d(getOrangeClosestTag(),
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
}
public double getOrangeTX(){
return orange_tx.get();
public double getOrangeRelativeX(){
return orangeRobotRelativeX.get();
}
public double getOrangeTY(){
return orange_ty.get();
public double getOrangeRelativeY(){
return orangeRobotRelativeY.get();
}
public double getOrangeDist(){
return orange_dist.get();
public double getOrangeRelativeZ(){
return orangeRobotRelativeZ.get();
}
public int getOrangeClosestTag(){
@@ -165,7 +128,7 @@ public class Vision{
}
public double getOrangeTimeStamp(){
return orange_tx.getLastChange()-VisionConstants.latencyFudge;
return orangeRobotRelativeX.getLastChange();
}
public boolean getOrangeTagDetected(){