7.9晚
This commit is contained in:
OnDYCLaptop
+855
-51
@@ -1821,28 +1821,41 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Goal Position",
|
||||
"title": "Position Enable",
|
||||
"x": 1024.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Toggle Switch",
|
||||
"properties": {
|
||||
"topic": "/Tuning/backboard/backboardPositionEnable",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Position",
|
||||
"x": 1280.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/backboardGoalPosition",
|
||||
"topic": "/Tuning/backboard/backboardTargetPosition",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "At Target",
|
||||
"x": 1280.0,
|
||||
"y": 0.0,
|
||||
"title": "Position Active",
|
||||
"x": 1024.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Shooter/isBackboardAtTarget",
|
||||
"topic": "/Tuning/backboard/backboardPositionEnableActive",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
@@ -1852,36 +1865,19 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Position Offset",
|
||||
"x": 1024.0,
|
||||
"title": "Clamped Target",
|
||||
"x": 1280.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Superstructure/backboardPositionOffset",
|
||||
"topic": "/Tuning/backboard/backboardClampedTargetPosition",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Can Shoot",
|
||||
"x": 1280.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Superstructure/canShoot",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Goal Error",
|
||||
"x": 0.0,
|
||||
@@ -2281,43 +2277,50 @@
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Position Enable",
|
||||
"name": "Flywheel Velocity Tuning",
|
||||
"grid_layout": {
|
||||
"layouts": [],
|
||||
"containers": [
|
||||
{
|
||||
"title": "Apply Gains",
|
||||
"x": 0.0,
|
||||
"y": 896.0,
|
||||
"width": 256.0,
|
||||
"y": 0.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Toggle Switch",
|
||||
"properties": {
|
||||
"topic": "/Tuning/backboard/backboardPositionEnable",
|
||||
"topic": "/Tuning/flywheel/flywheelEnable",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Position",
|
||||
"x": 256.0,
|
||||
"y": 896.0,
|
||||
"width": 256.0,
|
||||
"title": "Velocity Test",
|
||||
"x": 128.0,
|
||||
"y": 0.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"type": "Toggle Switch",
|
||||
"properties": {
|
||||
"topic": "/Tuning/backboard/backboardTargetPosition",
|
||||
"topic": "/Tuning/flywheel/flywheelVelocityEnable",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
"data_type": "boolean"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Position Active",
|
||||
"x": 512.0,
|
||||
"y": 896.0,
|
||||
"width": 256.0,
|
||||
"title": "Test Active",
|
||||
"x": 256.0,
|
||||
"y": 0.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Tuning/backboard/backboardPositionEnableActive",
|
||||
"topic": "/Tuning/flywheel/flywheelVelocityEnableActive",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
@@ -2327,14 +2330,76 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Commanded Target",
|
||||
"x": 768.0,
|
||||
"y": 896.0,
|
||||
"title": "At Velocity",
|
||||
"x": 448.0,
|
||||
"y": 0.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelAtVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Opposed Follower",
|
||||
"x": 640.0,
|
||||
"y": 0.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelFollowerOpposed",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Velocity",
|
||||
"x": 0.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/backboard/backboardCommandedTargetPosition",
|
||||
"topic": "/Tuning/flywheel/flywheelTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Tolerance",
|
||||
"x": 256.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelVelocityTolerance",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Commanded Target",
|
||||
"x": 512.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelCommandedTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
@@ -2342,17 +2407,756 @@
|
||||
},
|
||||
{
|
||||
"title": "Clamped Target",
|
||||
"x": 1024.0,
|
||||
"y": 896.0,
|
||||
"x": 768.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/backboard/backboardClampedTargetPosition",
|
||||
"topic": "/Tuning/flywheel/flywheelClampedTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Active Tolerance",
|
||||
"x": 1024.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelVelocityToleranceActive",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kP",
|
||||
"x": 0.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelkP",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kI",
|
||||
"x": 128.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelkI",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kD",
|
||||
"x": 256.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelkD",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kV",
|
||||
"x": 384.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelkV",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kA",
|
||||
"x": 512.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelkA",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kS",
|
||||
"x": 640.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelkS",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Velocity Now",
|
||||
"x": 768.0,
|
||||
"y": 320.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity Error",
|
||||
"x": 1024.0,
|
||||
"y": 320.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelVelocityError",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Average Velocity",
|
||||
"x": 0.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4278238420,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Velocity",
|
||||
"x": 384.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelTargetVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4283443184,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity Error",
|
||||
"x": 768.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelVelocityError",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4294198070,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Output Volts",
|
||||
"x": 1152.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelOutputVolts",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4289374890,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Left Velocity",
|
||||
"x": 0.0,
|
||||
"y": 768.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/leftFlywheelVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4278238420,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Right Velocity",
|
||||
"x": 384.0,
|
||||
"y": 768.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/rightFlywheelVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4283443184,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity Difference",
|
||||
"x": 768.0,
|
||||
"y": 768.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelVelocityDifference",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4294198070,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Left Current",
|
||||
"x": 1152.0,
|
||||
"y": 768.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/leftFlywheelStatorCurrent",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4289374890,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Right Current",
|
||||
"x": 1152.0,
|
||||
"y": 1024.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/rightFlywheelStatorCurrent",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4278238420,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Volts Estimate",
|
||||
"x": 0.0,
|
||||
"y": 1024.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelTargetVoltage",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Left Output Volts",
|
||||
"x": 256.0,
|
||||
"y": 1024.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/leftFlywheelOutputVolts",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Right Output Volts",
|
||||
"x": 512.0,
|
||||
"y": 1024.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/rightFlywheelOutputVolts",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Left Output Volts Graph",
|
||||
"x": 0.0,
|
||||
"y": 1152.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/leftFlywheelOutputVolts",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4283443184,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Right Output Volts Graph",
|
||||
"x": 384.0,
|
||||
"y": 1152.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/rightFlywheelOutputVolts",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4289374890,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity Offset",
|
||||
"x": 768.0,
|
||||
"y": 1024.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Superstructure/flywheelVelocityOffset",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Can Shoot",
|
||||
"x": 1024.0,
|
||||
"y": 1024.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Superstructure/canShoot",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Shooter Ready",
|
||||
"x": 1216.0,
|
||||
"y": 1024.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Superstructure/shooterAtVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "Shooter Conveyor Velocity Tuning",
|
||||
"grid_layout": {
|
||||
"layouts": [],
|
||||
"containers": [
|
||||
{
|
||||
"title": "Apply Gains",
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Toggle Switch",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorEnable",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity Test",
|
||||
"x": 128.0,
|
||||
"y": 0.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Toggle Switch",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorVelocityEnable",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Test Active",
|
||||
"x": 256.0,
|
||||
"y": 0.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorVelocityEnableActive",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Subsystem Active",
|
||||
"x": 448.0,
|
||||
"y": 0.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorVelocityTuningActive",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Superstructure Holdoff",
|
||||
"x": 640.0,
|
||||
"y": 0.0,
|
||||
"width": 192.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Superstructure/shooterConveyorTuningControlActive",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Velocity",
|
||||
"x": 0.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Commanded Target",
|
||||
"x": 256.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorCommandedTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Clamped Target",
|
||||
"x": 512.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorClampedTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Velocity Now",
|
||||
"x": 768.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorTargetVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity Error",
|
||||
"x": 1024.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorVelocityError",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kP",
|
||||
"x": 0.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorkP",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kI",
|
||||
"x": 128.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorkI",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kD",
|
||||
"x": 256.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorkD",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kV",
|
||||
"x": 384.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorkV",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kA",
|
||||
"x": 512.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorkA",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "kS",
|
||||
"x": 640.0,
|
||||
"y": 320.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/conveyor/conveyorkS",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Output Volts",
|
||||
"x": 768.0,
|
||||
"y": 320.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/shooterConveyorOutputVolts",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Stator Current",
|
||||
"x": 1024.0,
|
||||
"y": 320.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/statorCurrent",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity",
|
||||
"x": 0.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4278238420,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Target Velocity",
|
||||
"x": 384.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorTargetVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4283443184,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Velocity Error",
|
||||
"x": 768.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorVelocityError",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4294198070,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Output Volts",
|
||||
"x": 1152.0,
|
||||
"y": 512.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/shooterConveyorOutputVolts",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4289374890,
|
||||
"line_width": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Stator Current",
|
||||
"x": 0.0,
|
||||
"y": 768.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/statorCurrent",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 8.0,
|
||||
"color": 4278238420,
|
||||
"line_width": 2.0
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -18,30 +18,52 @@ public final class Constants {
|
||||
public static final int rightConveyorID = 37;
|
||||
public static final int backboardMotorID = 44;
|
||||
|
||||
public static final double flywheelNominalKv = 0.12;
|
||||
public static final double defaultFlywheelVoltage = 3.34;
|
||||
public static final double flywheelReadyVoltageThreshold = 1.0;
|
||||
public static final double defaultFlywheelVelocity = defaultFlywheelVoltage / flywheelNominalKv;
|
||||
public static final double flywheelMaxVelocity = 120.0;
|
||||
public static final double flywheelReadyVelocityThreshold = 5.0;
|
||||
public static final double flywheelReadyVelocityTolerance = 2.0;
|
||||
public static final double flywheelReadyDelaySeconds = 0.5;
|
||||
|
||||
// 距离(米) -> 飞轮电压(V) 射表。当前为占位点；
|
||||
// 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。
|
||||
public static final InterpolatingDoubleTreeMap flywheelVoltageByDistance = new InterpolatingDoubleTreeMap();
|
||||
static {
|
||||
flywheelVoltageByDistance.put(1.2, 5.82);
|
||||
flywheelVoltageByDistance.put(2.5, 6.34);
|
||||
flywheelVoltageByDistance.put(3.5, 6.73);
|
||||
flywheelVoltageByDistance.put(5.0, 7.33);
|
||||
flywheelVoltageByDistance.put(1.2, 2.0);
|
||||
flywheelVoltageByDistance.put(2.5, 2.1);
|
||||
flywheelVoltageByDistance.put(3.5, 2.2);
|
||||
flywheelVoltageByDistance.put(5.0, 2.3);
|
||||
}
|
||||
|
||||
public static final InterpolatingDoubleTreeMap flywheelVelocityByDistance = new InterpolatingDoubleTreeMap();
|
||||
static {
|
||||
flywheelVelocityByDistance.put(1.2, 2.0 / flywheelNominalKv);
|
||||
flywheelVelocityByDistance.put(2.5, 2.1 / flywheelNominalKv);
|
||||
flywheelVelocityByDistance.put(3.5, 2.2 / flywheelNominalKv);
|
||||
flywheelVelocityByDistance.put(5.0, 2.3 / flywheelNominalKv);
|
||||
}
|
||||
|
||||
public static final double shooterConveyorFeedVoltage = 3.0;
|
||||
public static final double shooterConveyorReverseVoltage = -2.0;
|
||||
public static final double shooterConveyorNominalKv = 0.125;
|
||||
public static final double shooterConveyorMaxVelocity = 12.0 / shooterConveyorNominalKv;
|
||||
public static final double shooterConveyorFeedVelocity =
|
||||
shooterConveyorFeedVoltage / shooterConveyorNominalKv;
|
||||
public static final double shooterConveyorReverseVelocity =
|
||||
shooterConveyorReverseVoltage / shooterConveyorNominalKv;
|
||||
|
||||
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.0).withKI(0).withKD(0)
|
||||
.withKV(0.099).withKA(0.0).withKS(0.0);
|
||||
.withKP(0.21)
|
||||
.withKI(0)
|
||||
.withKD(0.004)
|
||||
.withKS(0.25)
|
||||
.withKA(0.0)
|
||||
.withKV(flywheelNominalKv);
|
||||
|
||||
public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.0).withKI(0).withKD(0)
|
||||
.withKV(0.16).withKA(0.15).withKS(0);
|
||||
.withKP(0.19).withKI(0).withKD(0.001)
|
||||
.withKV(shooterConveyorNominalKv).withKA(0.0).withKS(0.07);
|
||||
|
||||
public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.01).withKI(0).withKD(0)
|
||||
@@ -65,7 +87,7 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static class AutoPositioning {
|
||||
public static final double autoRotationkP = 0.08;
|
||||
public static final double autoRotationkP = 0.07;
|
||||
public static final double autoRotationForBumpTargetDegrees = 45;
|
||||
public static final double autoRotationForTrenchTargetDegrees = 0;
|
||||
public static final boolean teleopOffsetEnabled = true;
|
||||
@@ -130,9 +152,6 @@ public final class Constants {
|
||||
public static final double intakeRotaterRaiseVoltage = 0.8;
|
||||
public static final double intakeRotaterLowerVoltage = 0.05;
|
||||
|
||||
public static final double intakeRotaterShootAssistPeriodSeconds = 0.4;
|
||||
public static final double intakeRotaterShootAssistUpDutyCycle = 0.5;
|
||||
|
||||
public static final Slot0Configs intakeRollerSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
@@ -160,14 +179,18 @@ public final class Constants {
|
||||
// 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
|
||||
public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage;
|
||||
public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage;
|
||||
public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity;
|
||||
public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity;
|
||||
public static final double shootTimeoutSeconds = 1.0;
|
||||
public static final double aimToleranceDegrees = 1.5;
|
||||
public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
|
||||
public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage;
|
||||
public static final double passBallFlywheelVelocity = Shooter.defaultFlywheelVelocity;
|
||||
public static final double passBallBackboardPosition = 175.0;
|
||||
|
||||
// 射击时背板的目标位置（外接编码器计数）。
|
||||
public static final double backboardShootPosition = 175.0;
|
||||
public static final double backboardReadyPositionTolerance = 100.0;
|
||||
// 射击门控是否要求底盘接近静止。
|
||||
public static final boolean stationaryGateEnabled = true;
|
||||
}
|
||||
|
||||
@@ -181,8 +181,8 @@ public class RobotContainer {
|
||||
controllerupper.b().and(teleopOnly)
|
||||
.onTrue(operatorControls.requestEjectCommand())
|
||||
.onFalse(operatorControls.requestIdleCommand());
|
||||
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(-0.1)));
|
||||
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(0.1)));
|
||||
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0)));
|
||||
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0)));
|
||||
controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
|
||||
controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
|
||||
controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets()));
|
||||
@@ -200,12 +200,15 @@ public class RobotContainer {
|
||||
public void tunerPeriodic() {
|
||||
driveGainsTuner.periodic();
|
||||
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
|
||||
shooterSubsystem.debugFlywheelVelocityPeriodic();
|
||||
configConveyorTuner.periodic(null);
|
||||
shooterSubsystem.debugShooterConveyorVelocityPeriodic();
|
||||
configBackboardTuner.periodic();
|
||||
shooterSubsystem.debugBackboardTargetPeriodic();
|
||||
configMainConveyorTuner.periodic(null);
|
||||
configIntakerRollerTuner.periodic(null);
|
||||
configIntakerRotaterTuner.periodic(null);
|
||||
shooterSubsystem.setRightFlywheelFollowLeft();
|
||||
shooterSubsystem.setRightConveyorFollowLeft();
|
||||
}
|
||||
|
||||
|
||||
@@ -244,7 +244,11 @@ public class DriveControls {
|
||||
|
||||
// If angle difference is within threshold, don't rotate
|
||||
if (Math.abs(calDifference) > 1.0) {
|
||||
return calDifference * Constants.AutoPositioning.autoRotationkP;
|
||||
double rotationRate = calDifference * Constants.AutoPositioning.autoRotationkP;
|
||||
if (Math.abs(rotationRate) < 0.24) {
|
||||
return Math.copySign(0.24, rotationRate);
|
||||
}
|
||||
return rotationRate;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -54,6 +54,10 @@ public class OperatorControls {
|
||||
superstructure.adjustFlywheelVoltageOffset(offset);
|
||||
}
|
||||
|
||||
public void adjustFlywheelVelocityOffset(double offset) {
|
||||
superstructure.adjustFlywheelVelocityOffset(offset);
|
||||
}
|
||||
|
||||
public void adjustBackboardRateOffset(double offset) {
|
||||
superstructure.adjustBackboardPositionOffset(offset);
|
||||
}
|
||||
|
||||
@@ -69,12 +69,20 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
|
||||
}
|
||||
|
||||
public void shootAssist() {
|
||||
double period = Constants.Intaker.intakeRotaterShootAssistPeriodSeconds;
|
||||
double phase = (Timer.getFPGATimestamp() % period) / period;
|
||||
double volts = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle
|
||||
? Constants.Intaker.intakeRotaterRaiseVoltage
|
||||
: Constants.Intaker.intakeRotaterLowerVoltage;
|
||||
setVoltage(volts);
|
||||
double cycleTime = Timer.getFPGATimestamp() % 1.7;
|
||||
|
||||
if (cycleTime < 0.7) {
|
||||
setVoltage(1);
|
||||
} else if (cycleTime < 0.9) {
|
||||
setVoltage(0.0);
|
||||
} else {
|
||||
setVoltage(0.03);
|
||||
}
|
||||
}
|
||||
|
||||
public void postShootIdleAssist(double elapsedSeconds) {
|
||||
double cycleTime = elapsedSeconds % 0.2;
|
||||
setVoltage(cycleTime < 0.1 ? 0.03 : 0.0);
|
||||
}
|
||||
|
||||
public void manualRaise() {
|
||||
|
||||
@@ -51,4 +51,8 @@ public class LimelightSubsystem extends SubsystemBase {
|
||||
// 查 Constants 里的插值射表；真车试射后往表里加实测点即可。
|
||||
return Constants.Shooter.flywheelVoltageByDistance.get(getDistanceToTarget());
|
||||
}
|
||||
|
||||
public double getShooterVelocityByDistance() {
|
||||
return Constants.Shooter.flywheelVelocityByDistance.get(getDistanceToTarget());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3,6 +3,7 @@ package frc.robot.subsystems;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.controls.VoltageOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.hardware.TalonFXS;
|
||||
@@ -39,18 +40,25 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
public final Encoder backboardEncoder;
|
||||
public final ProfiledPIDController backboardPID;
|
||||
|
||||
private final VoltageOut leftFlywheelVoltageRequest = new VoltageOut(0);
|
||||
private final VoltageOut rightFlywheelVoltageRequest = new VoltageOut(0);
|
||||
private final VoltageOut conveyorVoltageRequest = new VoltageOut(0);
|
||||
private final VelocityVoltage leftFlywheelVelocityRequest = new VelocityVoltage(0).withSlot(0);
|
||||
private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0).withSlot(0);
|
||||
private final VoltageOut backboardVoltageRequest = new VoltageOut(0);
|
||||
|
||||
private final Follower flywheelFollower;
|
||||
private final Follower conveyorFollower;
|
||||
|
||||
private final NetworkTable shooterNetworkTable;
|
||||
private final NetworkTable flywheelTuningTable;
|
||||
private final NetworkTable conveyorTuningTable;
|
||||
private final NetworkTable backboardTuningTable;
|
||||
|
||||
private double flywheelOutputVolts = 0.0;
|
||||
private double flywheelVoltageStartTimestamp = 0.0;
|
||||
private double flywheelTargetVelocity = 0.0;
|
||||
private double flywheelVelocityTolerance = Constants.Shooter.flywheelReadyVelocityTolerance;
|
||||
private double flywheelVelocityReadyStartTimestamp = 0.0;
|
||||
private boolean flywheelVelocityTuningActive = false;
|
||||
private double shooterConveyorTargetVelocity = 0.0;
|
||||
private boolean shooterConveyorVelocityTuningActive = false;
|
||||
private double shooterConveyorOutputVolts = 0.0;
|
||||
private double backboardOutputVolts = 0.0;
|
||||
private double backboardRawPidOutputVolts = 0.0;
|
||||
@@ -75,7 +83,9 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
leftFlywheel.getConfigurator().apply(motorOutputConfigs);
|
||||
leftConveyor.getConfigurator().apply(motorOutputConfigs);
|
||||
|
||||
flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
setRightFlywheelFollowLeft();
|
||||
setRightConveyorFollowLeft();
|
||||
|
||||
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
|
||||
@@ -95,6 +105,13 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
backboardPID.setTolerance(10.0);
|
||||
|
||||
shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
|
||||
flywheelTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/flywheel");
|
||||
flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false);
|
||||
flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity);
|
||||
flywheelTuningTable.getEntry("flywheelVelocityTolerance").setDefaultDouble(Constants.Shooter.flywheelReadyVelocityTolerance);
|
||||
conveyorTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/conveyor");
|
||||
conveyorTuningTable.getEntry("conveyorVelocityEnable").setDefaultBoolean(false);
|
||||
conveyorTuningTable.getEntry("conveyorTargetVelocity").setDefaultDouble(Constants.Shooter.shooterConveyorFeedVelocity);
|
||||
backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
|
||||
backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
|
||||
backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
|
||||
@@ -109,6 +126,10 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
rightConveyor.setControl(conveyorFollower);
|
||||
}
|
||||
|
||||
public void setRightFlywheelFollowLeft() {
|
||||
rightFlywheel.setControl(flywheelFollower);
|
||||
}
|
||||
|
||||
public void applyLeftConfigurationToRight() {
|
||||
Slot0Configs configs = new Slot0Configs();
|
||||
leftFlywheel.getConfigurator().refresh(configs);
|
||||
@@ -117,29 +138,51 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
|
||||
public void setFlywheelVoltage(double outputVolts) {
|
||||
double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||||
if (Math.abs(clampedOutputVolts - flywheelOutputVolts) > 1e-6) {
|
||||
flywheelVoltageStartTimestamp = Timer.getFPGATimestamp();
|
||||
setFlywheelVelocity(clampedOutputVolts / Constants.Shooter.flywheelNominalKv);
|
||||
}
|
||||
flywheelOutputVolts = clampedOutputVolts;
|
||||
leftFlywheel.setControl(leftFlywheelVoltageRequest.withOutput(flywheelOutputVolts));
|
||||
rightFlywheel.setControl(rightFlywheelVoltageRequest.withOutput(-flywheelOutputVolts));
|
||||
|
||||
public void setFlywheelVelocity(double targetVelocityRps) {
|
||||
double clampedTargetVelocity = MathUtil.clamp(
|
||||
targetVelocityRps,
|
||||
-Constants.Shooter.flywheelMaxVelocity,
|
||||
Constants.Shooter.flywheelMaxVelocity
|
||||
);
|
||||
if (Math.abs(clampedTargetVelocity - flywheelTargetVelocity) > 1e-6) {
|
||||
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
|
||||
}
|
||||
flywheelTargetVelocity = clampedTargetVelocity;
|
||||
leftFlywheel.setControl(leftFlywheelVelocityRequest.withVelocity(flywheelTargetVelocity));
|
||||
setRightFlywheelFollowLeft();
|
||||
}
|
||||
|
||||
public void setFlywheelVoltageByDistance(double distance) {
|
||||
double targetVolts = Constants.Shooter.flywheelVoltageByDistance.get(distance);
|
||||
setFlywheelVoltage(targetVolts);
|
||||
setFlywheelVelocityByDistance(distance);
|
||||
}
|
||||
|
||||
public void setFlywheelVelocityByDistance(double distance) {
|
||||
double targetVelocity = Constants.Shooter.flywheelVelocityByDistance.get(distance);
|
||||
setFlywheelVelocity(targetVelocity);
|
||||
}
|
||||
|
||||
public void stopFlywheel() {
|
||||
setFlywheelVoltage(0.0);
|
||||
setFlywheelVelocity(0.0);
|
||||
}
|
||||
|
||||
public boolean atVelocity() {
|
||||
if (Math.abs(flywheelTargetVelocity) < Constants.Shooter.flywheelReadyVelocityThreshold) {
|
||||
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
|
||||
return false;
|
||||
}
|
||||
if (Math.abs(getFlywheelVelocityError()) > flywheelVelocityTolerance) {
|
||||
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
|
||||
return false;
|
||||
}
|
||||
return Timer.getFPGATimestamp() - flywheelVelocityReadyStartTimestamp
|
||||
>= Constants.Shooter.flywheelReadyDelaySeconds;
|
||||
}
|
||||
|
||||
public boolean atVoltage() {
|
||||
if (Math.abs(flywheelOutputVolts) < Constants.Shooter.flywheelReadyVoltageThreshold) {
|
||||
return false;
|
||||
}
|
||||
return Timer.getFPGATimestamp() - flywheelVoltageStartTimestamp
|
||||
>= Constants.Shooter.flywheelReadyDelaySeconds;
|
||||
return atVelocity();
|
||||
}
|
||||
public double getLeftFlywheelVelocity() {
|
||||
return leftFlywheel.getVelocity().getValueAsDouble();
|
||||
@@ -157,22 +200,51 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
|
||||
}
|
||||
|
||||
public double getTargetFlywheelVelocity() {
|
||||
return flywheelTargetVelocity;
|
||||
}
|
||||
|
||||
public double getFlywheelVelocityError() {
|
||||
return Math.abs(flywheelTargetVelocity) - getAverageFlywheelVelocity();
|
||||
}
|
||||
|
||||
public double getTargetFlywheelVoltage() {
|
||||
return flywheelOutputVolts;
|
||||
return flywheelTargetVelocity * Constants.Shooter.flywheelNominalKv;
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* shooter conveyor */
|
||||
/* ====================== */
|
||||
|
||||
public void runShooterConveyorVoltage(double outputVolts) {
|
||||
shooterConveyorOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||||
leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorOutputVolts));
|
||||
public void runShooterConveyorVelocity(double targetVelocityRps) {
|
||||
shooterConveyorTargetVelocity = MathUtil.clamp(
|
||||
targetVelocityRps,
|
||||
-Constants.Shooter.shooterConveyorMaxVelocity,
|
||||
Constants.Shooter.shooterConveyorMaxVelocity
|
||||
);
|
||||
leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity));
|
||||
setRightConveyorFollowLeft();
|
||||
}
|
||||
|
||||
public void runShooterConveyorVoltage(double outputVolts) {
|
||||
double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||||
runShooterConveyorVelocity(clampedOutputVolts / Constants.Shooter.shooterConveyorNominalKv);
|
||||
}
|
||||
|
||||
public void stopShooterConveyor() {
|
||||
runShooterConveyorVoltage(0.0);
|
||||
runShooterConveyorVelocity(0.0);
|
||||
}
|
||||
|
||||
public double getShooterConveyorVelocity() {
|
||||
return leftConveyor.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getShooterConveyorTargetVelocity() {
|
||||
return shooterConveyorTargetVelocity;
|
||||
}
|
||||
|
||||
public double getShooterConveyorVelocityError() {
|
||||
return shooterConveyorTargetVelocity - getShooterConveyorVelocity();
|
||||
}
|
||||
|
||||
public double getConveyorStatorCurrent() {
|
||||
@@ -280,10 +352,36 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
return backboardPID.atGoal();
|
||||
}
|
||||
|
||||
public double getBackboardGoalPosition() {
|
||||
return backboardPID.getGoal().position;
|
||||
}
|
||||
|
||||
public double getBackboardPositionError() {
|
||||
return getBackboardGoalPosition() - getBackboardPosition();
|
||||
}
|
||||
|
||||
public boolean isBackboardNearTarget(double tolerance) {
|
||||
return Math.abs(getBackboardPositionError()) < tolerance;
|
||||
}
|
||||
|
||||
public boolean isBackboardPositionTuningActive() {
|
||||
return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
|
||||
}
|
||||
|
||||
public boolean isFlywheelTuningControlActive() {
|
||||
boolean voltageTunerEnabled = flywheelTuningTable.getEntry("flywheelEnable").getBoolean(false);
|
||||
boolean voltageTunerRunning = flywheelTuningTable.getEntry("flywheelEnableRunning").getBoolean(false);
|
||||
boolean velocityTunerEnabled = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
|
||||
return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled;
|
||||
}
|
||||
|
||||
public boolean isShooterConveyorTuningControlActive() {
|
||||
boolean voltageTunerEnabled = conveyorTuningTable.getEntry("conveyorEnable").getBoolean(false);
|
||||
boolean voltageTunerRunning = conveyorTuningTable.getEntry("conveyorEnableRunning").getBoolean(false);
|
||||
boolean velocityTunerEnabled = conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
|
||||
return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled;
|
||||
}
|
||||
|
||||
public void resetbackboardencoder() {
|
||||
backboardEncoder.reset();
|
||||
backboardPID.reset(0);
|
||||
@@ -329,20 +427,82 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
|
||||
public void debugFlywheelVelocityPeriodic() {
|
||||
flywheelVelocityTuningActive = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
|
||||
double requestedTargetVelocity =
|
||||
flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity);
|
||||
flywheelVelocityTolerance = Math.max(
|
||||
0.0,
|
||||
flywheelTuningTable.getEntry("flywheelVelocityTolerance")
|
||||
.getDouble(Constants.Shooter.flywheelReadyVelocityTolerance)
|
||||
);
|
||||
|
||||
double clampedTargetVelocity = MathUtil.clamp(
|
||||
requestedTargetVelocity,
|
||||
-Constants.Shooter.flywheelMaxVelocity,
|
||||
Constants.Shooter.flywheelMaxVelocity
|
||||
);
|
||||
flywheelTuningTable.getEntry("flywheelVelocityEnableActive").setBoolean(flywheelVelocityTuningActive);
|
||||
flywheelTuningTable.getEntry("flywheelCommandedTargetVelocity").setDouble(requestedTargetVelocity);
|
||||
flywheelTuningTable.getEntry("flywheelClampedTargetVelocity").setDouble(clampedTargetVelocity);
|
||||
flywheelTuningTable.getEntry("flywheelVelocityToleranceActive").setDouble(flywheelVelocityTolerance);
|
||||
|
||||
if (flywheelVelocityTuningActive) {
|
||||
setFlywheelVelocity(clampedTargetVelocity);
|
||||
}
|
||||
}
|
||||
|
||||
public void debugShooterConveyorVelocityPeriodic() {
|
||||
shooterConveyorVelocityTuningActive =
|
||||
conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
|
||||
double requestedTargetVelocity =
|
||||
conveyorTuningTable.getEntry("conveyorTargetVelocity")
|
||||
.getDouble(Constants.Shooter.shooterConveyorFeedVelocity);
|
||||
double clampedTargetVelocity = MathUtil.clamp(
|
||||
requestedTargetVelocity,
|
||||
-Constants.Shooter.shooterConveyorMaxVelocity,
|
||||
Constants.Shooter.shooterConveyorMaxVelocity
|
||||
);
|
||||
|
||||
conveyorTuningTable.getEntry("conveyorVelocityEnableActive")
|
||||
.setBoolean(shooterConveyorVelocityTuningActive);
|
||||
conveyorTuningTable.getEntry("conveyorCommandedTargetVelocity").setDouble(requestedTargetVelocity);
|
||||
conveyorTuningTable.getEntry("conveyorClampedTargetVelocity").setDouble(clampedTargetVelocity);
|
||||
|
||||
if (shooterConveyorVelocityTuningActive) {
|
||||
runShooterConveyorVelocity(clampedTargetVelocity);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
double backboardPosition = getBackboardPosition();
|
||||
double backboardGoalPosition = backboardPID.getGoal().position;
|
||||
double backboardSetpointPosition = backboardPID.getSetpoint().position;
|
||||
flywheelOutputVolts = leftFlywheel.getMotorVoltage().getValueAsDouble();
|
||||
shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference());
|
||||
shooterNetworkTable.getEntry("flywheelTargetVelocity").setDouble(getTargetFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("flywheelVelocityError").setDouble(getFlywheelVelocityError());
|
||||
shooterNetworkTable.getEntry("flywheelVelocityTolerance").setDouble(flywheelVelocityTolerance);
|
||||
shooterNetworkTable.getEntry("flywheelAtVelocity").setBoolean(atVelocity());
|
||||
shooterNetworkTable.getEntry("flywheelVelocityTuningActive").setBoolean(flywheelVelocityTuningActive);
|
||||
shooterNetworkTable.getEntry("flywheelFollowerOpposed").setBoolean(true);
|
||||
shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage());
|
||||
shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts);
|
||||
shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage());
|
||||
shooterNetworkTable.getEntry("leftFlywheelOutputVolts").setDouble(leftFlywheel.getMotorVoltage().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("rightFlywheelOutputVolts").setDouble(rightFlywheel.getMotorVoltage().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("leftFlywheelStatorCurrent").setDouble(leftFlywheel.getStatorCurrent().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("rightFlywheelStatorCurrent").setDouble(rightFlywheel.getStatorCurrent().getValueAsDouble());
|
||||
shooterConveyorOutputVolts = leftConveyor.getMotorVoltage().getValueAsDouble();
|
||||
shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts);
|
||||
shooterNetworkTable.getEntry("conveyorVelocity").setDouble(leftConveyor.getVelocity().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("conveyorTargetVelocity").setDouble(getShooterConveyorTargetVelocity());
|
||||
shooterNetworkTable.getEntry("conveyorVelocity").setDouble(getShooterConveyorVelocity());
|
||||
shooterNetworkTable.getEntry("conveyorVelocityError").setDouble(getShooterConveyorVelocityError());
|
||||
shooterNetworkTable.getEntry("conveyorVelocityTuningActive").setBoolean(shooterConveyorVelocityTuningActive);
|
||||
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
|
||||
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
|
||||
shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);
|
||||
|
||||
@@ -52,9 +52,10 @@ public class Superstructure extends SubsystemBase {
|
||||
private WantedState wantedState = WantedState.IDLE;
|
||||
private SystemState systemState = SystemState.STOWED;
|
||||
private double stateStartTimestamp = Timer.getFPGATimestamp();
|
||||
private double flywheelVoltageOffset = 0.0;
|
||||
private double flywheelVelocityOffset = 0.0;
|
||||
private double backboardPositionOffset = 0.0;
|
||||
private boolean shotCompletedThisRequest = false;
|
||||
private double postShootIdleAssistStartTimestamp = Double.NaN;
|
||||
|
||||
public Superstructure(
|
||||
ShooterSubsystem shooter,
|
||||
@@ -77,6 +78,9 @@ public class Superstructure extends SubsystemBase {
|
||||
/* ====================== */
|
||||
|
||||
public void requestIdle() {
|
||||
if (systemState == SystemState.SHOOTING) {
|
||||
postShootIdleAssistStartTimestamp = Timer.getFPGATimestamp();
|
||||
}
|
||||
wantedState = WantedState.IDLE;
|
||||
}
|
||||
|
||||
@@ -164,9 +168,10 @@ public class Superstructure extends SubsystemBase {
|
||||
|
||||
public boolean canShoot() {
|
||||
boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary();
|
||||
return shooter.atVoltage()
|
||||
return shooter.atVelocity()
|
||||
&& drive.isAimed()
|
||||
&& stationaryGate;
|
||||
&& stationaryGate
|
||||
&& isBackboardReady();
|
||||
}
|
||||
|
||||
private boolean isDriveStationary() {
|
||||
@@ -175,12 +180,20 @@ public class Superstructure extends SubsystemBase {
|
||||
return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond;
|
||||
}
|
||||
|
||||
private boolean isBackboardReady() {
|
||||
return shooter.isBackboardNearTarget(Constants.Superstructure.backboardReadyPositionTolerance);
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 现场微调 offset */
|
||||
/* ====================== */
|
||||
|
||||
public void adjustFlywheelVelocityOffset(double offsetVelocity) {
|
||||
flywheelVelocityOffset += offsetVelocity;
|
||||
}
|
||||
|
||||
public void adjustFlywheelVoltageOffset(double offsetVolts) {
|
||||
flywheelVoltageOffset += offsetVolts;
|
||||
adjustFlywheelVelocityOffset(offsetVolts / Constants.Shooter.flywheelNominalKv);
|
||||
}
|
||||
|
||||
public void adjustBackboardPositionOffset(double offset) {
|
||||
@@ -188,7 +201,7 @@ public class Superstructure extends SubsystemBase {
|
||||
}
|
||||
|
||||
public void resetOffsets() {
|
||||
flywheelVoltageOffset = 0.0;
|
||||
flywheelVelocityOffset = 0.0;
|
||||
backboardPositionOffset = 0.0;
|
||||
}
|
||||
|
||||
@@ -227,16 +240,33 @@ public class Superstructure extends SubsystemBase {
|
||||
// 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。
|
||||
conveyor.stop();
|
||||
intakeRoller.stop();
|
||||
if (!runPostShootIdleAssist()) {
|
||||
intakeRotater.stop();
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
}
|
||||
stopFlywheelUnlessTuning();
|
||||
stopShooterConveyorUnlessTuning();
|
||||
holdBackboardStowedUnlessTuning();
|
||||
setSystemState(SystemState.STOWED);
|
||||
}
|
||||
|
||||
private boolean runPostShootIdleAssist() {
|
||||
if (Double.isNaN(postShootIdleAssistStartTimestamp)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
double elapsed = Timer.getFPGATimestamp() - postShootIdleAssistStartTimestamp;
|
||||
if (elapsed >= 0.6) {
|
||||
postShootIdleAssistStartTimestamp = Double.NaN;
|
||||
return false;
|
||||
}
|
||||
|
||||
intakeRotater.postShootIdleAssist(elapsed);
|
||||
return true;
|
||||
}
|
||||
|
||||
private void handleIntake() {
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
stopFlywheelUnlessTuning();
|
||||
stopShooterConveyorUnlessTuning();
|
||||
holdBackboardStowedUnlessTuning();
|
||||
// Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。
|
||||
intakeRoller.intake();
|
||||
@@ -247,35 +277,35 @@ public class Superstructure extends SubsystemBase {
|
||||
private void handleAimHub() {
|
||||
intakeRoller.stop();
|
||||
conveyor.stop();
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
stopFlywheelUnlessTuning();
|
||||
stopShooterConveyorUnlessTuning();
|
||||
holdBackboardStowedUnlessTuning();
|
||||
setSystemState(SystemState.STOWED);
|
||||
}
|
||||
|
||||
private void handleShootHub() {
|
||||
double targetVolts = limelight.getShooterVoltageByDistance() + flywheelVoltageOffset;
|
||||
double targetVelocity = limelight.getShooterVelocityByDistance() + flywheelVelocityOffset;
|
||||
double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
|
||||
handlePreparedFeed(targetVolts, backboardPosition);
|
||||
handlePreparedFeed(targetVelocity, backboardPosition);
|
||||
}
|
||||
|
||||
private void handlePassBall() {
|
||||
double targetVolts = Constants.Superstructure.passBallFlywheelVoltage + flywheelVoltageOffset;
|
||||
double targetVelocity = Constants.Superstructure.passBallFlywheelVelocity + flywheelVelocityOffset;
|
||||
double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
|
||||
handlePreparedFeed(targetVolts, backboardPosition);
|
||||
handlePreparedFeed(targetVelocity, backboardPosition);
|
||||
}
|
||||
|
||||
private void handlePreparedFeed(double targetVolts, double backboardPosition) {
|
||||
private void handlePreparedFeed(double targetVelocity, double backboardPosition) {
|
||||
if (systemState != SystemState.PREP_SHOOT
|
||||
&& systemState != SystemState.SHOOTING) {
|
||||
setSystemState(SystemState.PREP_SHOOT);
|
||||
}
|
||||
intakeRoller.stop();
|
||||
conveyor.stop();
|
||||
shooter.stopShooterConveyor();
|
||||
stopShooterConveyorUnlessTuning();
|
||||
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
|
||||
// 但不会绕过 Superstructure 状态机。
|
||||
shooter.setFlywheelVoltage(targetVolts);
|
||||
shooter.setFlywheelVelocity(targetVelocity);
|
||||
shooter.holdBackboardAt(backboardPosition);
|
||||
|
||||
if (systemState == SystemState.SHOOTING) {
|
||||
@@ -283,18 +313,13 @@ public class Superstructure extends SubsystemBase {
|
||||
// 避免球接触飞轮导致的掉速中断喂球。
|
||||
intakeRotater.shootAssist();
|
||||
conveyor.feedToShooter();
|
||||
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterFeedVoltage);
|
||||
|
||||
if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
|
||||
shotCompletedThisRequest = true;
|
||||
setSystemState(SystemState.PREP_SHOOT);
|
||||
}
|
||||
runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity);
|
||||
return;
|
||||
}
|
||||
|
||||
// 未开始喂球前，球路保持静止。
|
||||
conveyor.stop();
|
||||
shooter.stopShooterConveyor();
|
||||
stopShooterConveyorUnlessTuning();
|
||||
|
||||
// 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
|
||||
if (canShoot()) {
|
||||
@@ -307,8 +332,8 @@ public class Superstructure extends SubsystemBase {
|
||||
setSystemState(SystemState.EJECTING);
|
||||
intakeRoller.outtake();
|
||||
conveyor.reverse();
|
||||
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage);
|
||||
shooter.stopFlywheel();
|
||||
runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterReverseVelocity);
|
||||
stopFlywheelUnlessTuning();
|
||||
holdBackboardStowedUnlessTuning();
|
||||
}
|
||||
|
||||
@@ -318,12 +343,30 @@ public class Superstructure extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
|
||||
private void stopFlywheelUnlessTuning() {
|
||||
if (!shooter.isFlywheelTuningControlActive()) {
|
||||
shooter.stopFlywheel();
|
||||
}
|
||||
}
|
||||
|
||||
private void runShooterConveyorVelocityUnlessTuning(double targetVelocity) {
|
||||
if (!shooter.isShooterConveyorTuningControlActive()) {
|
||||
shooter.runShooterConveyorVelocity(targetVelocity);
|
||||
}
|
||||
}
|
||||
|
||||
private void stopShooterConveyorUnlessTuning() {
|
||||
if (!shooter.isShooterConveyorTuningControlActive()) {
|
||||
shooter.stopShooterConveyor();
|
||||
}
|
||||
}
|
||||
|
||||
private void handleManual() {
|
||||
// 手动模式：状态机不再驱动机构，交给现场手动/调参工具；
|
||||
// 只保持飞轮与喂球停止，避免意外射出。
|
||||
setSystemState(SystemState.MANUAL_OVERRIDE);
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
stopFlywheelUnlessTuning();
|
||||
stopShooterConveyorUnlessTuning();
|
||||
conveyor.stop();
|
||||
intakeRoller.stop();
|
||||
}
|
||||
@@ -352,11 +395,19 @@ public class Superstructure extends SubsystemBase {
|
||||
table.getEntry("systemState").setString(systemState.name());
|
||||
table.getEntry("timeInState").setDouble(timeInState());
|
||||
table.getEntry("canShoot").setBoolean(canShoot());
|
||||
table.getEntry("shooterAtVelocity").setBoolean(shooter.atVelocity());
|
||||
table.getEntry("shooterAtVoltage").setBoolean(shooter.atVoltage());
|
||||
table.getEntry("driveAimed").setBoolean(drive.isAimed());
|
||||
table.getEntry("driveHeadingError").setDouble(drive.getHeadingError());
|
||||
table.getEntry("driveStationary").setBoolean(isDriveStationary());
|
||||
table.getEntry("flywheelVoltageOffset").setDouble(flywheelVoltageOffset);
|
||||
table.getEntry("backboardReady").setBoolean(isBackboardReady());
|
||||
table.getEntry("backboardPositionError").setDouble(shooter.getBackboardPositionError());
|
||||
table.getEntry("backboardReadyTolerance").setDouble(Constants.Superstructure.backboardReadyPositionTolerance);
|
||||
table.getEntry("flywheelVelocityOffset").setDouble(flywheelVelocityOffset);
|
||||
table.getEntry("flywheelTuningControlActive").setBoolean(shooter.isFlywheelTuningControlActive());
|
||||
table.getEntry("shooterConveyorTuningControlActive")
|
||||
.setBoolean(shooter.isShooterConveyorTuningControlActive());
|
||||
table.getEntry("flywheelVoltageOffset").setDouble(flywheelVelocityOffset * Constants.Shooter.flywheelNominalKv);
|
||||
table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
package frc.robot.tuning;
|
||||
import java.util.function.Function;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.VoltageOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
@@ -25,6 +23,7 @@ public class ConfigTalonFXMotorTuner {
|
||||
private final BooleanSubscriber enableRuningSub;
|
||||
private final DoubleSubscriber runningVolts;
|
||||
private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
|
||||
private boolean wasRunning = false;
|
||||
|
||||
private final VoltageOut voltageRequest = new VoltageOut(0);
|
||||
public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
|
||||
@@ -83,9 +82,11 @@ public class ConfigTalonFXMotorTuner {
|
||||
if (enableRuningSub.get()) {;
|
||||
double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0);
|
||||
motor.setControl(voltageRequest.withOutput(volts));
|
||||
wasRunning = true;
|
||||
// System.out.println("running motor with volts: "+volts+" on motor: "+motorName);
|
||||
}else{
|
||||
}else if (wasRunning){
|
||||
motor.setControl(voltageRequest.withOutput(0));
|
||||
wasRunning = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user