mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
mirage54321
+86
-55
@@ -8,10 +8,10 @@
|
||||
"layouts": [
|
||||
{
|
||||
"title": "Tag Processed",
|
||||
"x": 384.0,
|
||||
"x": 896.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 256.0,
|
||||
"height": 384.0,
|
||||
"type": "List Layout",
|
||||
"properties": {
|
||||
"label_position": "TOP"
|
||||
@@ -55,27 +55,11 @@
|
||||
}
|
||||
],
|
||||
"containers": [
|
||||
{
|
||||
"title": "MatchTime",
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Match Time",
|
||||
"properties": {
|
||||
"topic": "/AdvantageKit/DriverStation/MatchTime",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"time_display_mode": "Minutes and Seconds",
|
||||
"red_start_time": 15,
|
||||
"yellow_start_time": 30
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "RetractedLimit",
|
||||
"x": 0.0,
|
||||
"y": 384.0,
|
||||
"width": 128.0,
|
||||
"x": 512.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
@@ -90,9 +74,9 @@
|
||||
},
|
||||
{
|
||||
"title": "Auto Chooser",
|
||||
"x": 1024.0,
|
||||
"y": 256.0,
|
||||
"width": 384.0,
|
||||
"x": 896.0,
|
||||
"y": 384.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "ComboBox Chooser",
|
||||
"properties": {
|
||||
@@ -103,9 +87,9 @@
|
||||
},
|
||||
{
|
||||
"title": "Roller Percent Output",
|
||||
"x": 0.0,
|
||||
"x": 512.0,
|
||||
"y": 256.0,
|
||||
"width": 256.0,
|
||||
"width": 384.0,
|
||||
"height": 128.0,
|
||||
"type": "Number Slider",
|
||||
"properties": {
|
||||
@@ -120,8 +104,8 @@
|
||||
},
|
||||
{
|
||||
"title": "Shooter OVERRIDE Velocity",
|
||||
"x": 128.0,
|
||||
"y": 384.0,
|
||||
"x": 768.0,
|
||||
"y": 0.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
@@ -134,8 +118,8 @@
|
||||
},
|
||||
{
|
||||
"title": "TRIM SHOOTER SPEED",
|
||||
"x": 256.0,
|
||||
"y": 256.0,
|
||||
"x": 512.0,
|
||||
"y": 384.0,
|
||||
"width": 384.0,
|
||||
"height": 128.0,
|
||||
"type": "Number Slider",
|
||||
@@ -151,8 +135,8 @@
|
||||
},
|
||||
{
|
||||
"title": "Mode",
|
||||
"x": 256.0,
|
||||
"y": 384.0,
|
||||
"x": 512.0,
|
||||
"y": 128.0,
|
||||
"width": 384.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
@@ -164,30 +148,49 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Time to rev",
|
||||
"x": 768.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Large Text Display",
|
||||
"title": "IsActive",
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/AdvantageKit/RealOutputs/Time to rev",
|
||||
"topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
|
||||
"period": 0.06,
|
||||
"data_type": "double"
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Arm angle extended",
|
||||
"x": 1152.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"title": "RemainingInShift",
|
||||
"x": 0.0,
|
||||
"y": 256.0,
|
||||
"width": 384.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"type": "Match Time",
|
||||
"properties": {
|
||||
"topic": "/SmartDashboard/Arm angle extended",
|
||||
"topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
"time_display_mode": "Minutes and Seconds",
|
||||
"red_start_time": 15,
|
||||
"yellow_start_time": 30
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Phase",
|
||||
"x": 0.0,
|
||||
"y": 384.0,
|
||||
"width": 384.0,
|
||||
"height": 128.0,
|
||||
"type": "Large Text Display",
|
||||
"properties": {
|
||||
"topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
|
||||
"period": 0.06,
|
||||
"data_type": "string"
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -527,8 +530,8 @@
|
||||
},
|
||||
{
|
||||
"title": "Shooter Idle max current",
|
||||
"x": 384.0,
|
||||
"y": 512.0,
|
||||
"x": 1024.0,
|
||||
"y": 384.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
@@ -555,8 +558,8 @@
|
||||
},
|
||||
{
|
||||
"title": "Shooter OVERRIDE Velocity",
|
||||
"x": 640.0,
|
||||
"y": 512.0,
|
||||
"x": 1024.0,
|
||||
"y": 256.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
@@ -575,12 +578,40 @@
|
||||
"grid_layout": {
|
||||
"layouts": [],
|
||||
"containers": [
|
||||
{
|
||||
"title": "Stalled Motor: ",
|
||||
"x": 512.0,
|
||||
"y": 128.0,
|
||||
"width": 384.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/AdvantageKit/RealOutputs/Stalled Motor: ",
|
||||
"period": 0.06,
|
||||
"data_type": "string",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Shooter idle % output",
|
||||
"x": 128.0,
|
||||
"y": 256.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/SmartDashboard/Shooter idle % output",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Auto Chooser",
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"width": 640.0,
|
||||
"height": 768.0,
|
||||
"x": 512.0,
|
||||
"y": 256.0,
|
||||
"width": 384.0,
|
||||
"height": 128.0,
|
||||
"type": "ComboBox Chooser",
|
||||
"properties": {
|
||||
"topic": "/SmartDashboard/Auto Chooser",
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Robot Rev Up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Robot Shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot driving across"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Robot Shoot Driving"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Intake Extended"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bump test"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "BumpOffsetForward"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "BumpToCenter"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HubFarLeft-NeutralZone 2-2"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.4908076923076936,
|
||||
"y": 6.633345195729537
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.3977564102564113,
|
||||
"y": 6.0868195547038955
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.4908076923076936,
|
||||
"y": 5.6745769230769225
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.6330609248104158,
|
||||
"y": 5.743196664476699
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 3.7815128205128214,
|
||||
"y": 5.651320512820514
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.967615384615385,
|
||||
"y": 5.709461538461539
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.746653846153847,
|
||||
"y": 5.430384615384616
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 1.5211640211640187,
|
||||
"rotationDegrees": -112.5
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -91.24536426676825
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 179.59775645089275
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.935255041518388,
|
||||
"y": 5.6650177935943065
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.073930090574693,
|
||||
"y": 5.873030367178768
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.559893238434164,
|
||||
"y": 7.547876631079477
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.1940806642941855,
|
||||
"y": 7.182064056939502
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.98776039963968
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -91.19348942398209
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -58,7 +58,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -104,7 +104,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -48,7 +48,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -88,7 +88,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -88,7 +88,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -64,7 +64,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -58,7 +58,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -48,7 +48,7 @@
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"nominalVoltage": 10.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
|
||||
@@ -0,0 +1,102 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.9326282051282067,
|
||||
"y": 6.5815769230769225
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.9932621113872893,
|
||||
"y": 6.3390412980405895
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.9326282051282067,
|
||||
"y": 1.6628461538461545
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.936134248900721,
|
||||
"y": 1.9128215679513527
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 1.9291221613556924,
|
||||
"y": 1.4128707397409563
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.9326282051282067,
|
||||
"y": 6.5815769230769225
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.9198835421653975,
|
||||
"y": 6.331901987278464
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 1.9453728680910158,
|
||||
"y": 6.8312518588753814
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.9326282051282067,
|
||||
"y": 1.6628461538461545
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.9337461053582308,
|
||||
"y": 1.912843654431812
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 1.9315103048981825,
|
||||
"y": 1.4128486532604971
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.9326282051282067,
|
||||
"y": 6.5815769230769225
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.8878500514131944,
|
||||
"y": 6.335619778537727
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.8,
|
||||
"maxAcceleration": 10.0,
|
||||
"maxAngularVelocity": 600.0,
|
||||
"maxAngularAcceleration": 750.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0.0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -15,11 +15,14 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.robot.constants.BuildConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.compute.HubShiftTimer;
|
||||
import frc4388.utility.compute.HubShiftTimer.ShiftInfo;
|
||||
import frc4388.utility.compute.RobotTime;
|
||||
import frc4388.utility.compute.Trim;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
@@ -113,6 +116,7 @@ public class Robot extends LoggedRobot {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
HubShiftTimer.initializeAuto();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -138,6 +142,7 @@ public class Robot extends LoggedRobot {
|
||||
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
HubShiftTimer.initializeTeleop();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -145,7 +150,12 @@ public class Robot extends LoggedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
|
||||
var info = HubShiftTimer.getShiftInfo();
|
||||
|
||||
double rumble = (info.remainingInShift() < 5. && info.remainingInShift() > 0.1) ? 1 : 0;
|
||||
|
||||
// m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble);
|
||||
// m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -5,9 +5,14 @@
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
// 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;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.Optional;
|
||||
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
@@ -24,24 +29,27 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
// Commands
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
import frc4388.robot.commands.Swerve.StayInPosition;
|
||||
import frc4388.robot.commands.alignment.AutoAlign;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.constants.FieldConstants;
|
||||
import frc4388.robot.constants.Constants.OIConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
||||
// Subsystems
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
|
||||
import frc4388.robot.subsystems.led.LED;
|
||||
import frc4388.robot.subsystems.shooter.Shooter;
|
||||
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
||||
import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Lidar;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.compute.FieldPositions;
|
||||
@@ -61,7 +69,7 @@ import frc4388.utility.controller.XboxController;
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
|
||||
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
|
||||
public final RobotMap m_robotMap = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM);
|
||||
|
||||
/*Limit Switch */
|
||||
// public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
|
||||
@@ -69,6 +77,7 @@ public class RobotContainer {
|
||||
/* Subsystems */
|
||||
// public final Lidar m_lidar = new Lidar();
|
||||
public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
|
||||
public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim();
|
||||
//Testing of Colors
|
||||
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
||||
@@ -84,7 +93,9 @@ public class RobotContainer {
|
||||
// private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
||||
|
||||
// public List<Subsystem> subsystems = new ArrayList<>();
|
||||
private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive);
|
||||
|
||||
private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d());
|
||||
// ! Teleop Commands
|
||||
public void stop() {
|
||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
||||
@@ -126,6 +137,14 @@ public class RobotContainer {
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
|
||||
);
|
||||
|
||||
private Command RobotShootDriving = new SequentialCommandGroup(
|
||||
new RunCommand(() ->
|
||||
m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION)
|
||||
).withTimeout(20)
|
||||
).finallyDo((interrupted) ->
|
||||
m_robotSwerveDrive.disableRotationOverride()
|
||||
);
|
||||
|
||||
private Command IntakeRetracted = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
|
||||
);
|
||||
@@ -135,9 +154,9 @@ public class RobotContainer {
|
||||
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
|
||||
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
|
||||
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
|
||||
new WaitCommand(2),
|
||||
IntakeRetracted,
|
||||
new WaitCommand(5),
|
||||
IntakeRetracted,
|
||||
new WaitCommand(10),
|
||||
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
|
||||
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
|
||||
);
|
||||
@@ -145,7 +164,7 @@ public class RobotContainer {
|
||||
|
||||
public RobotContainer() {
|
||||
|
||||
configureButtonBindings();
|
||||
configureSINGLEBindings();
|
||||
|
||||
// Called on first robot enable
|
||||
DeferredBlock.addBlock(() -> {
|
||||
@@ -166,6 +185,29 @@ public class RobotContainer {
|
||||
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
|
||||
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
|
||||
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
|
||||
NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
|
||||
|
||||
NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed));
|
||||
NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter));
|
||||
NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter));
|
||||
NamedCommands.registerCommand("SpinUpShooting", new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter));
|
||||
NamedCommands.registerCommand("SpinUpIdle", new InstantCommand(() -> m_robotShooter.spinUpIdle(), m_robotShooter));
|
||||
|
||||
NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> {
|
||||
if (TimesNegativeOne.isRed) {
|
||||
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
|
||||
} else {
|
||||
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
|
||||
}
|
||||
}));
|
||||
|
||||
NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> {
|
||||
if (!TimesNegativeOne.isRed) {
|
||||
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
|
||||
} else {
|
||||
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
|
||||
}
|
||||
}));
|
||||
|
||||
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
@@ -216,24 +258,47 @@ public class RobotContainer {
|
||||
}));
|
||||
|
||||
|
||||
// IF the driver is holding the left trigger, intake driving
|
||||
// TEST-> the driver is holding the left trigger, drive slow and rotation up
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||
.whileTrue(new RunCommand(
|
||||
() -> {
|
||||
m_robotSwerveDrive.driveIntakeOrientation(
|
||||
getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight()
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
m_robotSwerveDrive.shiftUpRot();
|
||||
}))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.setToFast();
|
||||
m_robotSwerveDrive.shiftDownRot();
|
||||
}));
|
||||
|
||||
);
|
||||
//TEST - > X positino on wheels
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.whileTrue(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.defenseXPosition();
|
||||
}, m_robotSwerveDrive))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.stopDefenseXPosition();
|
||||
}));
|
||||
|
||||
//TEST - > PID positinon
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
currentPose = m_robotSwerveDrive.getCurrentPose();
|
||||
}))
|
||||
.whileTrue(new RunCommand(() -> {
|
||||
m_stayInPosition.goToTargetPose(currentPose);
|
||||
}, m_robotSwerveDrive))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.softStop();
|
||||
|
||||
}));
|
||||
|
||||
|
||||
|
||||
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
|
||||
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
||||
.whileTrue(new RunCommand(
|
||||
() -> {
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
}))
|
||||
.whileTrue(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveFacingPosition(
|
||||
getDeadbandedDriverController().getLeft(),
|
||||
FieldPositions.HUB_POSITION,
|
||||
@@ -326,6 +391,126 @@ public class RobotContainer {
|
||||
|
||||
}
|
||||
|
||||
|
||||
private void configureSINGLEBindings() {
|
||||
|
||||
//Driver controls
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
|
||||
|
||||
|
||||
// TEST-> the driver is holding the left trigger, drive slow and rotation up
|
||||
// new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||
// .onTrue(new InstantCommand(() -> {
|
||||
// m_robotSwerveDrive.setToSlow();
|
||||
// m_robotSwerveDrive.shiftUpRot();
|
||||
// }))
|
||||
// .onFalse(new InstantCommand(() -> {
|
||||
// m_robotSwerveDrive.setToFast();
|
||||
// m_robotSwerveDrive.shiftDownRot();
|
||||
// }));
|
||||
|
||||
//TEST - > X positino on wheels
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
.whileTrue(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.defenseXPosition();
|
||||
}, m_robotSwerveDrive))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.stopDefenseXPosition();
|
||||
}));
|
||||
|
||||
//TEST - > PID positinon
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
currentPose = m_robotSwerveDrive.getCurrentPose();
|
||||
}))
|
||||
.whileTrue(new RunCommand(() -> {
|
||||
m_stayInPosition.goToTargetPose(currentPose);
|
||||
}, m_robotSwerveDrive))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.softStop();
|
||||
|
||||
}));
|
||||
|
||||
|
||||
|
||||
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
}))
|
||||
.whileTrue(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveFacingPosition(
|
||||
getDeadbandedDriverController().getLeft(),
|
||||
FieldPositions.HUB_POSITION,
|
||||
ShooterConstants.AIM_LEAD_TIME.get()
|
||||
);
|
||||
}, m_robotSwerveDrive)
|
||||
);
|
||||
|
||||
// D-PAD fine alignment
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
|
||||
.whileTrue(new RunCommand(
|
||||
() -> m_robotSwerveDrive.driveFine(
|
||||
new Translation2d(
|
||||
1,
|
||||
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
|
||||
),
|
||||
getDeadbandedDriverController().getRight(), 0.15
|
||||
), m_robotSwerveDrive))
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
||||
|
||||
|
||||
//allow shooting with right trigger
|
||||
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotShooter.allowShooting();
|
||||
})).onFalse(new InstantCommand(() -> {
|
||||
m_robotShooter.denyShooting();
|
||||
}));
|
||||
|
||||
|
||||
|
||||
//set shooter ready (rev) with left trigger hold
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotIntake.setMode(IntakeMode.Idle);
|
||||
m_robotIntake.rollerStop();
|
||||
m_robotShooter.spinUpShooting();
|
||||
}))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotShooter.spinUpIdle();
|
||||
}));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotIntake.setMode(IntakeMode.Extending);
|
||||
}))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotIntake.setMode(IntakeMode.Idle);
|
||||
}));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotIntake.setMode(IntakeMode.Retracting);
|
||||
}))
|
||||
.onFalse(new InstantCommand(() -> {
|
||||
m_robotIntake.setMode(IntakeMode.Idle);
|
||||
}));
|
||||
|
||||
}
|
||||
|
||||
//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
|
||||
|
||||
/**
|
||||
|
||||
@@ -25,6 +25,7 @@ import frc4388.robot.subsystems.intake.IntakeReal;
|
||||
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
||||
import frc4388.robot.subsystems.shooter.ShooterIO;
|
||||
import frc4388.robot.subsystems.shooter.ShooterReal;
|
||||
import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
|
||||
// import frc4388.robot.subsystems.elevator.ElevatorIO;
|
||||
// import frc4388.robot.subsystems.elevator.ElevatorReal;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
||||
@@ -133,8 +134,15 @@ public class RobotMap {
|
||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
|
||||
|
||||
break;
|
||||
// case SIM:
|
||||
// break;
|
||||
case SIM:
|
||||
leftCamera = new VisionIO() {};
|
||||
rightCamera = new VisionIO() {};
|
||||
|
||||
swerveDrivetrain = new SimpleSwerveSim() {};
|
||||
|
||||
shooterIO = new ShooterIO() {};
|
||||
intakeIO = new IntakeIO() {};
|
||||
break;
|
||||
default:
|
||||
leftCamera = new VisionIO() {};
|
||||
rightCamera = new VisionIO() {};
|
||||
|
||||
@@ -1,49 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.time.Instant;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveForTimeCommand extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final long duration;
|
||||
private final boolean robotRelative;
|
||||
|
||||
private Instant startTime;
|
||||
|
||||
public MoveForTimeCommand(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
long millis,
|
||||
boolean robotRelative) {
|
||||
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.duration = millis;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = Instant.now();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
|
||||
}
|
||||
}
|
||||
@@ -1,45 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveUntilSuply extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final Supplier<Boolean> truth;
|
||||
private final boolean robotRelative;
|
||||
|
||||
public MoveUntilSuply(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
Supplier<Boolean> truth,
|
||||
boolean robotRelative) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.truth = truth;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return truth.get();
|
||||
}
|
||||
}
|
||||
@@ -1,35 +0,0 @@
|
||||
// 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.Swerve;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PID;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
public class RotateToAngle extends PID {
|
||||
|
||||
SwerveDrive drive;
|
||||
double targetAngle;
|
||||
|
||||
/** Creates a new RotateToAngle. */
|
||||
public RotateToAngle(SwerveDrive drive, double targetAngle) {
|
||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
this.targetAngle = targetAngle;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return targetAngle - drive.getGyroAngle();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
// 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.Swerve;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PID;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
public class StayInPosition extends PID {
|
||||
SwerveDrive drive;
|
||||
Translation2d driveTranslation = new Translation2d();
|
||||
|
||||
public StayInPosition(SwerveDrive drive) {
|
||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||
this.drive = drive;
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
public void goToTargetPose(Pose2d targetPose) {
|
||||
Pose2d currentPose = drive.getCurrentPose();
|
||||
double translationX = targetPose.getX() - currentPose.getX();
|
||||
double translationY = targetPose.getY() - currentPose.getY();
|
||||
if (translationX > 0.2){
|
||||
//If below is changed make this change too so it never goes over 1
|
||||
translationX = 0.2;
|
||||
}
|
||||
if (translationY > 0.2){
|
||||
//Same here
|
||||
translationY = 0.2;
|
||||
}
|
||||
if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) {
|
||||
driveTranslation = new Translation2d();
|
||||
} else {
|
||||
//TODO: Tweak for best corrector against another bot
|
||||
driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5);
|
||||
}
|
||||
|
||||
drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {}
|
||||
}
|
||||
@@ -1,197 +0,0 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileInputStream;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.DataUtils;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
||||
|
||||
|
||||
/**
|
||||
* The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class neoJoystickPlayback extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final VirtualController[] controllers;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
private final Supplier<String> filenameGetter;
|
||||
private String filename;
|
||||
private int frame_index = 0;
|
||||
// private long startTime = 0;
|
||||
// private long playbackTime = 0;
|
||||
private boolean m_finished = false; // ! There is no better way.
|
||||
private boolean m_shouldfree = false; // should free memory on ending
|
||||
|
||||
private byte m_numAxes = 0;
|
||||
private byte m_numPOVs = 0;
|
||||
private byte m_numControllers = 0;
|
||||
private short m_numFrames = -1;
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param shouldfree Unloads the auto on compleation or intruption.
|
||||
* @param instantload Load the auto on object instantiation
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
||||
this.swerve = swerve;
|
||||
this.filenameGetter = filenameGetter;
|
||||
this.controllers = controllers;
|
||||
this.m_shouldfree = shouldfree;
|
||||
|
||||
if (instantload) loadAuto();
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param shouldfree unloads the auto on compleation or intruption.
|
||||
* @param instantload load the auto on object instantiation
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
||||
this(swerve, () -> filename, controllers, shouldfree, instantload);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
|
||||
this(swerve, filenameGetter, controllers, true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
|
||||
this(swerve, () -> filename, controllers, true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Load the auto file from disk into memory
|
||||
* @return Returns true if loading was successful, else wise; return false
|
||||
* @implNote if the auto is already loaded, it will return true.
|
||||
*/
|
||||
public boolean loadAuto() {
|
||||
filename = filenameGetter.get();
|
||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
|
||||
if (m_numFrames != -1 && m_numFrames == frames.size()) {
|
||||
System.out.println("AUTOPLAYBACK: Auto Already loaded.");
|
||||
return true;
|
||||
}
|
||||
|
||||
m_numAxes = stream.readNBytes(1)[0];
|
||||
m_numPOVs = stream.readNBytes(1)[0];
|
||||
m_numControllers = stream.readNBytes(1)[0];
|
||||
m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
|
||||
if (m_numControllers > controllers.length) {
|
||||
System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers
|
||||
+ " virtual controllers but only " + controllers.length + " were given");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_numFrames; i++) {
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
for (int j = 0; j < m_numControllers; j++) {
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = new double[m_numAxes];
|
||||
for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
|
||||
axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
||||
}
|
||||
short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
short[] POV = new short[m_numPOVs];
|
||||
for (int k = 0; k < m_numPOVs; k++) {
|
||||
POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
}
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[j] = controllerFrame;
|
||||
}
|
||||
frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
|
||||
frames.add(frame);
|
||||
}
|
||||
|
||||
System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
|
||||
return true;
|
||||
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Unloads the auto.
|
||||
*/
|
||||
public void unloadAuto() {
|
||||
System.out.println("AUTOPLAYBACK: Auto unloaded");
|
||||
frames.clear();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
// startTime = System.currentTimeMillis();
|
||||
// playbackTime = 0;
|
||||
frame_index = 0;
|
||||
|
||||
m_finished = !loadAuto();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
if (frame_index >= m_numFrames) m_finished = true;
|
||||
if (m_finished) return;
|
||||
|
||||
// if (frame_index == 0) {
|
||||
// startTime = System.currentTimeMillis();
|
||||
// playbackTime = 0;
|
||||
// } else {
|
||||
// playbackTime = System.currentTimeMillis() - startTime;
|
||||
// }
|
||||
|
||||
AutoRecordingFrame frame = frames.get(frame_index);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
|
||||
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
|
||||
if (i == 0) {
|
||||
this.swerve.driveWithInput(
|
||||
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
|
||||
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
|
||||
true);
|
||||
}
|
||||
}
|
||||
frame_index++;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
for (VirtualController controller : controllers) controller.zeroControls();
|
||||
swerve.stopModules();
|
||||
if (m_shouldfree) unloadAuto();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
}
|
||||
@@ -1,129 +0,0 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileOutputStream;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.DataUtils;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
||||
|
||||
/**
|
||||
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class neoJoystickRecorder extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final XboxController[] controllers;
|
||||
private String filename;
|
||||
private final Supplier<String> filenameGetter;
|
||||
private long startTime = -1;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
*/
|
||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
|
||||
this.swerve = swerve;
|
||||
this.controllers = controllers;
|
||||
this.filenameGetter = filenameGetter;
|
||||
this.filename = "";
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
*/
|
||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
|
||||
this(swerve, controllers, () -> filename);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
frames.clear();
|
||||
|
||||
this.startTime = System.currentTimeMillis();
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
|
||||
frames.add(frame);
|
||||
this.filename = this.filenameGetter.get();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
System.out.println("AUTORECORD: RECORDING");
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
XboxController controller = controllers[i];
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = {controller.getLeftX(), controller.getLeftY(),
|
||||
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
|
||||
controller.getRightX(), controller.getRightY()};
|
||||
short button = 0;
|
||||
for (int j = 0; j < 10; j++)
|
||||
if (controller.getRawButton(j+1))
|
||||
button |= 1 << j;
|
||||
short[] POV = {(short)(controller.getPOV())};
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[i] = controllerFrame;
|
||||
}
|
||||
|
||||
frames.add(frame);
|
||||
|
||||
swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
|
||||
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
|
||||
true); // Really jank way of doing this.
|
||||
|
||||
}
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
|
||||
// header: size of 0x5
|
||||
// byte Number of axes per controller
|
||||
// byte Number of POVs per controller
|
||||
// byte Number of controllers
|
||||
// short Number of frames
|
||||
stream.write(new byte[]{6, 1, (byte) controllers.length});
|
||||
stream.write(DataUtils.shortToByteArray((short) frames.size()));
|
||||
|
||||
// frame
|
||||
// controller frame * number of controllers
|
||||
// int unix time stamp.
|
||||
for (AutoRecordingFrame frame : frames) {
|
||||
// controller frame
|
||||
// double axis * Number of axes per controller
|
||||
// short button states
|
||||
// short POV * Number of POVs per controller
|
||||
for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
|
||||
for (double axis: controllerFrame.axes) {
|
||||
stream.write(DataUtils.doubleToByteArray(axis));
|
||||
}
|
||||
stream.write(DataUtils.shortToByteArray(controllerFrame.button));
|
||||
for (short POV: controllerFrame.POV) {
|
||||
stream.write(DataUtils.shortToByteArray(POV));
|
||||
}
|
||||
}
|
||||
stream.write(DataUtils.intToByteArray(frame.timeStamp));
|
||||
}
|
||||
System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,104 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
// 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.
|
||||
|
||||
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
/**
|
||||
* A command composition that runs one of two commands, depending on the value of the given
|
||||
* condition when this command is initialized.
|
||||
*
|
||||
* <p>The rules for command compositions apply: command instances that are passed to it cannot be
|
||||
* added to any other composition or scheduled individually, and the composition requires all
|
||||
* subsystems its components require.
|
||||
*
|
||||
* <p>This class is provided by the NewCommands VendorDep
|
||||
*/
|
||||
public class WhileTrueCommand extends Command {
|
||||
private final Command m_whileTrue;
|
||||
private final BooleanSupplier m_condition;
|
||||
|
||||
/**
|
||||
* Creates a new WhileTrueCommand.
|
||||
*
|
||||
* @param whileTrue the command to run while the condition is true
|
||||
* @param condition the condition to determine which command to run
|
||||
*/
|
||||
@SuppressWarnings("this-escape")
|
||||
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
|
||||
m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
|
||||
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
|
||||
|
||||
//CommandScheduler.getInstance().registerComposedCommands(whileTrue);
|
||||
|
||||
// addRequirements(whileTrue.getRequirements());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
if(m_condition.getAsBoolean())
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_whileTrue.execute();
|
||||
|
||||
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
|
||||
|
||||
if(!m_whileTrue.isFinished())
|
||||
return;
|
||||
|
||||
if(m_condition.getAsBoolean()){
|
||||
m_whileTrue.end(false);
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_whileTrue.end(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_whileTrue.runsWhenDisabled();
|
||||
}
|
||||
|
||||
@Override
|
||||
public InterruptionBehavior getInterruptionBehavior() {
|
||||
if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
|
||||
return InterruptionBehavior.kCancelSelf;
|
||||
} else {
|
||||
return InterruptionBehavior.kCancelIncoming;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
super.initSendable(builder);
|
||||
builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
|
||||
builder.addStringProperty(
|
||||
"selected",
|
||||
() -> {
|
||||
if (m_whileTrue == null) {
|
||||
return "null";
|
||||
} else {
|
||||
return m_whileTrue.getName();
|
||||
}
|
||||
},
|
||||
null);
|
||||
}
|
||||
}
|
||||
@@ -6,8 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Lidar;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.FieldPositions;
|
||||
import frc4388.utility.structs.Gains;
|
||||
|
||||
@@ -1,37 +0,0 @@
|
||||
package frc4388.robot.commands.alignment;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
|
||||
public class RotTo45 extends Command {
|
||||
|
||||
SwerveDrive m_SwerveDrive;
|
||||
Rotation2d targetAngle;
|
||||
|
||||
|
||||
public RotTo45(SwerveDrive swerveDrive) {
|
||||
m_SwerveDrive = swerveDrive;
|
||||
|
||||
addRequirements(swerveDrive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
targetAngle = new Rotation2d();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
|
||||
// TODO: Tune
|
||||
return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,36 +0,0 @@
|
||||
// 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.wait;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
/* 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 waitSupplier extends Command {
|
||||
/** Creates a new waitSupplier. */
|
||||
private final Supplier<Boolean> truth;
|
||||
public waitSupplier(Supplier<Boolean> truth) {
|
||||
this.truth = truth;
|
||||
}
|
||||
|
||||
// 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 truth.get();
|
||||
}
|
||||
}
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2026KPopRobotHunters";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 147;
|
||||
public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6";
|
||||
public static final String GIT_DATE = "2026-03-07 20:08:15 MST";
|
||||
public static final String GIT_BRANCH = "PikesPeak";
|
||||
public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1773001639109L;
|
||||
public static final int GIT_REVISION = 182;
|
||||
public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5";
|
||||
public static final String GIT_DATE = "2026-03-21 13:38:34 MDT";
|
||||
public static final String GIT_BRANCH = "MiraOrg";
|
||||
public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1774129559544L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -117,6 +117,11 @@ public final class Constants {
|
||||
// Operator ready to shoot, but the driver conditions are bad
|
||||
public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
|
||||
|
||||
// LED color for when the indexer was stuck on ball and going in reverse
|
||||
public static final LEDPatterns INDEXER_REVERSE = LEDPatterns.SOLID_VIOLET;
|
||||
|
||||
// LED color for when the indexer, intake roller, or shooter is not going at right speed for more than 5 seconds and is likely stalled
|
||||
public static final LEDPatterns MOTOR_STALLED = LEDPatterns.SOLID_RED;
|
||||
|
||||
public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE;
|
||||
// public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;
|
||||
|
||||
@@ -1,18 +1,27 @@
|
||||
package frc4388.robot.constants;
|
||||
|
||||
import java.util.Arrays;
|
||||
import static edu.wpi.first.units.Units.Meters;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTag;
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
|
||||
public final class FieldConstants {
|
||||
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
|
||||
|
||||
public static final double BUMP_OFFSET = 0.4;
|
||||
public static final Transform2d BUMP_OFFSET_RED = new Transform2d(
|
||||
Meters.of(BUMP_OFFSET),
|
||||
Meters.of(0),
|
||||
new Rotation2d()
|
||||
);
|
||||
public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d(
|
||||
Meters.of(-BUMP_OFFSET),
|
||||
Meters.of(0),
|
||||
new Rotation2d()
|
||||
);
|
||||
|
||||
// Test april tag field layout
|
||||
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
|
||||
// Arrays.asList(new AprilTag[] {
|
||||
|
||||
@@ -45,6 +45,13 @@ public class Intake extends SubsystemBase {
|
||||
io.setRollerOutput(state, 0);
|
||||
}
|
||||
|
||||
public double getRollerTarget() {
|
||||
return state.rollerTargetOutput;
|
||||
}
|
||||
|
||||
public double getRollerSpeed() {
|
||||
return state.rollerOutput;
|
||||
}
|
||||
|
||||
// public enum FieldZone {
|
||||
// // The robot should aim at the hub
|
||||
|
||||
+1
-1
@@ -5,7 +5,7 @@
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
package frc4388.robot.subsystems.led;
|
||||
|
||||
import java.util.concurrent.TimeUnit;
|
||||
|
||||
@@ -9,16 +9,21 @@ import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.led.LED;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.FieldPositions;
|
||||
import frc4388.utility.compute.HubShiftTimer;
|
||||
import frc4388.utility.compute.HubShiftTimer.ShiftInfo;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
public ShooterIO io;
|
||||
@@ -31,6 +36,9 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
// Supplier<Pose2d> m_swervePoseSupplier;
|
||||
public boolean badShooterVelocity;
|
||||
public double distanceToHub = 5;
|
||||
public double chassisXSpeed = 0;
|
||||
|
||||
|
||||
|
||||
public Shooter(
|
||||
@@ -76,7 +84,9 @@ public class Shooter extends SubsystemBase {
|
||||
this.mode = ShooterMode.Idle;
|
||||
}
|
||||
|
||||
|
||||
public double getDistanceToHub(){
|
||||
return distanceToHub;
|
||||
}
|
||||
|
||||
public void allowShooting() {
|
||||
shooterButtonReady = true;
|
||||
@@ -86,6 +96,10 @@ public class Shooter extends SubsystemBase {
|
||||
shooterButtonReady = false;
|
||||
}
|
||||
|
||||
public double getBallVelocity() {
|
||||
return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI;
|
||||
//Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS)
|
||||
}
|
||||
|
||||
@AutoLogOutput
|
||||
public ShooterMode getMode() {
|
||||
@@ -102,40 +116,45 @@ public class Shooter extends SubsystemBase {
|
||||
@Override
|
||||
public void periodic() {
|
||||
// FaultReporter.register(this); // TODO Implement fault reporter
|
||||
|
||||
//Hub Shift logs
|
||||
ShiftInfo info = HubShiftTimer.getShiftInfo();
|
||||
Logger.recordOutput("HubShift/IsActive", info.isActive());
|
||||
Logger.recordOutput("HubShift/RemainingInShift", info.remainingInShift());
|
||||
Logger.recordOutput("HubShift/Phase", info.phase().name());
|
||||
Logger.processInputs("Shooter", state);
|
||||
|
||||
io.updateInputs(state);
|
||||
|
||||
|
||||
// Get robot positon and speeds
|
||||
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
|
||||
double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2));
|
||||
double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI));
|
||||
|
||||
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
|
||||
|
||||
|
||||
// Calculate aim lead
|
||||
// Get the current speed of the robot
|
||||
Translation2d robotSpeed = new Translation2d(
|
||||
chassisSpeeds.vxMetersPerSecond,
|
||||
chassisSpeeds.vyMetersPerSecond
|
||||
);
|
||||
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION);
|
||||
Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle().minus(m_SwerveDrive.getPose2d().getRotation());
|
||||
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
|
||||
|
||||
// Calculate a point to aim ahead of the actual position.
|
||||
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation());
|
||||
if (TimesNegativeOne.isRed) {
|
||||
chassisXSpeed = chassisSpeeds.vxMetersPerSecond;
|
||||
} else {
|
||||
chassisXSpeed = -chassisSpeeds.vxMetersPerSecond;
|
||||
}
|
||||
|
||||
Translation2d fieldPos = robotPose2d.getTranslation();
|
||||
// Get the robot's aim distance to hub
|
||||
double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
|
||||
distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm());
|
||||
|
||||
//Center of hub to cameras in inches
|
||||
//Center of hub to cameras in meters
|
||||
Logger.recordOutput("Hub Dist", distanceToHub);
|
||||
|
||||
boolean driverError =
|
||||
// XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
|
||||
// AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
|
||||
distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
|
||||
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
|
||||
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get() |
|
||||
Math.abs(ang.getDegrees()) > ShooterConstants.AIM_ANGLE.get();
|
||||
|
||||
|
||||
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
|
||||
@@ -145,14 +164,14 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
//revtime calculations
|
||||
// double shooterAcceleration =
|
||||
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
|
||||
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond);
|
||||
double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
|
||||
// double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
|
||||
Logger.recordOutput("Time to rev", revTime);
|
||||
|
||||
switch (mode) {
|
||||
case Shooting:
|
||||
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
|
||||
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
|
||||
|
||||
int bitmask = (
|
||||
(shooterButtonReady ? 1 : 0) +
|
||||
@@ -179,13 +198,13 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
case 0b100: // Driver error, button is not pressed
|
||||
case 0b101: // Driver error, button is pressed
|
||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
||||
io.setIndexerOutput(state, 0);
|
||||
m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
|
||||
break;
|
||||
|
||||
case 0b110: // Driver error, bad shooter vel, button is not pressed
|
||||
case 0b111: // Driver error, bad shooter vel, button is pressed
|
||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
||||
io.setIndexerOutput(state, 0);
|
||||
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
|
||||
break;
|
||||
}
|
||||
@@ -200,7 +219,7 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
switch (bitmask2) {
|
||||
case 0b000: // No errors but button is not pressed
|
||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
||||
io.setIndexerOutput(state, 0);
|
||||
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
|
||||
break;
|
||||
|
||||
@@ -211,7 +230,7 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
case 0b010: // Bad shooter velocity, button is not pressed
|
||||
case 0b011: // Bad shooter velocty, button is pressed
|
||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
||||
io.setIndexerOutput(state, 0);
|
||||
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
|
||||
break;
|
||||
|
||||
@@ -237,12 +256,11 @@ public class Shooter extends SubsystemBase {
|
||||
// Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()),
|
||||
// RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
|
||||
);
|
||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
||||
io.setIndexerOutput(state, 0);
|
||||
m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
|
||||
break;
|
||||
|
||||
|
||||
}
|
||||
|
||||
io.motorStalled(state, m_Intake, m_robotLED);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -15,30 +15,37 @@ import frc4388.utility.status.CanDevice;
|
||||
public class ShooterConstants {
|
||||
// Motor conversions
|
||||
|
||||
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
|
||||
public static final double INDEXER_GEAR_RATIO = 1.;
|
||||
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.286; // TODO: supposed to be 9 rotations in to 7 out -- 0.77 or 1.29
|
||||
public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31
|
||||
public static final double T_CONSTANT = 2;
|
||||
|
||||
public static final double SHOOTER_RADIUS = 2/39.37;
|
||||
public static final double INDEXER_RADIUS = 0.625/39.37;
|
||||
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
|
||||
public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42);
|
||||
// public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
|
||||
// public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
|
||||
|
||||
|
||||
public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15);
|
||||
// public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
|
||||
// public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
|
||||
|
||||
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
|
||||
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);
|
||||
public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", -1.5);
|
||||
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2);
|
||||
public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0);
|
||||
|
||||
|
||||
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
|
||||
public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.);
|
||||
public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.);
|
||||
|
||||
|
||||
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1);
|
||||
|
||||
// Shoot mode tolerances
|
||||
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
|
||||
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
|
||||
|
||||
public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 15);
|
||||
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
|
||||
|
||||
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
|
||||
@@ -47,20 +54,23 @@ public class ShooterConstants {
|
||||
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3);
|
||||
|
||||
//
|
||||
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
|
||||
// Model derived from points
|
||||
// double speed =
|
||||
// 1.11576*hubDistMeters*hubDistMeters +
|
||||
// 0.318464*hubDistMeters +
|
||||
// 30.6293;
|
||||
double speed =
|
||||
5.6939*hubDistMeters +
|
||||
22.76545 + MODEL_TRIM.get();
|
||||
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
|
||||
double speed = 0;
|
||||
|
||||
// double speed =
|
||||
// 0.00610938*hubDistMeters*hubDistMeters
|
||||
// 5.65235*hubDistMeters +
|
||||
// 22.82825;
|
||||
if (Math.abs(chassisXSpeed) < 0.1){
|
||||
speed = 0.0593402*hubDistMeters*hubDistMeters +
|
||||
4.90561*hubDistMeters +
|
||||
30.35696 + MODEL_TRIM.get();
|
||||
} else if (chassisXSpeed > 0){
|
||||
speed = 0.0593402*hubDistMeters*hubDistMeters +
|
||||
4.90561*hubDistMeters +
|
||||
30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
|
||||
|
||||
} else { // Negative is closer to hub
|
||||
speed = 0.0593402*hubDistMeters*hubDistMeters +
|
||||
4.90561*hubDistMeters +
|
||||
30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get();
|
||||
}
|
||||
|
||||
double max = SHOOTER_MAX_VELOCITY.get();
|
||||
|
||||
@@ -79,7 +89,7 @@ public class ShooterConstants {
|
||||
// Motor Configuration
|
||||
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
|
||||
.withKV(0.0)
|
||||
.withKP(0.08)
|
||||
.withKP(0.02)
|
||||
.withKI(0.15)
|
||||
.withKD(0.0);
|
||||
|
||||
@@ -88,7 +98,7 @@ public class ShooterConstants {
|
||||
|
||||
|
||||
|
||||
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
|
||||
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02);
|
||||
public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
|
||||
public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
|
||||
|
||||
|
||||
@@ -7,6 +7,8 @@ import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.led.LED;
|
||||
|
||||
public interface ShooterIO {
|
||||
@AutoLog
|
||||
@@ -49,6 +51,7 @@ public interface ShooterIO {
|
||||
|
||||
public default void updateInputs(ShooterState state) {}
|
||||
|
||||
public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {}
|
||||
|
||||
public default void updateGains() {}
|
||||
}
|
||||
@@ -3,11 +3,16 @@ package frc4388.robot.subsystems.shooter;
|
||||
import static edu.wpi.first.units.Units.Amps;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.led.LED;
|
||||
|
||||
public class ShooterReal implements ShooterIO {
|
||||
|
||||
@@ -19,6 +24,14 @@ public class ShooterReal implements ShooterIO {
|
||||
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
|
||||
// VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
|
||||
|
||||
private final Timer m_stallTimerShooter = new Timer();
|
||||
private final Timer m_stallTimerIndexer = new Timer();
|
||||
private final Timer m_stallTimerRoller = new Timer();
|
||||
private boolean m_shooterStalling = false;
|
||||
private boolean m_indexerStalling = false;
|
||||
private boolean m_rollerStalling = false;
|
||||
public String motorStall = "";
|
||||
|
||||
|
||||
public ShooterReal(
|
||||
TalonFX shooter1Motor,
|
||||
@@ -41,6 +54,59 @@ public class ShooterReal implements ShooterIO {
|
||||
// m_indexerVelocity.Slot = 0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {
|
||||
motorStall = "";
|
||||
if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) {
|
||||
if (!m_shooterStalling) {
|
||||
m_shooterStalling = true;
|
||||
m_stallTimerShooter.restart();
|
||||
}
|
||||
if (m_stallTimerShooter.hasElapsed(5.0)) {
|
||||
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
|
||||
motorStall = "Shooter Stalled";
|
||||
System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))));
|
||||
System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)));
|
||||
System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond)));
|
||||
}
|
||||
} else {
|
||||
m_shooterStalling = false;
|
||||
m_stallTimerShooter.reset();
|
||||
}
|
||||
|
||||
if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) {
|
||||
if (!m_indexerStalling) {
|
||||
m_indexerStalling = true;
|
||||
m_stallTimerIndexer.restart();
|
||||
}
|
||||
if (m_stallTimerIndexer.hasElapsed(5.0)) {
|
||||
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
|
||||
motorStall = "Indexer Stalled";
|
||||
System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)));
|
||||
}
|
||||
} else {
|
||||
m_indexerStalling = false;
|
||||
m_stallTimerIndexer.reset();
|
||||
}
|
||||
|
||||
if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.4) {
|
||||
if (!m_rollerStalling) {
|
||||
m_rollerStalling = true;
|
||||
m_stallTimerRoller.restart();
|
||||
}
|
||||
if (m_stallTimerRoller.hasElapsed(5.0)) {
|
||||
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
|
||||
motorStall = "Roller Stalled";
|
||||
System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())));
|
||||
}
|
||||
} else {
|
||||
m_rollerStalling = false;
|
||||
m_stallTimerRoller.reset();
|
||||
}
|
||||
Logger.recordOutput("Stalled Motor: ", motorStall);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
|
||||
state.motor1TargetVelocity = target;
|
||||
@@ -75,6 +141,8 @@ public class ShooterReal implements ShooterIO {
|
||||
// Math.abs(currentLimit.in(Amps)) > current &&
|
||||
// Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
|
||||
// ) {
|
||||
state.motor1TargetVelocity = RotationsPerSecond.of(percentOutput);
|
||||
state.motor2TargetVelocity = RotationsPerSecond.of(percentOutput);
|
||||
m_shooter1Motor.set(percentOutput);
|
||||
m_shooter2Motor.set(percentOutput);
|
||||
// } else {
|
||||
|
||||
@@ -0,0 +1,204 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
|
||||
public class SimpleSwerveSim implements SwerveIO {
|
||||
private Pose2d pose = new Pose2d();
|
||||
private Pose2d lastPose = pose;
|
||||
private double vx = 0.0;
|
||||
private double vy = 0.0;
|
||||
private double omega = 0.0;
|
||||
|
||||
private long lastTimeNs = System.nanoTime();
|
||||
|
||||
public SimpleSwerveSim() {
|
||||
}
|
||||
|
||||
public synchronized void setControl(SwerveRequest ctrl) {
|
||||
if (ctrl == null) return;
|
||||
|
||||
if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) {
|
||||
vx = facingAngle.VelocityX;
|
||||
vy = facingAngle.VelocityY;
|
||||
|
||||
double currentAngle = pose.getRotation().getRadians();
|
||||
double targetAngle = facingAngle.TargetDirection.getRadians();
|
||||
double error = targetAngle - currentAngle;
|
||||
|
||||
error = Math.atan2(Math.sin(error), Math.cos(error));
|
||||
|
||||
double kP = 5.0;
|
||||
omega = error * kP;
|
||||
return;
|
||||
}
|
||||
|
||||
if (ctrl instanceof SwerveRequest.FieldCentric fc) {
|
||||
vx = fc.VelocityX;
|
||||
vy = fc.VelocityY;
|
||||
omega = fc.RotationalRate;
|
||||
return;
|
||||
}
|
||||
|
||||
if (ctrl instanceof SwerveRequest.RobotCentric rc) {
|
||||
double cos = pose.getRotation().getCos();
|
||||
double sin = pose.getRotation().getSin();
|
||||
double vxRobot = rc.VelocityX;
|
||||
double vyRobot = rc.VelocityY;
|
||||
vx = vxRobot * cos - vyRobot * sin;
|
||||
vy = vxRobot * sin + vyRobot * cos;
|
||||
omega = rc.RotationalRate;
|
||||
return;
|
||||
}
|
||||
|
||||
if (ctrl instanceof SwerveRequest.SwerveDriveBrake) {
|
||||
vx = 0; vy = 0; omega = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
ChassisSpeeds cs = tryGetSpeedsField(ctrl);
|
||||
if (cs != null) {
|
||||
vx = cs.vxMetersPerSecond;
|
||||
vy = cs.vyMetersPerSecond;
|
||||
omega = cs.omegaRadiansPerSecond;
|
||||
}
|
||||
}
|
||||
|
||||
private ChassisSpeeds tryGetSpeedsField(SwerveRequest ctrl) {
|
||||
try {
|
||||
java.lang.reflect.Field f = ctrl.getClass().getDeclaredField("Speeds");
|
||||
f.setAccessible(true);
|
||||
Object o = f.get(ctrl);
|
||||
if (o instanceof ChassisSpeeds) {
|
||||
return (ChassisSpeeds) o;
|
||||
}
|
||||
} catch (NoSuchFieldException nsf) {
|
||||
} catch (IllegalAccessException iae) {
|
||||
} catch (SecurityException se) {
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private double tryGetDoubleField(Object obj, Class<?> cls, String... names) {
|
||||
for (String n : names) {
|
||||
try {
|
||||
java.lang.reflect.Field f = cls.getDeclaredField(n);
|
||||
f.setAccessible(true);
|
||||
Object val = f.get(obj);
|
||||
if (val instanceof Number) {
|
||||
return ((Number) val).doubleValue();
|
||||
}
|
||||
} catch (NoSuchFieldException nsf) {
|
||||
} catch (IllegalAccessException iae) {
|
||||
} catch (Throwable t) {
|
||||
}
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
|
||||
public synchronized void pointAtXY(double x, double y) {
|
||||
Translation2d target = new Translation2d(x, y);
|
||||
Translation2d toTarget = target.minus(pose.getTranslation());
|
||||
if (toTarget.getNorm() < 1e-9) return;
|
||||
Rotation2d desired = toTarget.getAngle();
|
||||
pose = new Pose2d(pose.getTranslation(), desired);
|
||||
lastPose = pose;
|
||||
vx = 0.0;
|
||||
vy = 0.0;
|
||||
omega = 0.0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public synchronized void updateInputs(SwerveState state) {
|
||||
if (state == null) return;
|
||||
|
||||
long now = System.nanoTime();
|
||||
double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9);
|
||||
lastTimeNs = now;
|
||||
|
||||
lastPose = pose;
|
||||
double dxField;
|
||||
double dyField;
|
||||
|
||||
if (DriverStation.isAutonomous()) {
|
||||
double dxRobot = vx * dt;
|
||||
double dyRobot = vy * dt;
|
||||
|
||||
Rotation2d r = pose.getRotation();
|
||||
double cos = r.getCos();
|
||||
double sin = r.getSin();
|
||||
|
||||
dxField = dxRobot * cos - dyRobot * sin;
|
||||
dyField = dxRobot * sin + dyRobot * cos;
|
||||
|
||||
} else {
|
||||
dxField = vx * dt;
|
||||
dyField = vy * dt;
|
||||
|
||||
}
|
||||
|
||||
|
||||
double dTheta = omega * dt;
|
||||
|
||||
Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField));
|
||||
Rotation2d newRot = pose.getRotation().plus(Rotation2d.fromRadians(dTheta));
|
||||
pose = new Pose2d(newTrans, newRot);
|
||||
|
||||
state.lastPose = lastPose;
|
||||
state.currentPose = pose;
|
||||
state.speeds = new ChassisSpeeds(vx, vy, omega);
|
||||
state.odometryRate = dt;
|
||||
}
|
||||
|
||||
@Override
|
||||
public synchronized void resetPose(Pose2d p) {
|
||||
if (p == null) return;
|
||||
pose = p;
|
||||
lastPose = p;
|
||||
lastTimeNs = System.nanoTime();
|
||||
}
|
||||
|
||||
@Override
|
||||
public synchronized void tareEverything() {
|
||||
pose = new Pose2d();
|
||||
lastPose = pose;
|
||||
vx = 0.0;
|
||||
vy = 0.0;
|
||||
omega = 0.0;
|
||||
lastTimeNs = System.nanoTime();
|
||||
}
|
||||
|
||||
@Override
|
||||
public synchronized void addVisionMeasurement(List<PoseObservation> poses) {
|
||||
if (poses == null || poses.isEmpty()) return;
|
||||
Pose2d visPose = poses.get(poses.size() - 1).pose();
|
||||
if (visPose != null) {
|
||||
pose = visPose;
|
||||
lastPose = visPose;
|
||||
}
|
||||
}
|
||||
|
||||
public synchronized void pointAt(Translation2d target) {
|
||||
if (target == null) return;
|
||||
Translation2d toTarget = target.minus(pose.getTranslation());
|
||||
if (toTarget.getNorm() < 1e-9) return;
|
||||
Rotation2d desired = toTarget.getAngle();
|
||||
pose = new Pose2d(pose.getTranslation(), desired);
|
||||
lastPose = pose;
|
||||
omega = 0.0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public synchronized void setLimits(double limitInAmps) {
|
||||
}
|
||||
}
|
||||
@@ -5,7 +5,9 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import java.util.Optional;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
@@ -19,10 +21,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.util.PathPlannerLogging;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
@@ -63,6 +68,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
private final PIDController m_rotationOverridePID = new PIDController(
|
||||
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
|
||||
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
|
||||
SwerveDriveConstants.PIDConstants.AIM_kD.get()
|
||||
);
|
||||
private boolean m_useRotationOverride = false;
|
||||
private Translation2d m_rotationOverrideTarget = new Translation2d();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
|
||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
|
||||
@@ -81,6 +94,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// Handle exception as needed
|
||||
config = null;
|
||||
}
|
||||
|
||||
PPHolonomicDriveController driveController = new PPHolonomicDriveController(
|
||||
new PIDConstants(5.0, 0.0, 0.0), // Translation PID
|
||||
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID (used when override is OFF)
|
||||
);
|
||||
driveController.setRotationTargetOverride(() -> {
|
||||
if (!m_useRotationOverride) return Optional.empty();
|
||||
Rotation2d targetAngle = getPose2d()
|
||||
.getTranslation()
|
||||
.minus(m_rotationOverrideTarget)
|
||||
.getAngle();
|
||||
return Optional.of(targetAngle);
|
||||
});
|
||||
|
||||
// DoubleSupplier a = () -> 1.d;
|
||||
AutoBuilder.configure(
|
||||
() -> {
|
||||
@@ -92,11 +119,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
(speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
|
||||
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
|
||||
// Also optionally outputs individual module feedforwards
|
||||
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
|
||||
// holonomic drive trains
|
||||
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
|
||||
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
|
||||
),
|
||||
driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...)
|
||||
config, // The robot configuration
|
||||
() -> {
|
||||
// Boolean supplier that controls when the path will be mirrored for the red
|
||||
@@ -148,14 +171,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
public void setInitalPose(Pose2d startingAutoPose){
|
||||
initalPose2d = startingAutoPose;
|
||||
}
|
||||
// MIRA public void setOdoPose(Pose2d pose) {
|
||||
// if (pose == null) return;
|
||||
// io.tareEverything();
|
||||
// initalPose2d = pose;
|
||||
// io.resetPose(pose);
|
||||
// robotKnowsWhereItIs = true;
|
||||
// rotTarget = pose.getRotation().getDegrees();
|
||||
// }
|
||||
|
||||
|
||||
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
|
||||
@@ -172,6 +187,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// module.setDesiredState(state);
|
||||
// }
|
||||
|
||||
public double chassisXSpeeds(){
|
||||
if (TimesNegativeOne.isRed) {
|
||||
return chassisSpeeds.vxMetersPerSecond;
|
||||
} else {
|
||||
return -chassisSpeeds.vxMetersPerSecond;
|
||||
}
|
||||
}
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
@@ -234,6 +257,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) {
|
||||
Translation2d robotSpeed = new Translation2d(
|
||||
chassisSpeeds.vxMetersPerSecond,
|
||||
chassisSpeeds.vyMetersPerSecond
|
||||
);
|
||||
Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
|
||||
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
|
||||
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(chassisSpeeds.vxMetersPerSecond)
|
||||
.withVelocityY(chassisSpeeds.vyMetersPerSecond)
|
||||
.withTargetDirection(ang);
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
|
||||
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
|
||||
SwerveDriveConstants.PIDConstants.AIM_kD.get()
|
||||
);
|
||||
io.setControl(ctrl);
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
|
||||
// reason to have a robot
|
||||
// relitive version of
|
||||
@@ -301,7 +345,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// SmartDashboard.putBoolean("drift correction", true);
|
||||
}
|
||||
|
||||
public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) {
|
||||
|
||||
rotTarget = heading.getDegrees();
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
|
||||
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
|
||||
SwerveDriveConstants.PIDConstants.AIM_kD.get()
|
||||
);
|
||||
io.setControl(ctrl);
|
||||
}
|
||||
|
||||
public void driveIntake(Translation2d leftStick, boolean invertRotation){
|
||||
// if (invert){
|
||||
@@ -360,23 +417,57 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
// Drive with the robot facing towards a specific position
|
||||
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) {
|
||||
|
||||
// Get the current speed of the robot
|
||||
Translation2d robotSpeed = new Translation2d(
|
||||
chassisSpeeds.vxMetersPerSecond,
|
||||
chassisSpeeds.vyMetersPerSecond
|
||||
);
|
||||
|
||||
// Calculate a point to aim ahead of the actual position.
|
||||
Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
|
||||
|
||||
// Calculate the angle between the current position and the lead position
|
||||
Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
|
||||
double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY());
|
||||
if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){
|
||||
if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) {
|
||||
if (TimesNegativeOne.isRed){
|
||||
robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond);
|
||||
} else {
|
||||
robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond);
|
||||
}
|
||||
} }
|
||||
Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
|
||||
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
|
||||
Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d());
|
||||
Logger.recordOutput("Lead Aim", fieldPosLeadLog);
|
||||
driveFieldAngle(leftStick, ang);
|
||||
}
|
||||
|
||||
public void offsetOdoPosition(Transform2d offset) {
|
||||
// Manually performing an addittion on the pose
|
||||
// WHY doesn't WPILIB have the ability to not transform poses
|
||||
Pose2d new_pose = new Pose2d(
|
||||
new Translation2d(
|
||||
state.currentPose.getX() + offset.getX(),
|
||||
state.currentPose.getY() + offset.getY()
|
||||
),
|
||||
state.currentPose.getRotation()
|
||||
);
|
||||
this.io.resetPose(new_pose);
|
||||
}
|
||||
|
||||
public void defenseXPosition(){
|
||||
io.setControl(new SwerveRequest.SwerveDriveBrake());
|
||||
}
|
||||
|
||||
public void stopDefenseXPosition(){
|
||||
stopModules();
|
||||
}
|
||||
|
||||
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
|
||||
// Calculate the angle between the current position and the lead position
|
||||
//Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle();
|
||||
Rotation2d ang = new Rotation2d(0,1);
|
||||
System.out.println(ang);
|
||||
|
||||
driveFieldAngle(leftStick, ang);
|
||||
}
|
||||
|
||||
public Pose2d getCurrentPose(){
|
||||
return state.currentPose;
|
||||
}
|
||||
|
||||
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(heading);
|
||||
|
||||
@@ -464,6 +555,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
softStop();
|
||||
}
|
||||
|
||||
public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) {
|
||||
Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
|
||||
|
||||
Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
|
||||
m_rotationOverrideTarget = fieldPosLead;
|
||||
m_useRotationOverride = true;
|
||||
}
|
||||
|
||||
public void disableRotationOverride() {
|
||||
m_useRotationOverride = false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
@@ -476,6 +579,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
vision.setLastOdomPose(state.currentPose);
|
||||
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
||||
|
||||
if (state.speeds != null) {
|
||||
this.chassisSpeeds = state.speeds;
|
||||
} else {
|
||||
this.chassisSpeeds = new ChassisSpeeds();
|
||||
}
|
||||
|
||||
if (vision.isTag()) {
|
||||
Pose2d pose = vision.getPose2d();
|
||||
if (!robotKnowsWhereItIs) {
|
||||
@@ -485,6 +594,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
io.addVisionMeasurement(vision.getPosesToAdd());
|
||||
io.updateInputs(state);
|
||||
Logger.processInputs("SwerveDrive", state);
|
||||
|
||||
vision.setLastOdomPose(state.currentPose);
|
||||
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
@@ -515,6 +629,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
@@ -590,4 +705,3 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -73,9 +73,10 @@ public final class SwerveDriveConstants {
|
||||
public static final boolean INVERT_Y = true;
|
||||
public static final boolean INVERT_ROTATION = false;
|
||||
|
||||
public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05);
|
||||
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
|
||||
|
||||
private static final class ModuleSpecificConstants { //2025
|
||||
public static final class ModuleSpecificConstants { //2026
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
|
||||
@@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
|
||||
public interface SwerveIO {
|
||||
@AutoLog
|
||||
|
||||
@@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
|
||||
public class SwerveReal implements SwerveIO {
|
||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
+3
-3
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.subsystems;
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.LinkedList;
|
||||
@@ -20,8 +20,8 @@ import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.subsystems.RPLidarA1.PolarPoint;
|
||||
import frc4388.robot.subsystems.RPLidarA1.ScanListener;
|
||||
import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint;
|
||||
import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
import frc4388.utility.status.FaultA1M8;
|
||||
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.subsystems;
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import com.fazecast.jSerialComm.SerialPort;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
@@ -0,0 +1,154 @@
|
||||
|
||||
package frc4388.utility.compute;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
public class HubShiftTimer {
|
||||
|
||||
public enum ShiftPhase {
|
||||
DISABLED,
|
||||
AUTO,
|
||||
TRANSITION, // 0 – 10 s
|
||||
SHIFT1, // 10 – 35 s
|
||||
SHIFT2, // 35 – 60 s
|
||||
SHIFT3, // 60 – 85 s
|
||||
SHIFT4, // 85 – 110 s
|
||||
ENDGAME // 110 – 140 s
|
||||
}
|
||||
|
||||
public record ShiftInfo(
|
||||
ShiftPhase phase,
|
||||
double elapsedInShift,
|
||||
double remainingInShift,
|
||||
boolean isActive) {}
|
||||
//total teleop time
|
||||
public static final double TELEOP_DURATION = 140.0;
|
||||
//total auto time
|
||||
public static final double AUTO_DURATION = 20.0;
|
||||
|
||||
//shift start and end times for calculations
|
||||
private static final double[] SHIFT_STARTS = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0};
|
||||
private static final double[] SHIFT_ENDS = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0};
|
||||
|
||||
//hub active schedule, true is active and false is inactive
|
||||
//starts always as active becasue transition is first and is active, but then is inactive for winner or active for loser
|
||||
private static final boolean[] WINNER_SCHEDULE = {true, false, true, false, true, true};
|
||||
private static final boolean[] LOSER_SCHEDULE = {true, true, false, true, false, true};
|
||||
|
||||
//shift phase names during teleop
|
||||
private static final ShiftPhase[] SHIFT_PHASES = {
|
||||
ShiftPhase.TRANSITION,
|
||||
ShiftPhase.SHIFT1,
|
||||
ShiftPhase.SHIFT2,
|
||||
ShiftPhase.SHIFT3,
|
||||
ShiftPhase.SHIFT4,
|
||||
ShiftPhase.ENDGAME
|
||||
};
|
||||
|
||||
//timer to track time
|
||||
private static final Timer teleopTimer = new Timer();
|
||||
private static double timerOffset = 0.0;
|
||||
|
||||
//fms syncing idk other team did it too
|
||||
private static final double RESYNC_THRESHOLD = 3.0;
|
||||
|
||||
|
||||
//call at start of auto to start timer
|
||||
public static void initializeAuto() {
|
||||
teleopTimer.restart();
|
||||
}
|
||||
|
||||
//call at start of teleop to start timer again because sometimes delay between auto and telop
|
||||
public static void initializeTeleop() {
|
||||
timerOffset = 0.0;
|
||||
teleopTimer.restart();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//returns the updated shift info based on the winner of auto
|
||||
public static ShiftInfo getShiftInfo() {
|
||||
if (!DriverStation.isEnabled()) {
|
||||
return new ShiftInfo(ShiftPhase.DISABLED, 0.0, 0.0, false);
|
||||
}
|
||||
if (DriverStation.isAutonomousEnabled()) {
|
||||
double autoElapsed = teleopTimer.get(); // timer restarts in initialize()
|
||||
return new ShiftInfo(
|
||||
ShiftPhase.AUTO,
|
||||
autoElapsed,
|
||||
Math.max(0.0, AUTO_DURATION - teleopTimer.get()),
|
||||
true);
|
||||
}
|
||||
return computeTeleopShift();
|
||||
}
|
||||
|
||||
//find auto winner, R = red wins, B = blue wins
|
||||
public static Alliance autoWinnerAlliance() {
|
||||
String msg = DriverStation.getGameSpecificMessage();
|
||||
if (msg != null && msg.length() > 0) {
|
||||
char c = msg.charAt(0);
|
||||
if (c == 'R') return Alliance.Red;
|
||||
if (c == 'B') return Alliance.Blue;
|
||||
}
|
||||
// backup if no msg, returns auto winner as opposite of our alliance. if we red -> blue wins auto
|
||||
Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue);
|
||||
return (ours == Alliance.Blue) ? Alliance.Red : Alliance.Blue;
|
||||
}
|
||||
|
||||
|
||||
//return our schedule for the shifts
|
||||
private static boolean[] getSchedule() {
|
||||
Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue);
|
||||
Alliance winner = autoWinnerAlliance();
|
||||
return (ours == winner) ? WINNER_SCHEDULE : LOSER_SCHEDULE;
|
||||
}
|
||||
|
||||
//time since start of teleop
|
||||
private static double getTeleopElapsed() {
|
||||
double localTime = teleopTimer.get() - timerOffset;
|
||||
|
||||
// Re-sync to FMS time if we drift too far (only when FMS is attached)
|
||||
if (DriverStation.isFMSAttached()) {
|
||||
double fmsElapsed = TELEOP_DURATION - DriverStation.getMatchTime();
|
||||
if (fmsElapsed <= TELEOP_DURATION - 5.0 // ignore the first few seconds of jitter
|
||||
&& Math.abs(localTime - fmsElapsed) >= RESYNC_THRESHOLD) {
|
||||
timerOffset += localTime - fmsElapsed;
|
||||
localTime = fmsElapsed;
|
||||
}
|
||||
}
|
||||
return Math.max(0.0, Math.min(TELEOP_DURATION, localTime));
|
||||
}
|
||||
|
||||
private static ShiftInfo computeTeleopShift() {
|
||||
boolean[] schedule = getSchedule();
|
||||
double elapsed = getTeleopElapsed();
|
||||
|
||||
// Find which shift we're in
|
||||
int phaseIndex = SHIFT_STARTS.length - 1; // default to last shift if past all bounds
|
||||
for (int i = 0; i < SHIFT_STARTS.length; i++) {
|
||||
if (elapsed >= SHIFT_STARTS[i] && elapsed < SHIFT_ENDS[i]) {
|
||||
phaseIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
double shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex];
|
||||
double shiftRemaining = SHIFT_ENDS[phaseIndex] - elapsed;
|
||||
|
||||
// merge time for elapsed if same active/inactive
|
||||
if (phaseIndex > 0 && schedule[phaseIndex] == schedule[phaseIndex - 1]) {
|
||||
shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex - 1];
|
||||
}
|
||||
|
||||
// merge time for remaining time if same active/inactive status
|
||||
if (phaseIndex < SHIFT_ENDS.length - 1 && schedule[phaseIndex] == schedule[phaseIndex + 1]) {
|
||||
shiftRemaining = SHIFT_ENDS[phaseIndex + 1] - elapsed;
|
||||
}
|
||||
|
||||
return new ShiftInfo(
|
||||
SHIFT_PHASES[phaseIndex],
|
||||
shiftElapsed,
|
||||
shiftRemaining,
|
||||
schedule[phaseIndex]);
|
||||
}
|
||||
}
|
||||
@@ -50,4 +50,9 @@ public class CanDevice {
|
||||
}
|
||||
|
||||
|
||||
public Object getId() {
|
||||
return id;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
package frc4388.utility.status;
|
||||
|
||||
import frc4388.robot.subsystems.RPLidarA1;
|
||||
import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus;
|
||||
import frc4388.robot.subsystems.vision.RPLidarA1;
|
||||
import frc4388.robot.subsystems.vision.RPLidarA1.ConnectionStatus;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
// Fault reporter for the RPLidar A1M8 Revolving lidar sensor
|
||||
|
||||
Reference in New Issue
Block a user