自动瞄准+射球已封装 增加conveyor独立控制
This commit is contained in:
OnDYCLaptop
+1
-1
@@ -7,7 +7,7 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "redBackAndShoot"
|
||||
"pathName": "Back To Shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -7,7 +7,7 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "redBackAndShoot"
|
||||
"pathName": "Back To Shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -19,13 +19,19 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Path step1"
|
||||
"pathName": "Shoot to Climb Step1"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Path step2"
|
||||
"pathName": "Climb Step2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step3"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Red Depot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot to Shoot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -17,7 +17,7 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "redDepotPath"
|
||||
"pathName": "To Red Depot"
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -42,7 +42,7 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "depotIntakePath"
|
||||
"pathName": "Depot Intake"
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -67,7 +67,7 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "depotShootPath"
|
||||
"pathName": "Depot to Shoot"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Path step2"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,37 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Path step2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "LineBack"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Rotate"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "RotateAndLine"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
+7
-7
@@ -4,24 +4,24 @@
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.01022182786158,
|
||||
"y": 4.119503105590061
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 13.865906250551847,
|
||||
"y": 4.119503105590061
|
||||
"y": 4.0
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.59,
|
||||
"y": 4.12
|
||||
"x": 13.5,
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 13.321071353115471,
|
||||
"y": 4.12
|
||||
"x": 13.231071353115471,
|
||||
"y": 4.0
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -45,7 +45,7 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
+5
-5
@@ -8,8 +8,8 @@
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 15.650000000000002,
|
||||
"y": 3.365
|
||||
"x": 15.610835339716738,
|
||||
"y": 3.3590426144037364
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
@@ -20,8 +20,8 @@
|
||||
"y": 3.365
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.935706120733446,
|
||||
"y": 3.365
|
||||
"x": 15.743555432745389,
|
||||
"y": 3.3550226257587115
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -45,7 +45,7 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.467,
|
||||
"y": 3.365
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 15.716589593303949,
|
||||
"y": 3.3506809597259686
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.467,
|
||||
"y": 3.465
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.191301517038063,
|
||||
"y": 3.4933990509259307
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.25,
|
||||
"maxAcceleration": 0.5,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
+1
-1
@@ -45,7 +45,7 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
+5
-5
@@ -8,8 +8,8 @@
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 16.145956799790877,
|
||||
"y": 1.9416406158462824
|
||||
"x": 16.024211489072027,
|
||||
"y": 1.797481861769637
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
@@ -20,8 +20,8 @@
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 13.28182215800934,
|
||||
"y": 4.126069541450674
|
||||
"x": 13.313026105373341,
|
||||
"y": 4.165954098256533
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -45,7 +45,7 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 6.668017118402283
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.299415121255347,
|
||||
"y": 7.128787446504993
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Point1"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.299415121255347,
|
||||
"y": 4.597831669044222
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "StartingPointRed2"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.064,
|
||||
"y": 4.15
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "StartingPointRed1"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.650181888846735,
|
||||
"y": 4.15
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.312353780313838,
|
||||
"y": 4.74015691868759
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "StartingPointRed1"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 6.357489300998574
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.312353780313838,
|
||||
"y": 6.098716119828817
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+11
-11
@@ -3,13 +3,13 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.59,
|
||||
"y": 4.12
|
||||
"x": 13.5,
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.266378040870812,
|
||||
"y": 3.6432882001482505
|
||||
"x": 13.951314803780512,
|
||||
"y": 3.7446544288446453
|
||||
},
|
||||
"isLocked": true,
|
||||
"linkedName": null
|
||||
@@ -20,12 +20,12 @@
|
||||
"y": 3.3650694444444444
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.336543591648573,
|
||||
"y": 3.4619706510920394
|
||||
"x": 14.32971291116739,
|
||||
"y": 3.4437767342805516
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 14.797456408351428,
|
||||
"y": 3.2681682377968495
|
||||
"x": 14.804287088832611,
|
||||
"y": 3.2863621546083372
|
||||
},
|
||||
"isLocked": true,
|
||||
"linkedName": null
|
||||
@@ -36,8 +36,8 @@
|
||||
"y": 3.365
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.652649792454762,
|
||||
"y": 3.401302545741686
|
||||
"x": 15.593954129696199,
|
||||
"y": 3.387182687738287
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -61,7 +61,7 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.5,
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 13.72461981440462,
|
||||
"y": 3.8902460070118927
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.400452528841061,
|
||||
"y": 2.1
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.201612527520673,
|
||||
"y": 2.25153433233069
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+1
-1
@@ -45,7 +45,7 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
@@ -2,7 +2,9 @@
|
||||
"robotWidth": 0.85,
|
||||
"robotLength": 0.85,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [],
|
||||
"pathFolders": [
|
||||
"Red Middle"
|
||||
],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 1.5,
|
||||
"defaultMaxAccel": 3.0,
|
||||
|
||||
@@ -149,6 +149,7 @@ public final class Constants {
|
||||
|
||||
public static double climbUpTicks = 94; // TODO: 实测后调整
|
||||
public static double climbDownTicks = 0; // TODO: 实测后调整
|
||||
public static double climbDownAutoTicks = -5; // TODO: 实测后调整，自动爬时可能不需要完全降到底
|
||||
public static final double climbVelocity = -200;
|
||||
public static final double holdToleranceRot = 0.5;
|
||||
public static final double deltaDownTicks = 1;
|
||||
|
||||
@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
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.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
@@ -76,7 +77,8 @@ public class RobotContainer {
|
||||
climberSubsystem,
|
||||
IntakerSubsystem,
|
||||
drivetrain,
|
||||
shooterSubsystem
|
||||
shooterSubsystem,
|
||||
driveControls
|
||||
);
|
||||
|
||||
SignalLogger.enableAutoLogging(false);
|
||||
@@ -162,7 +164,8 @@ public class RobotContainer {
|
||||
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
|
||||
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
|
||||
controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
|
||||
controllerupper.b().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
|
||||
controllerupper.b().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(10), shooterSubsystem));
|
||||
controllerupper.a().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(-25), shooterSubsystem));
|
||||
}
|
||||
/* ====================== */
|
||||
/* LED 绑定 */
|
||||
|
||||
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
import frc.robot.RobotStatusManager;
|
||||
import frc.robot.subsystems.IntakerSubsystem;
|
||||
@@ -26,7 +27,9 @@ import frc.robot.Constants.RobotStatus;
|
||||
ClimberSubsystem climber,
|
||||
IntakerSubsystem intaker,
|
||||
CommandSwerveDrivetrain drivetrain,
|
||||
ShooterSubsystem shooter
|
||||
ShooterSubsystem shooter,
|
||||
DriveControls driveControls,
|
||||
ShooterControls shooterControls
|
||||
) {
|
||||
NamedCommands.registerCommand("climb_up", climb_up(climber));
|
||||
NamedCommands.registerCommand("climb_down", climb_down(climber));
|
||||
@@ -37,20 +40,22 @@ import frc.robot.Constants.RobotStatus;
|
||||
NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
|
||||
NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
|
||||
NamedCommands.registerCommand("shoot", shoot(shooter));
|
||||
NamedCommands.registerCommand("auto_aim_and_shoot", autoAimAndShoot(driveControls, shooterControls));
|
||||
}
|
||||
|
||||
public static Command climb_up(ClimberSubsystem climber) {
|
||||
return new RunCommand(() -> {
|
||||
climber.climbUp(); // 控制爬升器上升
|
||||
}, climber).withTimeout(1.0) //
|
||||
.andThen(new InstantCommand(() -> {
|
||||
}, climber).withTimeout(1.0).andThen(new RunCommand(() -> {
|
||||
climber.climbUpNonStopforAuto();
|
||||
}, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
|
||||
climber.holdPosition(); // 停止并锁定当前爬升器位置
|
||||
}, climber));
|
||||
}
|
||||
public static Command climb_down(ClimberSubsystem climber) {
|
||||
return new RunCommand(() -> {
|
||||
climber.climbDown(); // 控制爬升器下降
|
||||
}, climber).withTimeout(1.0) //
|
||||
climber.climbDownAuto(); // 控制爬升器下降
|
||||
}, climber).withTimeout(2.0) //
|
||||
.andThen(new InstantCommand(() -> {
|
||||
climber.holdPosition(); // 停止并锁定当前爬升器位置
|
||||
}, climber));
|
||||
@@ -100,7 +105,7 @@ import frc.robot.Constants.RobotStatus;
|
||||
},
|
||||
drivetrain
|
||||
).until(() -> drivetrain.isAlignedToAprilTag()));
|
||||
}
|
||||
}
|
||||
|
||||
public static Command shoot(ShooterSubsystem shooter) {
|
||||
return new RunCommand(() -> {
|
||||
@@ -125,9 +130,14 @@ import frc.robot.Constants.RobotStatus;
|
||||
//shooter.setBackboardPosition(0); // 恢复背板位置
|
||||
shooter.setConveyorSpeedByRPS(0); // 停止输送
|
||||
}, shooter)));
|
||||
|
||||
|
||||
}
|
||||
|
||||
public static Command autoAimAndShoot(DriveControls driveControls, ShooterControls shooterControl) {
|
||||
return driveControls.autoAimCommand()
|
||||
.andThen(shooterControl.autoShootToHubCommand());
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
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 frc.robot.Constants;
|
||||
import frc.robot.RobotStatusManager;
|
||||
@@ -414,6 +415,22 @@ public class DriveControls {
|
||||
drivetrain.stop();
|
||||
}
|
||||
}
|
||||
public Command autoAimCommand() {
|
||||
return drivetrain.applyRequest(() -> {
|
||||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||||
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
|
||||
return fieldCentric
|
||||
.withVelocityX(0.0)
|
||||
.withVelocityY(0.0)
|
||||
.withRotationalRate(vomega);
|
||||
}, () -> DriveMode.FIELD_CENTRIC)
|
||||
.until(() -> Math.abs(calculateDistanceAndRotationToHub()[1]) < 1.5)
|
||||
.withTimeout(1.0)
|
||||
.andThen(new InstantCommand(() -> {
|
||||
drivetrain.stop();
|
||||
}, drivetrain));
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -1,9 +1,13 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
import java.util.Set;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
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.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.RobotStatusManager;
|
||||
@@ -147,8 +151,50 @@ public class ShooterControls {
|
||||
flywheelSpeedOffset = 0.0;
|
||||
backboardPositionOffset = 0.0;
|
||||
}
|
||||
public void startconveyor(){
|
||||
shooterSubsystem.setConveyorSpeedByRPS(10);
|
||||
|
||||
public Command autoShootToHubCommand() {
|
||||
return Commands.defer(() -> {
|
||||
double x = shooterSubsystem.getDistanceToHub();
|
||||
x = Math.max(1.2, Math.min(4.5, x));
|
||||
|
||||
double flywheelTargetSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
|
||||
double backboardTargetPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
|
||||
double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed;
|
||||
|
||||
return new RunCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0.0);
|
||||
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
}, shooterSubsystem)
|
||||
.until(() ->
|
||||
shooterSubsystem.isBackboardAtTarget()
|
||||
&& shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
|
||||
)
|
||||
.withTimeout(1.0)
|
||||
.andThen(new RunCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
|
||||
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) {
|
||||
shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed);
|
||||
} else {
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0.0);
|
||||
}
|
||||
}, shooterSubsystem).withTimeout(3.0))
|
||||
.andThen(new InstantCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(0.0);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0.0);
|
||||
shooterSubsystem.backboardMotor.set(0.0);
|
||||
}, shooterSubsystem));
|
||||
}, Set.of(shooterSubsystem));
|
||||
}
|
||||
}
|
||||
@@ -70,6 +70,10 @@ public class ClimberSubsystem extends SubsystemBase {
|
||||
targetPositionTicks = Constants.Climber.climbDownTicks;
|
||||
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
|
||||
}
|
||||
public void climbDownAuto() {
|
||||
targetPositionTicks = Constants.Climber.climbDownAutoTicks;
|
||||
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
|
||||
}
|
||||
|
||||
public void climbDownNonStop(){
|
||||
climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
|
||||
@@ -78,6 +82,9 @@ public class ClimberSubsystem extends SubsystemBase {
|
||||
public void climbUpNonStop(){
|
||||
climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
|
||||
}
|
||||
public void climbUpNonStopforAuto(){
|
||||
climberMotor.set(0.07);
|
||||
}
|
||||
|
||||
public void captureHoldPosition() {
|
||||
holdPositionTicks = getPositionTicks();
|
||||
|
||||
Reference in New Issue
Block a user