Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #38 from Team4388/autos-and-intake-fixes

Browse Source 
Autos and intake fixes
This commit is contained in:
C4llSqin
2025-02-28 18:51:35 -07:00
committed by GitHub
parent eea43692fc 125cfa929f
commit 1763225d53
36 changed files with 327 additions and 488 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/Blue Top Cage.auto
+2 -14
View File
@@ -13,13 +13,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "april-allign" "name": "place-coral-left-l4"
}
},
{
"type": "named",
"data": {
"name": "place-coral"
} }
}, },
{ {
@@ -43,13 +37,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "april-allign" "name": "place-coral-left-l4"
}
},
{
"type": "named",
"data": {
"name": "place-coral"
} }
} }
] ]
src/main/deploy/pathplanner/autos/Line-up-no-arm.auto
+1 -1
View File
@@ -13,7 +13,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "lineup-no-arm" "name": "place-coral-left-l4"
} }
} }
] ]
src/main/deploy/pathplanner/autos/Red Bottom Cage.auto
+2 -14
View File
@@ -13,13 +13,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "april-allign" "name": "place-coral-right-l4"
}
},
{
"type": "named",
"data": {
"name": "place-coral"
} }
}, },
{ {
@@ -43,13 +37,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "april-allign" "name": "place-coral-left-l4"
}
},
{
"type": "named",
"data": {
"name": "place-coral"
} }
} }
] ]
src/main/deploy/pathplanner/autos/Red Center Cage.auto
+8 -14
View File
@@ -13,13 +13,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "april-allign" "name": "place-coral-right-l4"
}
},
{
"type": "named",
"data": {
"name": "place-coral"
} }
}, },
{ {
@@ -28,6 +22,12 @@
"pathName": "Bottom Reef to Feed" "pathName": "Bottom Reef to Feed"
} }
}, },
{
"type": "named",
"data": {
"name": "align-feed"
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
@@ -43,13 +43,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "april-allign" "name": "place-coral-left-l4"
}
},
{
"type": "named",
"data": {
"name": "place-coral"
} }
} }
] ]
src/main/deploy/pathplanner/autos/Stupid auto.auto
-31
View File
@@ -1,31 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Stupid auto 1"
}
},
{
"type": "path",
"data": {
"pathName": "Stupid auto 2"
}
},
{
"type": "path",
"data": {
"pathName": "Stupid auto 3"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Test Auto.auto
-19
View File
@@ -1,19 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Stupid auto 1"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 60.35013649242443 "rotation": -121.26879518614867
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": 179.34296858150984
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path
+8 -8
View File
@@ -8,20 +8,20 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.115768324814807, "x": 4.39747159090909,
"y": 1.5107373181346522 "y": 1.3027556818181814
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 3.692929292929293, "x": 5.3048863636363635,
"y": 2.6833333333333327 "y": 2.5292613636363637
}, },
"prevControl": { "prevControl": {
"x": 3.11332763609877, "x": 5.893210227272728,
"y": 2.3131602797293294 "y": 1.9808238636363626
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -121.27770286686648 "rotation": 114.22774531795413
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -126.57303097851931 "rotation": 53.446462811652175
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path
+6 -6
View File
@@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 5.1565656565656575, "x": 5.344772727272727,
"y": 2.886616161616162 "y": 2.6688636363636364
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 5.532638888888889, "x": 6.4715625,
"y": 2.530871212121211 "y": 1.432386363636364
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -123.50343698241421 "rotation": 55.05578949900953
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -59.036243467926624 "rotation": 121.42956561483854
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Barge to Reef.path
+6 -6
View File
@@ -8,19 +8,19 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 8.114457611671098, "x": 7.269289772727273,
"y": 4.043616195025507 "y": 4.064886363636362
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 5.979861111111112, "x": 6.172414772727272,
"y": 4.045328282828282 "y": 4.045328282828282
}, },
"prevControl": { "prevControl": {
"x": 5.5035379766577375, "x": 5.696091638273898,
"y": 4.0462802990619195 "y": 4.0462802990619195
}, },
"nextControl": null, "nextControl": null,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 0.5456575934157175 "rotation": -179.96096735022735
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": -179.2890804030095
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 60.851928154286895 "rotation": -120.86136963407526
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 1.1457628387483283 "rotation": -179.99261564513898
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path
+3 -3
View File
@@ -9,7 +9,7 @@
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 5.96969696969697, "x": 5.96969696969697,
"y": 1.128219696969696 "y": 1.1282196969696958
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -60.52411099675423 "rotation": 122.87136694597014
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 1.97493401088202 "rotation": 179.5295681977034
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path
+6 -6
View File
@@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 5.786742424242425, "x": 6.092642045454545,
"y": 4.035164141414142 "y": 3.9651704545454542
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 7.026767676767678, "x": 9.024289772727268,
"y": 2.185290404040403 "y": 3.446647727272728
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -124.91183021661 "rotation": 52.857102599919905
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -1.6365770416167795 "rotation": -179.46454101443553
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Reef to Feed.path
+12 -12
View File
@@ -3,29 +3,29 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 5.817234848484849, "x": 5.982954545454545,
"y": 4.045328282828282 "y": 4.03497159090909
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 6.62020202020202, "x": 7.109744318181819,
"y": 4.797474747474748 "y": 3.8355397727272718
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 5.2175505050505055, "x": 6.232244318181818,
"y": 5.45814393939394 "y": 5.281420454545455
}, },
"prevControl": { "prevControl": {
"x": 5.45268897997343, "x": 7.1397939460232385,
"y": 5.373232823449551 "y": 4.676387369317844
}, },
"nextControl": { "nextControl": {
"x": 3.388005050505051, "x": 5.574119318181818,
"y": 6.1188131313131295 "y": 5.720170454545454
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -58,13 +58,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 126.3843518158359 "rotation": -53.93780932479241
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": 179.1574757392596
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Stupid auto 1.path
-54
View File
@@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 6.081502525252525,
"y": 7.267361111111111
},
"prevControl": null,
"nextControl": {
"x": 4.871969696969696,
"y": 8.070328282828283
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.081502525252525,
"y": 5.10239898989899
},
"prevControl": {
"x": 4.821148989898989,
"y": 5.10239898989899
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Stupid auto 2.path
-54
View File
@@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 6.101830808080808,
"y": 5.041414141414141
},
"prevControl": null,
"nextControl": {
"x": 7.10183080808081,
"y": 5.041414141414141
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.585795454545455,
"y": 6.149305555555555
},
"prevControl": {
"x": 8.175315656565658,
"y": 5.295517676767678
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Stupid auto 3.path
-54
View File
@@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.585795454545455,
"y": 6.200126262626262
},
"prevControl": null,
"nextControl": {
"x": 7.778914141414141,
"y": 7.033585858585859
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.081502525252525,
"y": 7.32834595959596
},
"prevControl": {
"x": 7.595959595959596,
"y": 7.724747474747475
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Taxi.path
+2 -2
View File
@@ -58,13 +58,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -1.9251837083231407 "rotation": 179.59913485447024
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -1.1306909512379957 "rotation": -179.85896618052556
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path
+4 -4
View File
@@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 5.464431818181818, "x": 5.344772727272727,
"y": 5.6403977272727275 "y": 5.391107954545454
}, },
"prevControl": { "prevControl": {
"x": 5.240820707070706, "x": 5.121161616161616,
"y": 5.254160353535354 "y": 5.00487058080808
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
src/main/deploy/pathplanner/paths/Top Feed to Reef.path
+10 -10
View File
@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.0807449494949497, "x": 1.116818181818182,
"y": 6.972601010101011 "y": 6.956647727272727
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.0869949494949496, "x": 2.123068181818182,
"y": 6.291603535353536 "y": 6.275650252525252
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 3.764078282828282, "x": 3.6196875,
"y": 5.24469696969697 "y": 5.35122159090909
}, },
"prevControl": { "prevControl": {
"x": 2.9836492542425197, "x": 2.8392584714142375,
"y": 5.685899280282051 "y": 5.792423901494171
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 121.34521414171566 "rotation": -59.239047023115106
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 124.23611780915067 "rotation": -52.93323259086456
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path
+6 -6
View File
@@ -8,8 +8,8 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 8.242449416720314, "x": 7.319147727272727,
"y": 3.1104783377417067 "y": 2.3198579545454536
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -20,8 +20,8 @@
"y": 2.6934974747474745 "y": 2.6934974747474745
}, },
"prevControl": { "prevControl": {
"x": 4.985594317577342, "x": 6.401761363636364,
"y": 2.6542548990156853 "y": 1.5819602272727276
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -54.659893078442295 "rotation": 123.49004753500587
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": -178.9832812445108
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Top Reef to Feed.path
+8 -8
View File
@@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 5.17689393939394, "x": 5.284943181818181,
"y": 5.224368686868687 "y": 5.401079545454545
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 5.9669821266547345, "x": 4.844554924242423,
"y": 4.892012503312241 "y": 6.744466540404039
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -20,8 +20,8 @@
"y": 7.003093434343434 "y": 7.003093434343434
}, },
"prevControl": { "prevControl": {
"x": 0.960567140436829, "x": 1.9644034090909093,
"y": 7.086188101328755 "y": 5.750085227272726
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 126.86989764584399 "rotation": -54.83470564502973
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 57.21571913413089 "rotation": -121.0370223454415
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/java/frc4388/robot/Constants.java
+42 -41
View File
@@ -229,29 +229,7 @@ public final class Constants {
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST
} }
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
public static final int LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
// public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
// public static final Pose2d targetpos =
}
public static final class Configurations { public static final class Configurations {
public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
@@ -342,6 +320,46 @@ public final class Constants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
} }
public static final class LiDARConstants {
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
public static final int LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
}
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
// X is tangent to reef side
// Y is normal to reef side
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
public static final int L4_DRIVE_TIME = 250; //Milliseconds
// public static final int L3_DRIVE_TIME = 500;
public static final int L2_DRIVE_TIME = 250; //Milliseconds
public static final int ALGAE_DRIVE_TIME = 500;
}
public static final class VisionConstants { public static final class VisionConstants {
public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
@@ -353,31 +371,14 @@ public final class Constants {
// Photonvision thing // Photonvision thing
// The standard deviations of our vision estimated poses, which affect correction rate // The standard deviations of our vision estimated poses, which affect correction rate
// (Fake values. Experiment and determine estimation noise on an actual robot.) // X, Y, Theta
// https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
} }
public static final class FieldConstants { public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5);
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L2_ALGAE_REMOVAL = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(3);
public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
// Test april tag field layout // Test april tag field layout
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
src/main/java/frc4388/robot/Robot.java
+3 -3
View File
@@ -10,7 +10,6 @@ package frc4388.robot;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Base64; import java.util.Base64;
import java.util.List; import java.util.List;
import java.util.logging.Level;
import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.CANBus.CANBusStatus; import com.ctre.phoenix6.CANBus.CANBusStatus;
@@ -87,8 +86,9 @@ public class Robot extends TimedRobot {
@Override @Override
public void robotPeriodic() { public void robotPeriodic() {
m_robotTime.updateTimes(); m_robotTime.updateTimes();
//System.out.println(m_robotContainer.limelight.isNearSpeaker()); // SmartDashboard.putNumber("Time", System.currentTimeMillis());
//mled.updateLED();
m_robotContainer.m_robotLED.update();
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands, // commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic // and running subsystem periodic() methods. This must be called from the robot's periodic
src/main/java/frc4388/robot/RobotContainer.java
+49 -57
View File
@@ -29,7 +29,7 @@ import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.AutoConstants;
import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -40,15 +40,15 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
// Autos // Autos
import frc4388.utility.controller.VirtualController; import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.IfCommand;
import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitElevatorRefrence;
import frc4388.robot.commands.waitEndefectorRefrence; import frc4388.robot.commands.waitEndefectorRefrence;
import frc4388.robot.commands.waitFeedCoral;
import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder; import frc4388.robot.commands.Swerve.neoJoystickRecorder;
@@ -86,7 +86,7 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap(); public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */ /* Subsystems */
private final LED m_robotLED = new LED(); public final LED m_robotLED = new LED();
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
public final Lidar m_lidar = new Lidar(); public final Lidar m_lidar = new Lidar();
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED); public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED);
@@ -125,11 +125,11 @@ public class RobotContainer {
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlignL4Right = new SequentialCommandGroup( private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT)
// )), ), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
@@ -137,7 +137,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -145,7 +145,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true), new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -155,9 +155,11 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT)
// )), ), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -165,7 +167,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -174,7 +176,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true), new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
@@ -184,15 +186,16 @@ public class RobotContainer {
private Command AprilLidarAlignL3Left = new SequentialCommandGroup( private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT)
// )), ), () -> m_robotElevator.isL3Primed()),
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
@@ -206,18 +209,19 @@ public class RobotContainer {
private Command AprilLidarAlignL3Right = new SequentialCommandGroup( private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT)
// )), ),() -> m_robotElevator.isL3Primed()),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -233,14 +237,14 @@ public class RobotContainer {
private Command AprilLidarAlignL2Left = new SequentialCommandGroup( private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true), new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -248,46 +252,28 @@ public class RobotContainer {
private Command AprilLidarAlignL2Right = new SequentialCommandGroup( private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true), new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
); );
private Command placeCoral = new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
new InstantCommand(() -> System.out.println("Placing Some Coral")),
new WaitCommand(3),
new InstantCommand(() -> System.out.println("Done placing Some Coral"))
);
private Command aprilAlign = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Aligning...")),
new WaitCommand(1)
);
private Command grabCoral = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Yoinking some coral...")),
new WaitCommand(2),
new InstantCommand(() -> System.out.println("Done yoinking some coral..."))
);
private Command leftAlgaeRemove = new SequentialCommandGroup( private Command leftAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true), new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
); );
@@ -297,11 +283,11 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar), new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true), new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
); );
@@ -317,21 +303,26 @@ public class RobotContainer {
* The container for the robot. Contains subsystems, OI devices, and commands. * The container for the robot. Contains subsystems, OI devices, and commands.
*/ */
public RobotContainer() { public RobotContainer() {
NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator));
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0, 1),
new Translation2d(), 200, true
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left); NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right); NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup( NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar) new LidarAlign(m_robotSwerveDrive, m_lidar)
)); ));
NamedCommands.registerCommand("grab-coral", grabCoral);
configureButtonBindings(); configureButtonBindings();
configureVirtualButtonBindings(); configureVirtualButtonBindings();
new DeferredBlock(() -> { // Called on first robot enable new DeferredBlock(() -> { // Called on first robot enable
@@ -628,7 +619,8 @@ public class RobotContainer {
for (String auto : autos) { for (String auto : autos) {
if (auto.endsWith(".auto")) if (auto.endsWith(".auto"))
autoChooser.addOption(auto.replaceAll(".auto", ""), auto); autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
// System.out.println(auto);
} }
autoChooser.onChange((filename) -> { autoChooser.onChange((filename) -> {
src/main/java/frc4388/robot/commands/GotoLastApril.java
+1 -1
View File
@@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains; import frc4388.utility.Gains;
src/main/java/frc4388/robot/commands/IfCommand.java
-33
View File
@@ -1,33 +0,0 @@
package frc4388.robot.commands;
import java.time.Instant;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
// Command that is called if something is true
public class IfCommand extends InstantCommand {
BooleanSupplier isTrue;
Command trueCommand;
Command falseCommand;
public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){
this.isTrue = isTrue;
this.trueCommand = trueCommand;
this.falseCommand = falseCommand;
}
public IfCommand(BooleanSupplier isTrue, Command trueCommand){
this(isTrue, trueCommand, Commands.none());
}
@Override
public void execute() {
if(isTrue.getAsBoolean())
trueCommand.schedule();
else
falseCommand.schedule();
}
}
src/main/java/frc4388/robot/commands/LidarAlign.java
+5 -2
View File
@@ -22,6 +22,7 @@ public class LidarAlign extends Command {
private boolean headedRight; private boolean headedRight;
private double speed; private double speed;
private int bounces; private int bounces;
private double additionalDistance = 0;
// private final boolean constructedHeadedRight; // private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */ /** Creates a new LidarAlign. */
@@ -41,6 +42,7 @@ public class LidarAlign extends Command {
this.speed = 0.4; // TODO: find good speed for this this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false; this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0); this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0; this.bounces = 0;
} }
@@ -54,11 +56,12 @@ public class LidarAlign extends Command {
return; return;
} }
if (currentFinderTick > 10) { //arbutrary threshhold for now. if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
headedRight = !headedRight; headedRight = !headedRight;
currentFinderTick *= -1; currentFinderTick *= -1;
bounces++; bounces++;
if (bounces == 2) return; additionalDistance += 5;
if (bounces == 5) return;
} }
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
src/main/java/frc4388/robot/commands/waitFeedCoral.java
+36
View File
@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitFeedCoral extends Command {
/** Creates a new waitElevatorRefrence. */
private Elevator elevator;
public waitFeedCoral(Elevator elevator) {
this.elevator = elevator;
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.hasCoral();
}
}
src/main/java/frc4388/robot/subsystems/DiffDrive.java
-4
View File
@@ -7,21 +7,17 @@
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
import java.util.logging.Level;
import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.DriveConstants;
import frc4388.utility.RobotGyro; import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime; import frc4388.utility.RobotTime;
import frc4388.utility.Status; import frc4388.utility.Status;
import frc4388.utility.Subsystem; import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
/** /**
* Add your docs here. * Add your docs here.
src/main/java/frc4388/robot/subsystems/Elevator.java
+35 -4
View File
@@ -22,12 +22,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ElevatorConstants; import frc4388.robot.Constants.ElevatorConstants;
import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.LED;
import frc4388.utility.LEDPatterns; import frc4388.utility.LEDPatterns;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne; import frc4388.utility.TimesNegativeOne;
import frc4388.utility.Status.ReportLevel;
public class Elevator extends SubsystemBase { public class Elevator extends Subsystem {
/** Creates a new Elevator. */ /** Creates a new Elevator. */
private TalonFX elevatorMotor; private TalonFX elevatorMotor;
private TalonFX endeffectorMotor; private TalonFX endeffectorMotor;
@@ -237,6 +240,8 @@ public class Elevator extends SubsystemBase {
private void periodicWaiting() { private void periodicWaiting() {
if (!basinBeamBreak.get()) if (!basinBeamBreak.get())
transitionState(CoordinationState.Ready); transitionState(CoordinationState.Ready);
if(!endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering);
} }
// private void periodicWaitingTripped() { // private void periodicWaitingTripped() {
@@ -245,12 +250,15 @@ public class Elevator extends SubsystemBase {
// } // }
private void periodicReady() { private void periodicReady() {
if (elevatorAtReference()) if (elevatorAtReference() && !endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering); transitionState(CoordinationState.Hovering);
if(elevatorAtReference() && endeffectorLimitSwitch.get())
transitionState(CoordinationState.Waiting);
} }
private void periodicScoring() { private void periodicScoring() {
if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); if (!endeffectorLimitSwitch.get())
transitionState(CoordinationState.Waiting);
} }
public void manualElevatorVel(double velocity) { public void manualElevatorVel(double velocity) {
@@ -307,9 +315,32 @@ public class Elevator extends SubsystemBase {
return currentState == CoordinationState.PrimedThree; return currentState == CoordinationState.PrimedThree;
} }
public boolean hasCoral(){
return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get();
}
public void armShuffle(){ public void armShuffle(){
if(!basinBeamBreak.get()){ if(!basinBeamBreak.get()){
//shuffle the coral with the arm until coral hits beam break //shuffle the coral with the arm until coral hits beam break
} }
} }
@Override
public String getSubsystemName() {
return "Elevator";
}
@Override
public void queryStatus() {}
@Override
public Status diagnosticStatus() {
Status status = new Status();
status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor);
status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor);
return status;
}
} }
src/main/java/frc4388/robot/subsystems/LED.java
+42 -4
View File
@@ -9,29 +9,67 @@ package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLED;
import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.AddressableLEDBuffer;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns; import frc4388.utility.LEDPatterns;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
/** /**
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
* Driver * Driver
*/ */
public class LED extends SubsystemBase { public class LED extends Subsystem {
private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
private double mode = LEDConstants.DEFAULT_PATTERN.getValue(); private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
public void setMode(LEDPatterns pattern){ public void setMode(LEDPatterns pattern){
this.mode = pattern.getValue(); this.mode = pattern;
} }
@Override @Override
public void periodic() { public void periodic() {
LEDController.set(mode); update();
} }
public void update() {
if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return;
if(DriverStation.isDisabled()){
LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue());
}else
LEDController.set(mode.getValue());
}
@Override
public String getSubsystemName() {
return "LEDs";
}
@Override
public void queryStatus() {
SmartDashboard.putString("LED status", mode.name());
}
@Override
public Status diagnosticStatus() {
Status status = new Status();
if(LEDController.isAlive())
status.addReport(ReportLevel.INFO, "LED is CONNECTED");
else
status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
return status;
}
} }
src/main/java/frc4388/robot/subsystems/Lidar.java
+10 -6
View File
@@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.LiDARConstants;
import frc4388.utility.Status; import frc4388.utility.Status;
import frc4388.utility.Subsystem; import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel; import frc4388.utility.Status.ReportLevel;
@@ -16,7 +16,7 @@ import frc4388.utility.Status.ReportLevel;
public class Lidar extends Subsystem { public class Lidar extends Subsystem {
private double distance = -1; private double distance = -1;
Counter LidarPWM = new Counter(AutoConstants.LIDAR_DIO_CHANNEL); Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL);
public Lidar() { public Lidar() {
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
@@ -29,7 +29,7 @@ public class Lidar extends Subsystem {
if(LidarPWM.get() < 1) if(LidarPWM.get() < 1)
distance = -1; distance = -1;
else else
distance = (LidarPWM.getPeriod() * AutoConstants.SECONDS_TO_MICROS) / AutoConstants.LIDAR_MICROS_TO_CM; distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
} }
public double getDistance(){ public double getDistance(){
@@ -38,7 +38,7 @@ public class Lidar extends Subsystem {
public boolean withinDistance(){ public boolean withinDistance(){
if(distance == -1) return false; if(distance == -1) return false;
return distance < AutoConstants.LIDAR_DETECT_DISTANCE; return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
} }
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
@@ -69,11 +69,15 @@ public class Lidar extends Subsystem {
@Override @Override
public Status diagnosticStatus() { public Status diagnosticStatus() {
Status s = new Status(); Status s = new Status();
if(distance == -1){ if(distance == -1){
s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED"); s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
}else{ }else{
s.addReport(ReportLevel.INFO, "LIDAR CONNECTED"); s.addReport(ReportLevel.INFO, "LiDAR Connected");
} }
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance);
return s; return s;
} }
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+1 -1
View File
@@ -428,7 +428,7 @@ public class SwerveDrive extends Subsystem {
public Status diagnosticStatus() { public Status diagnosticStatus() {
Status status = new Status(); Status status = new Status();
status.addReport(ReportLevel.ERROR, status.addReport(ReportLevel.INFO,
"Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
return status; return status;
src/main/java/frc4388/robot/subsystems/Vision.java
+12
View File
@@ -36,6 +36,7 @@ import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.VisionConstants; import frc4388.robot.Constants.VisionConstants;
import frc4388.utility.Status; import frc4388.utility.Status;
import frc4388.utility.Subsystem; import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
public class Vision extends Subsystem { public class Vision extends Subsystem {
@@ -311,6 +312,17 @@ public class Vision extends Subsystem {
public Status diagnosticStatus() { public Status diagnosticStatus() {
Status status = new Status(); Status status = new Status();
if(cameras[0].isConnected())
status.addReport(ReportLevel.INFO, "Left Camera Connected");
else
status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED");
if(cameras[1].isConnected())
status.addReport(ReportLevel.INFO, "Right Camera Connected");
else
status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED");
return status; return status;
} }
src/main/java/frc4388/utility/ReefPositionHelper.java
+3 -2
View File
@@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.FieldConstants;
public class ReefPositionHelper { public class ReefPositionHelper {
@@ -79,9 +80,9 @@ public class ReefPositionHelper {
public static double getSide(Side side){ public static double getSide(Side side){
switch(side) { switch(side) {
case LEFT: case LEFT:
return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET); return -(AutoConstants.X_SCORING_POSITION_OFFSET);
case RIGHT: case RIGHT:
return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET); return (AutoConstants.X_SCORING_POSITION_OFFSET);
case CENTER: case CENTER:
return 0; return 0;
} }