Compare commits
23 Commits
f57cf77200
...
maybe_fix_
| Author | SHA1 | Date | |
|---|---|---|---|
| 740b4a57b9 | |||
| 24d6a7a5cf | |||
| c822b2f95a | |||
| c6d1b96006 | |||
| c52a9ead0f | |||
| e0d0a121ba | |||
| 92206fa252 | |||
| d61314fc01 | |||
| 496b9c15f9 | |||
| 2c1899f3b5 | |||
| f3b17422e1 | |||
| d2076e7afb | |||
| 3cf33a049e | |||
| 52e92574c4 | |||
| 2990b917e7 | |||
| d934cdf35b | |||
| 4d260809d8 | |||
| eb00b1146e | |||
| 87e7eb4974 | |||
| 44a036f420 | |||
| a145c290fd | |||
| 3dafb3c269 | |||
| 1c64d7344b |
@@ -10,6 +10,11 @@ java {
|
||||
|
||||
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
||||
|
||||
task(replayWatch, type: JavaExec) {
|
||||
mainClass = "org.littletonrobotics.junction.ReplayWatch"
|
||||
classpath = sourceSets.main.runtimeClasspath
|
||||
}
|
||||
|
||||
// Define my targets (RoboRIO) and artifacts (deployable files)
|
||||
// This is added by GradleRIO's backing project DeployUtils.
|
||||
deploy {
|
||||
@@ -72,6 +77,9 @@ dependencies {
|
||||
|
||||
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
|
||||
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
|
||||
|
||||
def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
|
||||
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
|
||||
}
|
||||
|
||||
test {
|
||||
|
||||
BIN
src/main/deploy/Orchestra/doomE1M1.chrp
Normal file
BIN
src/main/deploy/Orchestra/doomE1M1.chrp
Normal file
Binary file not shown.
31
src/main/deploy/pathplanner/autos/1 L4 Center.auto
Normal file
31
src/main/deploy/pathplanner/autos/1 L4 Center.auto
Normal file
@@ -0,0 +1,31 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Start to H"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "H Backup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Field Oriented Test Path"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -15,6 +15,12 @@
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "J Backup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
19
src/main/deploy/pathplanner/autos/fein.auto
Normal file
19
src/main/deploy/pathplanner/autos/fein.auto
Normal file
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "fein"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -34,9 +34,9 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -41,9 +41,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -46,9 +46,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.0,
|
||||
"y": 7.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.0,
|
||||
"y": 7.0
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.495389344262295,
|
||||
"y": 6.788165983606557
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.4164959016393444,
|
||||
"y": 6.80015368852459
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/H Backup.path
Normal file
54
src/main/deploy/pathplanner/paths/H Backup.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.7,
|
||||
"y": 4.3
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.053790983606557,
|
||||
"y": 4.312704918032787
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "H"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.389446721311475,
|
||||
"y": 4.3
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.1394487099079695,
|
||||
"y": 4.300997143065429
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -41,9 +41,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
54
src/main/deploy/pathplanner/paths/J Backup.path
Normal file
54
src/main/deploy/pathplanner/paths/J Backup.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.988527397260274,
|
||||
"y": 5.227054794520548
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.1321837955756875,
|
||||
"y": 5.49468698033176
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "J"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.442044107776481,
|
||||
"y": 6.005045141603656
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.268886874487802,
|
||||
"y": 5.749866060967707
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -118.30075576600632
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -121.60750224624898
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
65
src/main/deploy/pathplanner/paths/New Path.path
Normal file
65
src/main/deploy/pathplanner/paths/New Path.path
Normal file
@@ -0,0 +1,65 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.0,
|
||||
"y": 7.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.0,
|
||||
"y": 7.0
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 12.272,
|
||||
"y": 2.975
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 10.940715672291898,
|
||||
"y": 2.975
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [
|
||||
{
|
||||
"fieldPosition": {
|
||||
"x": 0.4,
|
||||
"y": 5.5
|
||||
},
|
||||
"rotationOffset": 0.0,
|
||||
"minWaypointRelativePos": 0.15,
|
||||
"maxWaypointRelativePos": 0.4,
|
||||
"name": "Point Towards Zone"
|
||||
}
|
||||
],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 59.99999999999999
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -41,9 +41,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "J"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -41,9 +41,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
61
src/main/deploy/pathplanner/paths/Start to H.path
Normal file
61
src/main/deploy/pathplanner/paths/Start to H.path
Normal file
@@ -0,0 +1,61 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.588217213114754,
|
||||
"y": 3.9890368852459024
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.916905737704918,
|
||||
"y": 4.036987704918033
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.7,
|
||||
"y": 4.3
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.347336065573771,
|
||||
"y": 4.2640368852459005
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "H"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.08214624881291864,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
70
src/main/deploy/pathplanner/paths/fein.path
Normal file
70
src/main/deploy/pathplanner/paths/fein.path
Normal file
@@ -0,0 +1,70 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.600204918039607,
|
||||
"y": 6.374590163938041
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.600204918032786,
|
||||
"y": 7.573360655737705
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.209631147540984,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.120696721311476,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 4.745363537952068,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.8670081967213115,
|
||||
"y": 6.973975409836065
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.826024590163935,
|
||||
"y": 6.9979508196721305
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -5,9 +5,9 @@
|
||||
"pathFolders": [],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 4.0,
|
||||
"defaultMaxAccel": 4.0,
|
||||
"defaultMaxAccel": 1.0,
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 720.0,
|
||||
"defaultMaxAngAccel": 400.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
"robotMass": 48.35,
|
||||
"robotMOI": 6.883,
|
||||
|
||||
@@ -4,7 +4,15 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import org.littletonrobotics.junction.LoggedRobot;
|
||||
import org.littletonrobotics.junction.LogFileUtil;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||
|
||||
import edu.wpi.first.wpilibj.PowerDistribution;
|
||||
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
@@ -14,11 +22,31 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
public class Robot extends LoggedRobot {
|
||||
|
||||
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private RobotContainer m_robotContainer;
|
||||
|
||||
@SuppressWarnings("resource")
|
||||
public Robot() {
|
||||
Logger.recordMetadata("ProjectName", "2025_Robot_Code"); // Set a metadata value
|
||||
|
||||
if (isReal()) {
|
||||
Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
|
||||
Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
|
||||
new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
|
||||
} else {
|
||||
setUseTiming(false); // Run as fast as possible
|
||||
String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
|
||||
Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
|
||||
Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
|
||||
}
|
||||
|
||||
Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be used for any
|
||||
* initialization code.
|
||||
|
||||
@@ -7,7 +7,9 @@ package frc.robot;
|
||||
import frc.robot.constants.ManipulatorPivotConstants;
|
||||
import frc.robot.constants.ClimberPivotConstants;
|
||||
import frc.robot.constants.ElevatorConstants;
|
||||
import frc.robot.constants.ManipulatorConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
import frc.robot.subsystems.ManipulatorPivot;
|
||||
import frc.robot.subsystems.Vision;
|
||||
import frc.robot.subsystems.ClimberPivot;
|
||||
@@ -15,11 +17,14 @@ import frc.robot.subsystems.ClimberRollers;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
import frc.robot.subsystems.Elevator;
|
||||
import frc.robot.subsystems.Manipulator;
|
||||
|
||||
import java.util.function.IntSupplier;
|
||||
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
import com.pathplanner.lib.events.EventTrigger;
|
||||
import com.pathplanner.lib.path.EventMarker;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
@@ -46,16 +51,20 @@ public class RobotContainer {
|
||||
private CommandXboxController operator;
|
||||
|
||||
private SendableChooser<Command> autoChooser;
|
||||
|
||||
private Vision vision;
|
||||
|
||||
private IntSupplier closestTag;
|
||||
|
||||
public RobotContainer() {
|
||||
climberPivot = new ClimberPivot();
|
||||
|
||||
climberRollers = new ClimberRollers();
|
||||
|
||||
//vision = new Vision(drivetrain::getGyroValue);
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
vision = new Vision(drivetrain::getGyroValue);
|
||||
|
||||
elevator = new Elevator();
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
@@ -63,15 +72,18 @@ public class RobotContainer {
|
||||
|
||||
manipulatorPivot = new ManipulatorPivot();
|
||||
|
||||
configureNamedCommands();
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
||||
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
||||
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
|
||||
closestTag = drivetrain::getClosestTag;
|
||||
|
||||
configureButtonBindings();
|
||||
//elevatorSysIDBindings();
|
||||
|
||||
configureNamedCommands();
|
||||
//elevatorOnlyBindings();
|
||||
|
||||
configureShuffleboard();
|
||||
}
|
||||
@@ -88,18 +100,18 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
//Default commands
|
||||
climberPivot.setDefaultCommand(
|
||||
climberPivot.runPivot(0)
|
||||
climberPivot.runPivot(() -> 0)
|
||||
);
|
||||
|
||||
climberRollers.setDefaultCommand(
|
||||
climberRollers.runRoller(0)
|
||||
climberRollers.runRoller(() -> 0)
|
||||
);
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver::getRightX,
|
||||
() -> Math.pow(driver.getLeftY(), 3),
|
||||
() -> Math.pow(driver.getLeftX(), 3),
|
||||
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
|
||||
() -> true
|
||||
)
|
||||
);
|
||||
@@ -116,7 +128,7 @@ public class RobotContainer {
|
||||
|
||||
manipulator.setDefaultCommand(
|
||||
manipulator.runUntilCollected(
|
||||
() -> 0
|
||||
() -> 0.0
|
||||
)
|
||||
);
|
||||
|
||||
@@ -127,23 +139,39 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
driver.rightTrigger().whileTrue(
|
||||
manipulator.runManipulator(() -> 1, true)
|
||||
manipulator.runManipulator(() -> 0.35, true)
|
||||
);
|
||||
|
||||
driver.leftTrigger().whileTrue(
|
||||
manipulator.runUntilCollected(() -> 0.75)
|
||||
manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
|
||||
);
|
||||
|
||||
driver.start().and(driver.back()).onTrue(
|
||||
startingConfig()
|
||||
);
|
||||
|
||||
driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
|
||||
driver.povUp().whileTrue(climberPivot.runPivot(0.5));
|
||||
|
||||
driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
|
||||
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
|
||||
driver.y().whileTrue(drivetrain.zeroHeading());
|
||||
|
||||
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
|
||||
|
||||
/*
|
||||
driver.rightBumper().whileTrue(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
|
||||
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
|
||||
)
|
||||
);
|
||||
|
||||
driver.leftBumper().whileTrue(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
|
||||
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
|
||||
)
|
||||
);
|
||||
*/
|
||||
|
||||
//Operator inputs
|
||||
operator.povUp().onTrue(
|
||||
safeMoveManipulator(
|
||||
@@ -173,27 +201,40 @@ public class RobotContainer {
|
||||
)
|
||||
);
|
||||
|
||||
operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
|
||||
|
||||
operator.a().onTrue(
|
||||
coralIntakeRoutine()
|
||||
safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
|
||||
);
|
||||
|
||||
operator.x().onTrue(
|
||||
algaeIntakeRoutine(true)
|
||||
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
);
|
||||
|
||||
operator.b().onTrue(
|
||||
algaeIntakeRoutine(false)
|
||||
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
);
|
||||
|
||||
operator.y().whileTrue(
|
||||
elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
|
||||
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
);
|
||||
|
||||
|
||||
}
|
||||
|
||||
private void configureNamedCommands() {
|
||||
new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
|
||||
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
||||
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
|
||||
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
|
||||
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
|
||||
}
|
||||
|
||||
//creates tabs and transforms them on the shuffleboard
|
||||
@@ -244,12 +285,22 @@ public class RobotContainer {
|
||||
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
|
||||
.withWidget(BuiltInWidgets.kGraph);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
|
||||
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator error", elevator::getPIDError)
|
||||
sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
@@ -258,6 +309,12 @@ public class RobotContainer {
|
||||
|
||||
sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
|
||||
|
||||
sensorTab.addDouble("dt distance", drivetrain::driveDistance);
|
||||
|
||||
sensorTab.addDouble("velocity", drivetrain::getVelocity);
|
||||
|
||||
|
||||
//sensorTab.add("odometry", drivetrain::getPose);
|
||||
|
||||
}
|
||||
|
||||
@@ -381,10 +438,22 @@ public class RobotContainer {
|
||||
* @param armPosition The target rotation of the arm
|
||||
* @return Moves the elevator and arm to the setpoints
|
||||
*/
|
||||
@SuppressWarnings("unused")
|
||||
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
|
||||
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
|
||||
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
|
||||
elevator.maintainPosition());*/
|
||||
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
|
||||
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
|
||||
.raceWith(elevator.maintainPosition()));
|
||||
}
|
||||
|
||||
private Command moveWithAlgae(double elevatorPosition, double armPosition) {
|
||||
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
|
||||
elevator.maintainPosition());*/
|
||||
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
|
||||
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
|
||||
.raceWith(elevator.maintainPosition()));
|
||||
}
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
|
||||
@@ -12,13 +12,13 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
public class AutoConstants {
|
||||
public static final double kMaxSpeedMetersPerSecond = 4;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 4;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 1;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
||||
|
||||
public static final double kPXController = 1;
|
||||
public static final double kPYController = 1;
|
||||
public static final double kPThetaController = 1;
|
||||
public static final double kPXController = 6;
|
||||
public static final double kPYController = 6;
|
||||
public static final double kPThetaController = 5.5;
|
||||
|
||||
// Constraint for the motion profiled robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
|
||||
|
||||
@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
public class DrivetrainConstants {
|
||||
// Driving Parameters - Note that these are not the maximum capable speeds of
|
||||
// the robot, rather the allowed maximum speeds
|
||||
public static final double kMaxSpeedMetersPerSecond = 4.8;
|
||||
public static final double kMaxSpeedMetersPerSecond = 5.5;
|
||||
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
|
||||
|
||||
// Chassis configuration
|
||||
@@ -22,10 +22,17 @@ public class DrivetrainConstants {
|
||||
public static final double kWheelBase = Units.inchesToMeters(24.5);
|
||||
|
||||
// Angular offsets of the modules relative to the chassis in radians
|
||||
/*
|
||||
public static final double kFrontLeftChassisAngularOffset = Math.PI;
|
||||
public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
|
||||
public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
|
||||
public static final double kBackRightChassisAngularOffset = 0;
|
||||
*/
|
||||
|
||||
public static final double kFrontLeftChassisAngularOffset = 0;
|
||||
public static final double kFrontRightChassisAngularOffset = Math.PI / 2;
|
||||
public static final double kBackLeftChassisAngularOffset = -Math.PI / 2;
|
||||
public static final double kBackRightChassisAngularOffset = Math.PI;
|
||||
|
||||
// 1, 7, 10 is the default for these three values
|
||||
public static final double kSysIDDrivingRampRate = 1;
|
||||
@@ -50,6 +57,12 @@ public class DrivetrainConstants {
|
||||
|
||||
public static final boolean kGyroReversed = true;
|
||||
|
||||
public static final double kHeadingP = 0.0;
|
||||
|
||||
public static final double kXTranslationP = 0.0;
|
||||
public static final double kYTranslationP = 0.0;
|
||||
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
// Distance between front and back wheels on robot
|
||||
|
||||
@@ -29,6 +29,8 @@ public class ElevatorConstants {
|
||||
public static final double kDownControllerP = 5.6;//7; //
|
||||
public static final double kDownControllerI = 0;
|
||||
public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
|
||||
|
||||
public static final double kMaintainP = 3;
|
||||
|
||||
public static final double kAllowedError = 1;
|
||||
|
||||
@@ -41,16 +43,18 @@ public class ElevatorConstants {
|
||||
|
||||
public static final double kCoralIntakePosition = 0;
|
||||
public static final double kL1Position = 0;
|
||||
public static final double kL2Position = 14.5;
|
||||
public static final double kL3Position = 29.0;
|
||||
public static final double kL4Position = 53.0;
|
||||
public static final double kL2Position = 9;
|
||||
public static final double kL3Position = 23.0;
|
||||
public static final double kL4Position = 50.5;
|
||||
public static final double kL4TransitionPosition = 40.0;
|
||||
public static final double kL2AlgaePosition = 22.0;
|
||||
public static final double kL2AlgaePosition = 23.0;
|
||||
public static final double kL3AlgaePosition = 39.0;
|
||||
public static final double kProcessorPosition = 4.0;
|
||||
/**The position of the top of the elevator brace */
|
||||
public static final double kBracePosition = 0;
|
||||
public static final double kMaxHeight = 47.5; //actual is 53
|
||||
public static final double kMaxHeight = 51.0; //actual is 51
|
||||
|
||||
public static final double kVoltageLimit = 7;
|
||||
|
||||
// 1, 7, 10 are the defaults for these, change as necessary
|
||||
public static final double kSysIDRampRate = .25;
|
||||
|
||||
@@ -23,7 +23,7 @@ public class ManipulatorPivotConstants {
|
||||
public static final double kPositionalP = 4;
|
||||
public static final double kPositionalI = 0;
|
||||
public static final double kPositionalD = 0;
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
|
||||
|
||||
public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
|
||||
public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
|
||||
@@ -34,19 +34,19 @@ public class ManipulatorPivotConstants {
|
||||
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
|
||||
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
|
||||
|
||||
public static final double kEncoderOffset = 0.7815;
|
||||
public static final double kEncoderOffset = 0.780;
|
||||
|
||||
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kL1Position = Units.degreesToRadians(0.0);
|
||||
public static final double kL2Position = Units.degreesToRadians(60.0);
|
||||
public static final double kL3Position = Units.degreesToRadians(60.0);
|
||||
public static final double kL2Position = Units.degreesToRadians(22.0);
|
||||
public static final double kL3Position = Units.degreesToRadians(22.0);
|
||||
public static final double kL4Position = Units.degreesToRadians(45.0);
|
||||
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kProcesserPosition = Units.degreesToRadians(175.0);
|
||||
public static final double kProcessorPosition = Units.degreesToRadians(175.0);
|
||||
public static final double kNetPosition = Units.degreesToRadians(175.0);
|
||||
/**The closest position to the elevator brace without hitting it */
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
|
||||
/**The forward rotation limit of the pivot */
|
||||
public static final double kRotationLimit = Units.degreesToRadians(175.0);
|
||||
|
||||
|
||||
@@ -2,6 +2,7 @@ package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
import com.ctre.phoenix6.configs.AudioConfigs;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.FeedbackConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
@@ -22,7 +23,7 @@ public class ModuleConstants {
|
||||
public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
|
||||
// 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
|
||||
// teeth on the bevel pinion
|
||||
public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
|
||||
public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15);
|
||||
public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
|
||||
/ kDrivingMotorReduction;
|
||||
|
||||
@@ -47,7 +48,7 @@ public class ModuleConstants {
|
||||
|
||||
public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
|
||||
|
||||
public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
|
||||
public static final InvertedValue kDriveInversionState = InvertedValue.CounterClockwise_Positive;
|
||||
public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
@@ -57,6 +58,7 @@ public class ModuleConstants {
|
||||
public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
|
||||
public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
|
||||
public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
|
||||
public static final AudioConfigs kAudioConfig = new AudioConfigs();
|
||||
public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
|
||||
|
||||
static {
|
||||
@@ -70,6 +72,8 @@ public class ModuleConstants {
|
||||
kDriveMotorConfig.Inverted = kDriveInversionState;
|
||||
kDriveMotorConfig.NeutralMode = kDriveIdleMode;
|
||||
|
||||
kAudioConfig.AllowMusicDurDisable = true;
|
||||
|
||||
kDriveSlot0Config.kP = kDriveP;
|
||||
kDriveSlot0Config.kI = kDriveI;
|
||||
kDriveSlot0Config.kD = kDriveD;
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class NeoMotorConstants {
|
||||
public static final double kFreeSpeedRpm = 5676;
|
||||
public static final double kFreeSpeedRpm = 6000; //for kraken not neo
|
||||
}
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class VisionConstants {
|
||||
|
||||
// global coordinate map of all tags. index is the tag id.
|
||||
// Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
|
||||
// This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
|
||||
@@ -28,4 +29,34 @@ public class VisionConstants {
|
||||
{209.49, 158.3, 12.13, 0, 0},
|
||||
{193.1, 129.97, 12.13, 300, 0},
|
||||
};
|
||||
|
||||
//map of coral placing setpoints based on the tag that is on the same reef face
|
||||
// and the on the left or right branch of that side of the reef
|
||||
// <tag_number, {left_x, left_y, right_x, right_y}>
|
||||
public static final double[][] reefSetpointsMap = {
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{4.993+12.272, 2.816, 5.272+12.272, 2.996},//6
|
||||
{5.789+12.272, 3.862, 5.789+12.272, 4.194},
|
||||
{5.275+12.272, 5.075, 4.991+12.272, 5.246},
|
||||
{3.986+12.272, 5.24, 3.701+12.272, 5.076},
|
||||
{3.183+12.272, 4.191, 3.183, 3.857},
|
||||
{3.703+12.272, 3.975, 3.982+12.272, 2.806},//11
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{3.703, 3.975, 3.982, 2.806},
|
||||
{3.183, 4.191, 3.183, 3.857},
|
||||
{3.986, 5.24, 3.701, 5.076},
|
||||
{5.275, 5.075, 4.991, 5.246},
|
||||
{5.789, 3.862, 5.789, 4.194},
|
||||
{4.993, 2.816, 5.272, 2.996}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
@@ -30,9 +32,9 @@ public class ClimberPivot extends SubsystemBase {
|
||||
neoEncoder = pivotMotor.getEncoder();
|
||||
}
|
||||
|
||||
public Command runPivot(double speed) {
|
||||
public Command runPivot(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
pivotMotor.set(speed);
|
||||
pivotMotor.set(speed.getAsDouble());
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
@@ -32,9 +34,9 @@ public class ClimberRollers extends SubsystemBase {
|
||||
* @param speed The speed in which the roller runs
|
||||
* @return Runs the rollers at a set speed
|
||||
*/
|
||||
public Command runRoller(double speed) {
|
||||
public Command runRoller(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
rollerMotor.set(speed);
|
||||
rollerMotor.set(speed.getAsDouble());
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,15 +4,18 @@
|
||||
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.Optional;
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.ctre.phoenix6.Orchestra;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.studica.frc.AHRS;
|
||||
import com.studica.frc.AHRS.NavXComType;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
@@ -21,12 +24,14 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.AutoConstants;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
|
||||
public class Drivetrain extends SubsystemBase {
|
||||
// Create MAXSwerveModules
|
||||
@@ -41,7 +46,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
// Odometry class for tracking robot pose
|
||||
private SwerveDrivePoseEstimator m_estimator;
|
||||
|
||||
private Vision vision;
|
||||
public Orchestra m_orchestra = new Orchestra();
|
||||
private Timer musicTimer = new Timer();
|
||||
|
||||
private PIDController pidHeading;
|
||||
private PIDController pidTranslationX;
|
||||
private PIDController pidTranslationY;
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public Drivetrain() {
|
||||
@@ -73,7 +83,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
m_estimator = new SwerveDrivePoseEstimator(
|
||||
DrivetrainConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(gyro.getAngle()),
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
@@ -83,6 +93,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
new Pose2d()
|
||||
);
|
||||
|
||||
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
|
||||
pidHeading.enableContinuousInput(-180, 180);
|
||||
|
||||
pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
|
||||
pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
|
||||
|
||||
AutoBuilder.configure(
|
||||
this::getPose,
|
||||
this::resetOdometry,
|
||||
@@ -99,6 +115,21 @@ public class Drivetrain extends SubsystemBase {
|
||||
},
|
||||
this
|
||||
);
|
||||
|
||||
m_orchestra.loadMusic(Filesystem.getDeployDirectory()
|
||||
.toPath()
|
||||
.resolve("Orchestra" + File.separator + "doomE1M1.chrp")
|
||||
.toString());
|
||||
|
||||
// Add a single device to the orchestra
|
||||
m_orchestra.addInstrument(m_frontLeft.getDrivingMotor(), 0);
|
||||
m_orchestra.addInstrument(m_frontRight.getDrivingMotor(), 1);
|
||||
m_orchestra.addInstrument(m_rearLeft.getDrivingMotor(), 2);
|
||||
m_orchestra.addInstrument(m_rearRight.getDrivingMotor(), 3);
|
||||
|
||||
m_orchestra.play();
|
||||
musicTimer.reset();
|
||||
musicTimer.start();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -130,6 +161,13 @@ public class Drivetrain extends SubsystemBase {
|
||||
}
|
||||
*/
|
||||
|
||||
if(musicTimer.get()>20){
|
||||
if (m_orchestra.isPlaying()) {
|
||||
m_orchestra.stop();
|
||||
}
|
||||
musicTimer.stop();
|
||||
musicTimer.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -165,7 +203,14 @@ public class Drivetrain extends SubsystemBase {
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_estimator.resetPose(
|
||||
m_estimator.resetPosition(
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
pose
|
||||
);
|
||||
}
|
||||
@@ -216,6 +261,50 @@ public class Drivetrain extends SubsystemBase {
|
||||
});
|
||||
}
|
||||
|
||||
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
|
||||
return run(() -> {
|
||||
drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()),
|
||||
pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()),
|
||||
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
|
||||
true);
|
||||
});
|
||||
}
|
||||
|
||||
public int getClosestTag(){
|
||||
|
||||
if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
|
||||
int closestTag = 17;
|
||||
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
|
||||
|
||||
for(int i = 17; i <= 22; ++i){
|
||||
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
|
||||
|
||||
if(distance < closestTagDist){
|
||||
closestTag = i;
|
||||
closestTagDist = distance;
|
||||
}
|
||||
}
|
||||
return closestTag;
|
||||
}else{
|
||||
int closestTag = 6;
|
||||
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
|
||||
|
||||
for(int i = 6; i <= 11; ++i){
|
||||
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
|
||||
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
|
||||
|
||||
if(distance < closestTagDist){
|
||||
closestTag = i;
|
||||
closestTagDist = distance;
|
||||
}
|
||||
}
|
||||
return closestTag;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the wheels into an X formation to prevent movement.
|
||||
*/
|
||||
@@ -248,9 +337,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearRight.resetEncoders();
|
||||
}
|
||||
|
||||
/** Zeroes the heading of the robot. */
|
||||
public void zeroHeading() {
|
||||
gyro.reset();
|
||||
/** Zeroes the heading of the robot.
|
||||
* @return */
|
||||
public Command zeroHeading() {
|
||||
return run(() -> {
|
||||
gyro.reset();
|
||||
});
|
||||
}
|
||||
|
||||
public double getGyroValue() {
|
||||
@@ -278,4 +370,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
public void addVisionMeasurement(Pose2d pose, double timestamp){
|
||||
m_estimator.addVisionMeasurement(pose, timestamp);
|
||||
}
|
||||
|
||||
public double driveDistance(){
|
||||
return m_frontLeft.getTotalDist();
|
||||
}
|
||||
|
||||
public double getVelocity(){
|
||||
return m_frontLeft.getState().speedMetersPerSecond;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,7 +24,10 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
private DigitalInput bottomLimitSwitch;
|
||||
|
||||
private PIDController pidController;
|
||||
private PIDController pidControllerUp;
|
||||
private PIDController pidControllerDown;
|
||||
|
||||
private PIDController maintainPID;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
|
||||
@@ -57,14 +60,27 @@ public class Elevator extends SubsystemBase {
|
||||
ElevatorConstants.kBottomLimitSwitchID
|
||||
);
|
||||
|
||||
pidController = new PIDController(
|
||||
pidControllerDown = new PIDController(
|
||||
ElevatorConstants.kDownControllerP,
|
||||
ElevatorConstants.kDownControllerI,
|
||||
ElevatorConstants.kDownControllerD
|
||||
);
|
||||
pidController.setSetpoint(0);
|
||||
pidControllerDown.setSetpoint(0);
|
||||
|
||||
pidController.setTolerance(ElevatorConstants.kAllowedError);
|
||||
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
pidControllerUp = new PIDController(
|
||||
ElevatorConstants.kUpControllerP,
|
||||
ElevatorConstants.kUpControllerI,
|
||||
ElevatorConstants.kUpControllerD
|
||||
);
|
||||
pidControllerUp.setSetpoint(0);
|
||||
|
||||
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
|
||||
|
||||
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardS,
|
||||
@@ -131,54 +147,31 @@ public class Elevator extends SubsystemBase {
|
||||
public Command maintainPosition() {
|
||||
|
||||
return startRun(() -> {
|
||||
|
||||
//pidController.reset();
|
||||
// pidController.setSetpoint(encoder.getPosition());
|
||||
maintainPID.reset();
|
||||
maintainPID.setSetpoint(pidControllerUp.getSetpoint());
|
||||
},
|
||||
() -> {
|
||||
/*if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition()
|
||||
) + feedForward.calculate(0)
|
||||
|
||||
double maintainOutput = maintainPID.calculate(getEncoderPosition());
|
||||
|
||||
if(!maintainPID.atSetpoint())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
maintainOutput + feedForward.calculate(0), -2, 2)
|
||||
);
|
||||
} else {
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}*/
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
*/
|
||||
|
||||
});
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
);
|
||||
*/
|
||||
/*
|
||||
if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
|
||||
});*/
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -189,29 +182,62 @@ public class Elevator extends SubsystemBase {
|
||||
* @return Sets motor voltage to achieve the target destination
|
||||
*/
|
||||
public Command goToSetpoint(DoubleSupplier setpoint) {
|
||||
|
||||
if (setpoint.getAsDouble() == 0) {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
|
||||
.andThen(runManualElevator(() -> -.1)
|
||||
.until(() -> encoder.getPosition() == 0));
|
||||
|
||||
return startRun(() -> {
|
||||
} else {
|
||||
return startRun(() -> {
|
||||
|
||||
pidController.setSetpoint(setpoint.getAsDouble());
|
||||
pidController.reset();
|
||||
},
|
||||
() -> {
|
||||
if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
setpoint.getAsDouble()
|
||||
) + feedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
}).until(() -> pidController.atSetpoint());
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
@@ -293,11 +319,19 @@ public class Elevator extends SubsystemBase {
|
||||
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
|
||||
}
|
||||
|
||||
public double getPIDSetpoint() {
|
||||
return pidController.getSetpoint();
|
||||
public double getPIDUpSetpoint() {
|
||||
return pidControllerUp.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDError() {
|
||||
return pidController.getError();
|
||||
public double getPIDUpError() {
|
||||
return pidControllerUp.getError();
|
||||
}
|
||||
|
||||
public double getPIDDownSetpoint() {
|
||||
return pidControllerDown.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDDownError() {
|
||||
return pidControllerDown.getError();
|
||||
}
|
||||
}
|
||||
@@ -56,6 +56,7 @@ public class MAXSwerveModule {
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
|
||||
|
||||
m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
|
||||
@@ -74,7 +75,7 @@ public class MAXSwerveModule {
|
||||
public SwerveModuleState getState() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
|
||||
return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
@@ -86,7 +87,7 @@ public class MAXSwerveModule {
|
||||
public SwerveModulePosition getPosition() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
|
||||
return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
@@ -107,9 +108,9 @@ public class MAXSwerveModule {
|
||||
// Command driving and turning SPARKS towards their respective setpoints.
|
||||
m_drive.setControl(
|
||||
driveVelocityRequest.withVelocity(
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
).withFeedForward(
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
)
|
||||
);
|
||||
|
||||
@@ -134,8 +135,16 @@ public class MAXSwerveModule {
|
||||
return m_turningSpark.get() * RobotController.getBatteryVoltage();
|
||||
}
|
||||
|
||||
public TalonFX getDrivingMotor(){
|
||||
return m_drive;
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_drive.setPosition(0);
|
||||
}
|
||||
|
||||
public double getTotalDist(){
|
||||
return m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -74,6 +74,13 @@ public class Manipulator extends SubsystemBase {
|
||||
}).until(() -> !coralBeamBreak.get());
|
||||
}
|
||||
|
||||
public Command retractCommand(DoubleSupplier retractSpeed){
|
||||
return run(() -> {
|
||||
manipulatorMotor.set(-retractSpeed.getAsDouble());
|
||||
}
|
||||
).until(() -> coralBeamBreak.get());
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the manipulator in a way that will bring the coral to a reliable holding position
|
||||
*
|
||||
|
||||
@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
);
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidController.disableContinuousInput();
|
||||
pidController.enableContinuousInput(0, 180);
|
||||
|
||||
feedForward = new ArmFeedforward(
|
||||
ManipulatorPivotConstants.kFeedForwardS,
|
||||
|
||||
@@ -40,9 +40,9 @@ public class Vision{
|
||||
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
|
||||
NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
|
||||
|
||||
blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
|
||||
blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
|
||||
blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
|
||||
blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
|
||||
blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
|
||||
blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
|
||||
|
||||
blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
|
||||
blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
|
||||
@@ -59,24 +59,23 @@ public class Vision{
|
||||
orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
|
||||
}
|
||||
|
||||
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
|
||||
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
|
||||
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
|
||||
VisionConstants.globalTagCoords[tagID][1],
|
||||
new Rotation2d());
|
||||
|
||||
Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
|
||||
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
|
||||
|
||||
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
|
||||
|
||||
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
|
||||
|
||||
return new Pose2d(globalPose.getTranslation(), gyroAngle);
|
||||
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
|
||||
}
|
||||
|
||||
public Pose2d getBlackGlobalPose(){
|
||||
return relativeToGlobalPose2d(getBlackClosestTag(),
|
||||
new Translation2d(getBlackRelativeX(), getBlackRelativeY()),
|
||||
new Rotation2d(gyroAngle.getAsDouble()));
|
||||
new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
|
||||
}
|
||||
|
||||
public double getBlackRelativeX(){
|
||||
@@ -109,8 +108,7 @@ public class Vision{
|
||||
|
||||
public Pose2d getOrangeGlobalPose(){
|
||||
return relativeToGlobalPose2d(getOrangeClosestTag(),
|
||||
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()),
|
||||
new Rotation2d(gyroAngle.getAsDouble()));
|
||||
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
|
||||
}
|
||||
|
||||
public double getOrangeRelativeX(){
|
||||
|
||||
35
vendordeps/AdvantageKit.json
Normal file
35
vendordeps/AdvantageKit.json
Normal file
@@ -0,0 +1,35 @@
|
||||
{
|
||||
"fileName": "AdvantageKit.json",
|
||||
"name": "AdvantageKit",
|
||||
"version": "4.1.1",
|
||||
"uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
|
||||
"frcYear": "2025",
|
||||
"mavenUrls": [
|
||||
"https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
|
||||
],
|
||||
"jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "org.littletonrobotics.akit",
|
||||
"artifactId": "akit-java",
|
||||
"version": "4.1.1"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "org.littletonrobotics.akit",
|
||||
"artifactId": "akit-wpilibio",
|
||||
"version": "4.1.1",
|
||||
"skipInvalidPlatforms": false,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"linuxarm64",
|
||||
"osxuniversal",
|
||||
"windowsx86-64"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": []
|
||||
}
|
||||
Reference in New Issue
Block a user