Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

pose, logger, and barge shot tweaks

Browse Source
This commit is contained in:
Tylr-J42 2025-04-13 04:32:46 -04:00
parent 339bf642a1
commit 0589463c4e
16 changed files with 289 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/deploy/pathplanner/autos/1 L4 Center.auto
View File

@ -27,6 +27,12 @@
"data": {
"pathName": "H Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}

55
src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto Normal file
View File

@ -0,0 +1,55 @@
{
"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"
}
},
{
"type": "named",
"data": {
"name": "Pickup Algae L2"
}
},
{
"type": "path",
"data": {
"pathName": "HG Algae"
}
},
{
"type": "path",
"data": {
"pathName": "HG to Barge"
}
},
{
"type": "named",
"data": {
"name": "Shoot Algae"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

24
src/main/deploy/pathplanner/paths/H Backup.path
View File

@ -3,29 +3,29 @@
"waypoints": [
{
"anchor": {
"x": 5.7,
"y": 4.3
"x": 5.758260140458621,
"y": 4.193633481772718
},
"prevControl": null,
"nextControl": {
"x": 6.053790983606557,
"y": 4.312704918032787
"x": 6.112051124065178,
"y": 4.206338399805505
},
"isLocked": false,
"linkedName": "H"
},
{
"anchor": {
"x": 6.389446721311475,
"y": 4.3
"x": 6.5,
"y": 4.021985060730393
},
"prevControl": {
"x": 6.1394487099079695,
"y": 4.300997143065429
"x": 6.250001988596495,
"y": 4.022982203795822
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "Behind HG Face"
}
],
"rotationTargets": [],
@ -34,7 +34,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAcceleration": 0.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -45,10 +45,10 @@
"rotation": 180.0
},
"reversed": false,
"folder": null,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

26
src/main/deploy/pathplanner/paths/New Path.path → src/main/deploy/pathplanner/paths/HG Algae.path
View File

@ -3,29 +3,29 @@
"waypoints": [
{
"anchor": {
"x": 8.786987704919465,
"y": 4.348668032784689
"x": 6.5,
"y": 4.021985060730393
},
"prevControl": null,
"nextControl": {
"x": 9.786987704919456,
"y": 4.348668032784689
"x": 6.224227344787433,
"y": 4.009974315068493
},
"isLocked": false,
"linkedName": null
"linkedName": "Behind HG Face"
},
{
"anchor": {
"x": 11.256454918034219,
"y": 4.348668032784689
"x": 5.872671261177789,
"y": 4.021985060730393
},
"prevControl": {
"x": 7.543546317006037,
"y": 6.492316813064786
"x": 6.125859753477119,
"y": 4.004837296105254
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "HG Algae"
}
],
"rotationTargets": [],
@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": 180.0
},
"reversed": false,
"folder": null,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 124.695153531234
"rotation": 180.0
},
"useDefaultConstraints": true
}

71
src/main/deploy/pathplanner/paths/HG to Barge.path Normal file
View File

@ -0,0 +1,71 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.872671261177789,
"y": 4.021985060730393
},
"prevControl": null,
"nextControl": {
"x": 7.829105562465259,
"y": 3.8676166530198106
},
"isLocked": false,
"linkedName": "HG Algae"
},
{
"anchor": {
"x": 7.032020547945206,
"y": 4.761258561643835
},
"prevControl": {
"x": 7.262336691900294,
"y": 4.359747737103425
},
"nextControl": null,
"isLocked": false,
"linkedName": "Pre-Barge"
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.17234468937875755,
"rotationDegrees": 180.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Processor Position",
"waypointRelativePos": 0.188095238095238,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Processor Position"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 300.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}

4
src/main/deploy/pathplanner/paths/HP to K.path
View File

@ -46,7 +46,7 @@
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -62,5 +62,5 @@
"velocity": 0,
"rotation": -53.97262661489646
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

28
src/main/deploy/pathplanner/paths/Start to H.path
View File

@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 5.7,
"y": 4.3
"x": 5.758260140458621,
"y": 4.193633481772718
},
"prevControl": {
"x": 6.347336065573771,
"y": 4.2640368852459005
"x": 6.405596206032391,
"y": 4.157670367018619
},
"nextControl": null,
"isLocked": false,
@ -31,10 +31,22 @@
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.10238095238095252,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -45,10 +57,10 @@
"rotation": 180.0
},
"reversed": false,
"folder": null,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

3
src/main/deploy/pathplanner/settings.json
View File

@ -3,7 +3,8 @@
"robotLength": 0.8763,
"holonomicMode": true,
"pathFolders": [
"Left Paths"
"Left Paths",
"Center"
],
"autoFolders": [],
"defaultMaxVel": 3.5,

18
src/main/java/frc/robot/RobotContainer.java
View File

@ -351,6 +351,18 @@ public class RobotContainer {
ManipulatorPivotConstants.kStartingPosition
)
);
NamedCommands.registerCommand(
"Shoot Algae",
shootAlgae()
);
NamedCommands.registerCommand(
"Processor Position",
moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
);
}
//creates tabs and transforms them on the shuffleboard
@ -607,10 +619,10 @@ public class RobotContainer {
}
private Command shootAlgae(){
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition()));
}

4
src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@ -23,7 +23,9 @@ public class ManipulatorPivotConstants {
public static final double kPositionalP = 4.5;
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);
public static final double kAlgaeP = 7;
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

2
src/main/java/frc/robot/constants/VisionConstants.java
View File

@ -85,6 +85,6 @@ public class VisionConstants {
{5.322, 2.511}//22
};
public static final double latencyFudge = 0.0;
public static final double latencyFudge = 0.03;
}

27
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -111,8 +111,8 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
},
new Pose2d(),
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
VecBuilder.fill(0.07, 0.7, Units.degreesToRadians(0.5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(4000))
);
pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
@ -177,17 +177,17 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(4000)));
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
}else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
}
// if the detected tags match your alliances reef tags use their pose estimates
@ -203,17 +203,18 @@ public class Drivetrain extends SubsystemBase {
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
Logger.recordOutput("orange dist", vision.getOrangeDist());
Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
}else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
@ -227,9 +228,11 @@ public class Drivetrain extends SubsystemBase {
}
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
Logger.recordOutput("black dist", vision.getBlackDist());
Logger.recordOutput("orange detected", vision.getBlackTagDetected());
Logger.recordOutput("black detected", vision.getBlackTagDetected());
Logger.recordOutput("black tag", vision.getBlackTagDetected());
Logger.recordOutput("drive velocity", getVelocity());
Logger.recordOutput("closest tag", getClosestTag());
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>10){

49
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -2,6 +2,8 @@ package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
@ -94,6 +96,10 @@ public class Elevator extends SubsystemBase {
if (!getBottomLimitSwitch()) {
encoder.setPosition(0);
}
Logger.recordOutput("elevator position", getEncoderPosition());
Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint());
Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint());
}
/**
@ -245,51 +251,8 @@ public class Elevator extends SubsystemBase {
}).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)
);
}
});*/
}
/*
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
*

9
src/main/java/frc/robot/subsystems/Manipulator.java
View File

@ -2,6 +2,8 @@ package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
@ -46,6 +48,13 @@ public class Manipulator extends SubsystemBase {
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
}
@Override
public void periodic() {
super.periodic();
Logger.recordOutput("coral beam break", getCoralBeamBreak());
}
/**
* The default command for the manipulator that either stops the manipulator or slowly
* runs the manipulator to retain the algae

56
src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@ -4,6 +4,8 @@ import com.revrobotics.spark.SparkMax;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
@ -25,6 +27,8 @@ public class ManipulatorPivot extends SubsystemBase {
private PIDController pidController;
private PIDController algaePIDController;
public ManipulatorPivot() {
pivotMotor = new SparkMax(
ManipulatorPivotConstants.kPivotMotorID,
@ -44,10 +48,22 @@ public class ManipulatorPivot extends SubsystemBase {
ManipulatorPivotConstants.kPositionalI,
ManipulatorPivotConstants.kPositionalD
);
pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
pidController.setSetpoint(0);
pidController.enableContinuousInput(0, 280);
algaePIDController = new PIDController(
ManipulatorPivotConstants.kAlgaeP,
0,
0);
algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
algaePIDController.setSetpoint(0);
algaePIDController.enableContinuousInput(0, 280);
feedForward = new ArmFeedforward(
ManipulatorPivotConstants.kFeedForwardS,
ManipulatorPivotConstants.kFeedForwardG,
@ -55,6 +71,14 @@ public class ManipulatorPivot extends SubsystemBase {
);
}
@Override
public void periodic() {
super.periodic();
Logger.recordOutput("manipulator position", getEncoderPosition());
Logger.recordOutput("manipulator setpoint", pidController.getSetpoint());
}
/**
* Returns whether or not the motion is safe relative to the encoder's current position
* and the arm safe stow position
@ -125,6 +149,38 @@ public class ManipulatorPivot extends SubsystemBase {
}).until(() -> pidController.atSetpoint());
}
public Command goToSetpointAlgae(DoubleSupplier setpoint) {
return startRun(() -> {
algaePIDController.setSetpoint(setpoint.getAsDouble());
algaePIDController.reset();
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
},
() -> {
/*
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
} else {
pivotMotor.setVoltage(
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}
*/
pivotMotor.setVoltage(
algaePIDController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}).until(() -> algaePIDController.atSetpoint());
}
public Command maintainPosition() {
return startRun(() -> {

4
src/main/java/frc/robot/subsystems/Vision.java
View File

@ -177,11 +177,11 @@ public class Vision{
}
public boolean isBlackConnected(){
return Timer.getFPGATimestamp()-black_tx.getLastChange() > 2.0;
return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
}
public boolean isOrangeConnected(){
return Timer.getFPGATimestamp()-orange_tx.getLastChange() > 2.0;
return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
}
}