Compare commits
2 Commits
2c1899f3b5
...
brad_eleva
| Author | SHA1 | Date | |
|---|---|---|---|
| 431c1c2f01 | |||
| 771bcc89e1 |
@@ -10,11 +10,6 @@ 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 {
|
||||
@@ -77,9 +72,6 @@ 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 {
|
||||
|
||||
Binary file not shown.
@@ -1,31 +0,0 @@
|
||||
{
|
||||
"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
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "fein"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
@@ -34,9 +34,9 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -41,9 +41,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -46,9 +46,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"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": 1.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -1,65 +0,0 @@
|
||||
{
|
||||
"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": 1.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -41,9 +41,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -1,61 +0,0 @@
|
||||
{
|
||||
"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
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
{
|
||||
"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": 1.0,
|
||||
"defaultMaxAccel": 4.0,
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 400.0,
|
||||
"defaultMaxAngAccel": 720.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
"robotMass": 48.35,
|
||||
"robotMOI": 6.883,
|
||||
|
||||
@@ -4,15 +4,7 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
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.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
@@ -22,31 +14,11 @@ 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 LoggedRobot {
|
||||
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
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,9 +7,8 @@ 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.constants.ElevatorConstants.ElevatorPositions;
|
||||
import frc.robot.subsystems.ManipulatorPivot;
|
||||
import frc.robot.subsystems.Vision;
|
||||
import frc.robot.subsystems.ClimberPivot;
|
||||
@@ -17,21 +16,20 @@ 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;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
|
||||
public class RobotContainer {
|
||||
private ClimberPivot climberPivot;
|
||||
@@ -51,39 +49,32 @@ public class RobotContainer {
|
||||
private CommandXboxController operator;
|
||||
|
||||
private SendableChooser<Command> autoChooser;
|
||||
|
||||
private Vision vision;
|
||||
|
||||
private IntSupplier closestTag;
|
||||
|
||||
public RobotContainer() {
|
||||
climberPivot = new ClimberPivot();
|
||||
|
||||
climberRollers = new ClimberRollers();
|
||||
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
vision = new Vision(drivetrain::getGyroValue);
|
||||
|
||||
elevator = new Elevator();
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
manipulator = new Manipulator();
|
||||
|
||||
manipulatorPivot = new ManipulatorPivot();
|
||||
//vision = new Vision(drivetrain::getGyroValue);
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
configureNamedCommands();
|
||||
elevator = new Elevator(manipulator::getAlgaePhotoswitch);
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
manipulatorPivot = new ManipulatorPivot();
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
||||
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
||||
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
|
||||
closestTag = drivetrain::getClosestTag;
|
||||
|
||||
configureButtonBindings();
|
||||
//elevatorSysIDBindings();
|
||||
//elevatorOnlyBindings();
|
||||
|
||||
configureNamedCommands();
|
||||
|
||||
configureShuffleboard();
|
||||
}
|
||||
@@ -97,13 +88,6 @@ public class RobotContainer {
|
||||
operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
|
||||
}*/
|
||||
|
||||
private void elevatorOnlyBindings(){
|
||||
elevator.setDefaultCommand(elevator.maintainPosition());
|
||||
manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition());
|
||||
|
||||
driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position));
|
||||
}
|
||||
|
||||
private void configureButtonBindings() {
|
||||
//Default commands
|
||||
climberPivot.setDefaultCommand(
|
||||
@@ -116,18 +100,13 @@ public class RobotContainer {
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
() -> Math.pow(driver.getLeftY(), 3),
|
||||
() -> Math.pow(driver.getLeftX(), 3),
|
||||
driver::getRightX, //Math.pow(driver.getRightX(), 3)
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver::getRightX,
|
||||
() -> true
|
||||
)
|
||||
);
|
||||
|
||||
elevator.setDefaultCommand(
|
||||
elevator.maintainPosition()
|
||||
);
|
||||
|
||||
|
||||
manipulatorPivot.setDefaultCommand(
|
||||
manipulatorPivot.maintainPosition()
|
||||
);
|
||||
@@ -135,7 +114,7 @@ public class RobotContainer {
|
||||
|
||||
manipulator.setDefaultCommand(
|
||||
manipulator.runUntilCollected(
|
||||
() -> 0.0
|
||||
() -> 0
|
||||
)
|
||||
);
|
||||
|
||||
@@ -146,7 +125,7 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
driver.rightTrigger().whileTrue(
|
||||
manipulator.runManipulator(() -> 0.35, true)
|
||||
manipulator.runManipulator(() -> 1, true)
|
||||
);
|
||||
|
||||
driver.leftTrigger().whileTrue(
|
||||
@@ -156,93 +135,67 @@ public class RobotContainer {
|
||||
driver.start().and(driver.back()).onTrue(
|
||||
startingConfig()
|
||||
);
|
||||
|
||||
driver.y().whileTrue(drivetrain.zeroHeading());
|
||||
|
||||
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.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(
|
||||
ElevatorConstants.kL4Position,
|
||||
ElevatorPositions.kL4,
|
||||
ManipulatorPivotConstants.kL4Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povRight().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL3Position,
|
||||
ElevatorPositions.kL3,
|
||||
ManipulatorPivotConstants.kL3Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povLeft().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL2Position,
|
||||
ElevatorPositions.kL2,
|
||||
ManipulatorPivotConstants.kL2Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povDown().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL1Position,
|
||||
ElevatorPositions.kL1,
|
||||
ManipulatorPivotConstants.kL1Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.a().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
|
||||
coralIntakeRoutine()
|
||||
);
|
||||
|
||||
operator.x().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.until(driver.rightTrigger())
|
||||
algaeIntakeRoutine(true)
|
||||
);
|
||||
|
||||
operator.b().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.until(driver.rightTrigger())
|
||||
algaeIntakeRoutine(false)
|
||||
);
|
||||
|
||||
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.5, false))
|
||||
.until(driver.rightTrigger())
|
||||
operator.y().onTrue(
|
||||
elevator.setTargetPosition(ElevatorPositions.kL2)
|
||||
);
|
||||
|
||||
new Trigger(() -> Math.abs(MathUtil.applyDeadband(operator.getLeftY(), .05)) > .05).onTrue(
|
||||
elevator.runManualElevator(operator::getLeftY)
|
||||
);
|
||||
|
||||
|
||||
}
|
||||
|
||||
private void configureNamedCommands() {
|
||||
new EventTrigger("Lift L4").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
|
||||
@@ -293,22 +246,12 @@ public class RobotContainer {
|
||||
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
|
||||
.withWidget(BuiltInWidgets.kGraph);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
|
||||
sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
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)
|
||||
sensorTab.addDouble("Elevator error", elevator::getPIDError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
@@ -317,12 +260,6 @@ 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);
|
||||
|
||||
}
|
||||
|
||||
@@ -337,7 +274,7 @@ public class RobotContainer {
|
||||
@SuppressWarnings("unused")
|
||||
private Command coralIntakeRoutine() {
|
||||
return moveManipulator(
|
||||
ElevatorConstants.kCoralIntakePosition,
|
||||
ElevatorPositions.kCoralIntake,
|
||||
ManipulatorPivotConstants.kCoralIntakePosition
|
||||
)
|
||||
.andThen(manipulator.runUntilCollected(() -> .5));
|
||||
@@ -352,7 +289,7 @@ public class RobotContainer {
|
||||
@SuppressWarnings("unused")
|
||||
private Command algaeIntakeRoutine(boolean l2) {
|
||||
return moveManipulator(
|
||||
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition,
|
||||
l2 ? ElevatorPositions.kL2Algae : ElevatorPositions.kL3Algae,
|
||||
l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
|
||||
)
|
||||
.andThen(manipulator.runUntilCollected(() -> 1));
|
||||
@@ -365,10 +302,10 @@ public class RobotContainer {
|
||||
* @param armPosition The target rotation of the arm
|
||||
* @return Moves the elevator and arm to the setpoints using the most efficient path
|
||||
*/
|
||||
private Command moveManipulator(double elevatorPosition, double armPosition) {
|
||||
private Command moveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
|
||||
// If the elevator current and target positions are above the brace, or the arm current and target position is in
|
||||
// front of the brace, move together
|
||||
if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) {
|
||||
if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition.getPosition())) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) {
|
||||
return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
|
||||
// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
|
||||
// then the elevator, then the arm again
|
||||
@@ -397,7 +334,7 @@ public class RobotContainer {
|
||||
* @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
|
||||
* @return Moves the elevator and arm to the setpoints
|
||||
*/
|
||||
private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
|
||||
private Command moveManipulatorUtil(ElevatorPositions elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
|
||||
|
||||
/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
|
||||
armPosition = MathUtil.clamp(
|
||||
@@ -409,13 +346,16 @@ public class RobotContainer {
|
||||
|
||||
return Commands.either(
|
||||
Commands.either(
|
||||
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
elevator.setTargetPosition(elevatorPosition).andThen(
|
||||
new WaitUntilCommand(elevator::atSetpoint),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition)
|
||||
),
|
||||
elevator.setTargetPosition(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
() -> sequential
|
||||
),
|
||||
Commands.either(
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.setTargetPosition(elevatorPosition)),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.setTargetPosition(elevatorPosition)),
|
||||
() -> sequential
|
||||
),
|
||||
() -> elevatorFirst
|
||||
@@ -423,18 +363,24 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
|
||||
private Command manipulatorSafeTravel(ElevatorPositions elevatorPosition, double armPosition, boolean isL4){
|
||||
if(!isL4){
|
||||
return Commands.sequence(
|
||||
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
|
||||
elevator.goToSetpoint(() -> elevatorPosition),
|
||||
elevator.setTargetPosition(elevatorPosition),
|
||||
new WaitUntilCommand(elevator::atSetpoint),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition));
|
||||
|
||||
}else{
|
||||
return Commands.sequence(
|
||||
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
|
||||
elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
|
||||
Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
|
||||
elevator.setTargetPosition(elevatorPosition),
|
||||
new WaitUntilCommand(() -> elevator.getEncoderPosition() > ElevatorPositions.kL4Transition.getPosition()),
|
||||
Commands.parallel(
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition),
|
||||
elevator.setTargetPosition(elevatorPosition).andThen(new WaitUntilCommand(elevator::atSetpoint))
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -446,27 +392,15 @@ public class RobotContainer {
|
||||
* @param armPosition The target rotation of the arm
|
||||
* @return Moves the elevator and arm to the setpoints
|
||||
*/
|
||||
private Command safeMoveManipulator(double elevatorPosition, double 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")
|
||||
private Command safeMoveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
|
||||
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
|
||||
}
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private Command startingConfig() {
|
||||
return moveManipulatorUtil(0, 0, false, true)
|
||||
return moveManipulatorUtil(ElevatorPositions.kStartingPosition, 0, false, true)
|
||||
.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
|
||||
}
|
||||
}
|
||||
@@ -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 = 1;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 4;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
||||
|
||||
public static final double kPXController = 6;
|
||||
public static final double kPYController = 6;
|
||||
public static final double kPThetaController = 5.5;
|
||||
public static final double kPXController = 1;
|
||||
public static final double kPYController = 1;
|
||||
public static final double kPThetaController = 1;
|
||||
|
||||
// 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 = 5.5;
|
||||
public static final double kMaxSpeedMetersPerSecond = 4.8;
|
||||
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
|
||||
|
||||
// Chassis configuration
|
||||
@@ -22,17 +22,10 @@ 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;
|
||||
@@ -57,12 +50,6 @@ 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
|
||||
|
||||
@@ -11,6 +11,34 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
|
||||
public class ElevatorConstants {
|
||||
public enum ElevatorControlMode {
|
||||
kPID,
|
||||
kManualMaintain
|
||||
}
|
||||
|
||||
public enum ElevatorPositions {
|
||||
kStartingPosition(0),
|
||||
kCoralIntake(0),
|
||||
kL1(0),
|
||||
kL2(14.5),
|
||||
kL2Algae(22),
|
||||
kL3(29),
|
||||
kL3Algae(39),
|
||||
kL4(53),
|
||||
kL4Transition(40),
|
||||
kProcessor(4);
|
||||
|
||||
private double position;
|
||||
|
||||
private ElevatorPositions(double position) {
|
||||
this.position = position;
|
||||
}
|
||||
|
||||
public double getPosition() {
|
||||
return position;
|
||||
}
|
||||
}
|
||||
|
||||
public static final int kElevatorMotor1ID = 8;
|
||||
public static final int kElevatorMotor2ID = 6;
|
||||
|
||||
@@ -36,23 +64,16 @@ public class ElevatorConstants {
|
||||
public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6
|
||||
public static final double kFeedForwardV = 0.12; // calculated value 0.12
|
||||
|
||||
public static final double kFeedForwardAlgaeHeldS = 0;
|
||||
public static final double kFeedForwardAlgaeHeldG = 0;
|
||||
public static final double kFeedForwardAlgaeHeldV = 0;
|
||||
|
||||
public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
|
||||
public static final double kCoralIntakePosition = 0;
|
||||
public static final double kL1Position = 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 = 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 = 51.0; //actual is 51
|
||||
|
||||
public static final double kVoltageLimit = 7;
|
||||
public static final double kMaxHeight = 47.5; //actual is 53
|
||||
|
||||
// 1, 7, 10 are the defaults for these, change as necessary
|
||||
public static final double kSysIDRampRate = .25;
|
||||
|
||||
@@ -4,7 +4,9 @@ import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
|
||||
public class ManipulatorConstants {
|
||||
public static final int kManipulatorMotorID = 12;
|
||||
|
||||
public static final int kCoralBeamBreakID = 2;
|
||||
public static final int kAlgaePhotoswitchID = 3;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
}
|
||||
|
||||
@@ -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(1.5);
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
|
||||
|
||||
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.780;
|
||||
public static final double kEncoderOffset = 0.7815;
|
||||
|
||||
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(22.0);
|
||||
public static final double kL3Position = Units.degreesToRadians(22.0);
|
||||
public static final double kL2Position = Units.degreesToRadians(60.0);
|
||||
public static final double kL3Position = Units.degreesToRadians(60.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 kProcessorPosition = Units.degreesToRadians(175.0);
|
||||
public static final double kProcesserPosition = 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(71.0);
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
|
||||
/**The forward rotation limit of the pivot */
|
||||
public static final double kRotationLimit = Units.degreesToRadians(175.0);
|
||||
|
||||
|
||||
@@ -22,7 +22,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 * 20) / (kDrivingMotorPinionTeeth * 15);
|
||||
public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
|
||||
public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
|
||||
/ kDrivingMotorReduction;
|
||||
|
||||
@@ -47,7 +47,7 @@ public class ModuleConstants {
|
||||
|
||||
public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
|
||||
|
||||
public static final InvertedValue kDriveInversionState = InvertedValue.CounterClockwise_Positive;
|
||||
public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_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
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class NeoMotorConstants {
|
||||
public static final double kFreeSpeedRpm = 6000; //for kraken not neo
|
||||
public static final double kFreeSpeedRpm = 5676;
|
||||
}
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
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.
|
||||
@@ -29,34 +28,4 @@ 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}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -4,18 +4,15 @@
|
||||
|
||||
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;
|
||||
@@ -25,14 +22,11 @@ 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
|
||||
@@ -49,13 +43,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
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() {
|
||||
m_frontLeft = new MAXSwerveModule(
|
||||
@@ -86,7 +73,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
m_estimator = new SwerveDrivePoseEstimator(
|
||||
DrivetrainConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
Rotation2d.fromDegrees(gyro.getAngle()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
@@ -96,12 +83,6 @@ 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,
|
||||
@@ -118,21 +99,6 @@ 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
|
||||
@@ -164,13 +130,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
}
|
||||
*/
|
||||
|
||||
if(musicTimer.get()>4){
|
||||
if (m_orchestra.isPlaying()) {
|
||||
m_orchestra.stop();
|
||||
}
|
||||
musicTimer.stop();
|
||||
musicTimer.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -257,50 +216,6 @@ 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.
|
||||
*/
|
||||
@@ -333,12 +248,9 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearRight.resetEncoders();
|
||||
}
|
||||
|
||||
/** Zeroes the heading of the robot.
|
||||
* @return */
|
||||
public Command zeroHeading() {
|
||||
return run(() -> {
|
||||
gyro.reset();
|
||||
});
|
||||
/** Zeroes the heading of the robot. */
|
||||
public void zeroHeading() {
|
||||
gyro.reset();
|
||||
}
|
||||
|
||||
public double getGyroValue() {
|
||||
@@ -366,12 +278,4 @@ 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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
@@ -15,6 +16,8 @@ import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.ElevatorConstants;
|
||||
import frc.robot.constants.ElevatorConstants.ElevatorControlMode;
|
||||
import frc.robot.constants.ElevatorConstants.ElevatorPositions;
|
||||
|
||||
public class Elevator extends SubsystemBase {
|
||||
protected SparkMax elevatorMotor1;
|
||||
@@ -22,14 +25,20 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
protected RelativeEncoder encoder;
|
||||
|
||||
private BooleanSupplier hasAlgae;
|
||||
|
||||
private DigitalInput bottomLimitSwitch;
|
||||
|
||||
private PIDController pidControllerUp;
|
||||
private PIDController pidControllerDown;
|
||||
private PIDController pidController;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
private ElevatorFeedforward algaeHeldFeedForward;
|
||||
|
||||
private ElevatorControlMode mode;
|
||||
|
||||
private ElevatorPositions currentTargetPosition;
|
||||
|
||||
public Elevator() {
|
||||
public Elevator(BooleanSupplier hasAlgae) {
|
||||
elevatorMotor1 = new SparkMax(
|
||||
ElevatorConstants.kElevatorMotor1ID,
|
||||
MotorType.kBrushless
|
||||
@@ -54,35 +63,36 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
encoder = elevatorMotor1.getEncoder();
|
||||
|
||||
this.hasAlgae = hasAlgae;
|
||||
|
||||
bottomLimitSwitch = new DigitalInput(
|
||||
ElevatorConstants.kBottomLimitSwitchID
|
||||
);
|
||||
|
||||
pidControllerDown = new PIDController(
|
||||
pidController = new PIDController(
|
||||
ElevatorConstants.kDownControllerP,
|
||||
ElevatorConstants.kDownControllerI,
|
||||
ElevatorConstants.kDownControllerD
|
||||
);
|
||||
pidControllerDown.setSetpoint(0);
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
pidControllerUp = new PIDController(
|
||||
ElevatorConstants.kUpControllerP,
|
||||
ElevatorConstants.kUpControllerI,
|
||||
ElevatorConstants.kUpControllerD
|
||||
);
|
||||
pidControllerUp.setSetpoint(0);
|
||||
|
||||
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
|
||||
pidController.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardS,
|
||||
ElevatorConstants.kFeedForwardG,
|
||||
ElevatorConstants.kFeedForwardV
|
||||
);
|
||||
|
||||
algaeHeldFeedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardAlgaeHeldS,
|
||||
ElevatorConstants.kFeedForwardAlgaeHeldG,
|
||||
ElevatorConstants.kFeedForwardAlgaeHeldV
|
||||
);
|
||||
|
||||
mode = ElevatorControlMode.kPID;
|
||||
|
||||
currentTargetPosition = ElevatorPositions.kCoralIntake;
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -90,6 +100,27 @@ public class Elevator extends SubsystemBase {
|
||||
if (!getBottomLimitSwitch()) {
|
||||
encoder.setPosition(0);
|
||||
}
|
||||
|
||||
if(mode == ElevatorControlMode.kPID) {
|
||||
if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
currentTargetPosition.getPosition()
|
||||
) + (hasAlgae.getAsBoolean() ? algaeHeldFeedForward.calculate(0) : feedForward.calculate(0))
|
||||
);
|
||||
} else {
|
||||
if (hasAlgae.getAsBoolean()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
algaeHeldFeedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -120,7 +151,10 @@ public class Elevator extends SubsystemBase {
|
||||
* @return Sets motor voltage to translate the elevator and maintain position
|
||||
*/
|
||||
public Command runManualElevator(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
return startRun(() -> {
|
||||
mode = ElevatorControlMode.kManualMaintain;
|
||||
},
|
||||
() -> {
|
||||
double desired = speed.getAsDouble();
|
||||
|
||||
if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
|
||||
@@ -128,158 +162,33 @@ public class Elevator extends SubsystemBase {
|
||||
speed.getAsDouble()
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(feedForward.calculate(0));
|
||||
if (hasAlgae.getAsBoolean()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
algaeHeldFeedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* A command that will use the feed forward to hold up the elevator.
|
||||
* Used for feed forward tuning.
|
||||
*
|
||||
* @return Sets motor voltage based on feed forward calculation.
|
||||
*/
|
||||
public Command maintainPosition() {
|
||||
|
||||
return startRun(() -> {
|
||||
/*
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
*/
|
||||
},
|
||||
() -> {
|
||||
/*
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(pidControllerUp.getSetpoint()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), -1, 1)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), -1, 1)
|
||||
);
|
||||
}
|
||||
*/
|
||||
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
public Command setTargetPosition(ElevatorPositions position) {
|
||||
return runOnce(() -> {
|
||||
if (mode == ElevatorControlMode.kManualMaintain) {
|
||||
pidController.reset();
|
||||
mode = ElevatorControlMode.kPID;
|
||||
}
|
||||
|
||||
currentTargetPosition = position;
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the elevator to a target destination (setpoint).
|
||||
*
|
||||
* @param setpoint Target destination of the subsystem
|
||||
* @param timeout Time to achieve the setpoint before quitting
|
||||
* @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));
|
||||
|
||||
} else {
|
||||
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());
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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)
|
||||
);
|
||||
}
|
||||
|
||||
});*/
|
||||
public boolean atSetpoint() {
|
||||
return pidController.atSetpoint();
|
||||
}
|
||||
|
||||
/*
|
||||
if(encoder.getPosition() >= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerUp.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
|
||||
);
|
||||
}else if(encoder.getPosition() <= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerDown.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
|
||||
);
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* Returns the current encoder position
|
||||
@@ -318,19 +227,11 @@ public class Elevator extends SubsystemBase {
|
||||
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
|
||||
}
|
||||
|
||||
public double getPIDUpSetpoint() {
|
||||
return pidControllerUp.getSetpoint();
|
||||
public double getPIDSetpoint() {
|
||||
return pidController.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDUpError() {
|
||||
return pidControllerUp.getError();
|
||||
}
|
||||
|
||||
public double getPIDDownSetpoint() {
|
||||
return pidControllerDown.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDDownError() {
|
||||
return pidControllerDown.getError();
|
||||
public double getPIDError() {
|
||||
return pidController.getError();
|
||||
}
|
||||
}
|
||||
@@ -74,7 +74,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() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
@@ -86,7 +86,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() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
@@ -107,9 +107,9 @@ public class MAXSwerveModule {
|
||||
// Command driving and turning SPARKS towards their respective setpoints.
|
||||
m_drive.setControl(
|
||||
driveVelocityRequest.withVelocity(
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
).withFeedForward(
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
)
|
||||
);
|
||||
|
||||
@@ -134,16 +134,8 @@ 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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -16,6 +16,7 @@ public class Manipulator extends SubsystemBase {
|
||||
private SparkMax manipulatorMotor;
|
||||
|
||||
private DigitalInput coralBeamBreak;
|
||||
private DigitalInput algaePhotoswitch;
|
||||
|
||||
public Manipulator() {
|
||||
manipulatorMotor = new SparkMax(
|
||||
@@ -30,6 +31,7 @@ public class Manipulator extends SubsystemBase {
|
||||
);
|
||||
|
||||
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
|
||||
algaePhotoswitch = new DigitalInput(ManipulatorConstants.kAlgaePhotoswitchID);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -74,13 +76,6 @@ public class Manipulator extends SubsystemBase {
|
||||
}).until(() -> !coralBeamBreak.get());
|
||||
}
|
||||
|
||||
public Command retractCommand(DoubleSupplier speed){
|
||||
return run(() -> {
|
||||
manipulatorMotor.set(-speed.getAsDouble());
|
||||
}
|
||||
).until(() -> coralBeamBreak.get());
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the manipulator in a way that will bring the coral to a reliable holding position
|
||||
*
|
||||
@@ -97,4 +92,8 @@ public class Manipulator extends SubsystemBase {
|
||||
public boolean getCoralBeamBreak() {
|
||||
return coralBeamBreak.get();
|
||||
}
|
||||
|
||||
public boolean getAlgaePhotoswitch() {
|
||||
return algaePhotoswitch.get();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
);
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidController.enableContinuousInput(0, 180);
|
||||
pidController.disableContinuousInput();
|
||||
|
||||
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 = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
|
||||
blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
|
||||
blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
|
||||
blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
|
||||
blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
|
||||
blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
|
||||
|
||||
blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
|
||||
blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
|
||||
@@ -59,23 +59,24 @@ public class Vision{
|
||||
orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
|
||||
}
|
||||
|
||||
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
|
||||
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
|
||||
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
|
||||
VisionConstants.globalTagCoords[tagID][1],
|
||||
new Rotation2d());
|
||||
|
||||
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
|
||||
Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
|
||||
|
||||
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
|
||||
|
||||
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
|
||||
|
||||
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
|
||||
return new Pose2d(globalPose.getTranslation(), gyroAngle);
|
||||
}
|
||||
|
||||
public Pose2d getBlackGlobalPose(){
|
||||
return relativeToGlobalPose2d(getBlackClosestTag(),
|
||||
new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
|
||||
new Translation2d(getBlackRelativeX(), getBlackRelativeY()),
|
||||
new Rotation2d(gyroAngle.getAsDouble()));
|
||||
}
|
||||
|
||||
public double getBlackRelativeX(){
|
||||
@@ -108,7 +109,8 @@ public class Vision{
|
||||
|
||||
public Pose2d getOrangeGlobalPose(){
|
||||
return relativeToGlobalPose2d(getOrangeClosestTag(),
|
||||
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
|
||||
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()),
|
||||
new Rotation2d(gyroAngle.getAsDouble()));
|
||||
}
|
||||
|
||||
public double getOrangeRelativeX(){
|
||||
|
||||
@@ -2,6 +2,9 @@ package frc.robot.subsystems.sysid;
|
||||
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.Volts;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import static edu.wpi.first.units.Units.InchesPerSecond;
|
||||
|
||||
import edu.wpi.first.units.measure.MutDistance;
|
||||
@@ -22,8 +25,8 @@ public class ElevatorSysID extends Elevator {
|
||||
|
||||
private SysIdRoutine routine;
|
||||
|
||||
public ElevatorSysID() {
|
||||
super();
|
||||
public ElevatorSysID(BooleanSupplier hasAlgae) {
|
||||
super(hasAlgae);
|
||||
|
||||
appliedVoltage = Volts.mutable(0);
|
||||
|
||||
@@ -38,7 +41,7 @@ public class ElevatorSysID extends Elevator {
|
||||
(log) -> {
|
||||
log.motor("elevatorMotor1")
|
||||
.voltage(appliedVoltage.mut_replace(
|
||||
elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
|
||||
getMotor1(), Volts
|
||||
))
|
||||
.linearPosition(elevatorPosition.mut_replace(
|
||||
encoder.getPosition(), Inches
|
||||
|
||||
@@ -1,35 +0,0 @@
|
||||
{
|
||||
"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