Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Optimization and improvements

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-03 16:32:27 -07:00
parent 97630fe2b4
commit 204ba6d511
7 changed files with 74 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path
+7 -7
View File
@@ -8,20 +8,20 @@
},
"prevControl": null,
"nextControl": {
"x": 4.39747159090909,
"y": 1.3027556818181814
"x": 2.4125846006173957,
"y": 1.7771562346874141
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.3048863636363635,
"y": 2.5292613636363637
"x": 3.5997443181818185,
"y": 2.6888068181818174
},
"prevControl": {
"x": 5.893210227272728,
"y": 1.9808238636363626
"x": 2.6837095087024796,
"y": 1.9835311238550895
},
"nextControl": null,
"isLocked": false,
@@ -42,7 +42,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 114.22774531795413
"rotation": 56.633633998940475
},
"reversed": false,
"folder": null,
src/main/java/frc4388/robot/Constants.java
+12 -6
View File
@@ -69,7 +69,7 @@ public final class Constants {
public static final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = 3.5;
public static final double MAX_ROT_SPEED = Math.PI * 2;
public static final double AUTO_MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 1.0;
public static double ROTATION_SPEED = MAX_ROT_SPEED;
@@ -329,7 +329,7 @@ public final class Constants {
}
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
@@ -339,6 +339,9 @@ public final class Constants {
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
public static final double MIN_XY_PID_OUTPUT = 0.0;
public static final double MIN_ROT_PID_OUTPUT = 0.0;
// X is tangent to reef side
// Y is normal to reef side
@@ -346,7 +349,7 @@ public final class Constants {
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
@@ -366,15 +369,15 @@ public final class Constants {
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547+(1*-1)), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
// Photonvision thing
// The standard deviations of our vision estimated poses, which affect correction rate
// X, Y, Theta
// https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(3, 3, 4);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
}
@@ -424,6 +427,8 @@ public final class Constants {
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
// public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
@@ -432,6 +437,7 @@ public final class Constants {
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
public static final double SCORING_THREE_ELEVATOR = -9.25;
public static final double DEALGAE_L2_ELEVATOR = -23.5;
src/main/java/frc4388/robot/RobotContainer.java
+25 -9
View File
@@ -148,8 +148,12 @@ public class RobotContainer {
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new ConditionalCommand(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
() -> m_robotElevator.hasCoral()),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -177,11 +181,17 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new ConditionalCommand(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
() -> m_robotElevator.hasCoral()),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -295,8 +305,10 @@ public class RobotContainer {
);
private Command thrustIntake = new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true),
new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)
new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod(), m_robotSwerveDrive),
new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 300, true),
new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive),
new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive)
);
private Boolean operatorManualMode = false;
@@ -310,7 +322,7 @@ public class RobotContainer {
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0, 1),
new Translation2d(), 200, true
new Translation2d(), 400, true
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
@@ -428,6 +440,10 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod()));
// While Left Trigger NOT Pressed: Fine Alignment
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8))
.whileTrue(new RunCommand(
@@ -436,7 +452,7 @@ public class RobotContainer {
1,
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
),
getDeadbandedDriverController().getRight(), 0.15
getDeadbandedDriverController().getRight(), 0.30
), m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
@@ -511,10 +527,10 @@ public class RobotContainer {
.onTrue (new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator))
.onFalse(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator));
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
// Auto Scoring
src/main/java/frc4388/robot/commands/GotoLastApril.java
+13 -4
View File
@@ -1,5 +1,7 @@
package frc4388.robot.commands;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import java.util.Optional;
import edu.wpi.first.math.geometry.Pose2d;
@@ -74,6 +76,10 @@ public class GotoLastApril extends Command {
double yerr;
double roterr;
double xoutput;
double youtput;
double rotoutput;
@Override
public void execute() {
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
@@ -100,9 +106,9 @@ public class GotoLastApril extends Command {
// SmartDashboard.putNumber("PID X Error", xerr);
// SmartDashboard.putNumber("PID Y Error", yerr);
double xoutput = xPID.update(xerr);
double youtput = yPID.update(yerr);
double rotoutput = rotPID.update(roterr);
xoutput = xPID.update(xerr);
youtput = yPID.update(yerr);
rotoutput = rotPID.update(roterr);
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
@@ -133,7 +139,10 @@ public class GotoLastApril extends Command {
@Override
public final boolean isFinished() {
boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
boolean finished = (
(Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE || Math.abs(rotoutput) <= AutoConstants.MIN_ROT_PID_OUTPUT));
// System.out.println(finished);
if(finished)
src/main/java/frc4388/robot/subsystems/Elevator.java
+8 -5
View File
@@ -49,6 +49,7 @@ public class Elevator extends Subsystem {
private DigitalInput basinBeamBreak;
private DigitalInput endeffectorLimitSwitch;
// private DigitalInput intakeIR;
public enum CoordinationState {
Waiting, // for coral into the though
@@ -69,12 +70,14 @@ public class Elevator extends Subsystem {
private CoordinationState currentState;
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
// public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
elevatorMotor = elevatorTalonFX;
endeffectorMotor = endeffectorTalonFX;
this.led = led;
this.basinBeamBreak = basinLimitSwitch;
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
// this.intakeIR = intakeDigitalInput;
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
@@ -129,14 +132,14 @@ public class Elevator extends Subsystem {
}
case Hovering: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.READY_PATTERN);
break;
}
case L2Score: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
break;
@@ -240,8 +243,8 @@ public class Elevator extends Subsystem {
private void periodicWaiting() {
if (!basinBeamBreak.get())
transitionState(CoordinationState.Ready);
if(!endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering);
// if(!endeffectorLimitSwitch.get())
// transitionState(CoordinationState.Hovering);
}
// private void periodicWaitingTripped() {
@@ -253,7 +256,7 @@ public class Elevator extends Subsystem {
if (elevatorAtReference() && !endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering);
if(elevatorAtReference() && endeffectorLimitSwitch.get())
transitionState(CoordinationState.Waiting);
transitionState(CoordinationState.Hovering);
}
private void periodicScoring() {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -1
View File
@@ -326,7 +326,7 @@ public class SwerveDrive extends Subsystem {
rotTarget += deltaAngle;
}
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
}
// if(e.isPresent())
@@ -392,6 +392,11 @@ public class SwerveDrive extends Subsystem {
setToSlow();
}
public void startTurboPeriod() {
tmp_gear_index = gear_index;
setToTurbo();
}
public void endSlowPeriod() {
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
gear_index = tmp_gear_index;
src/main/java/frc4388/robot/subsystems/Vision.java
+3 -2
View File
@@ -48,7 +48,7 @@ public class Vision extends Subsystem {
private boolean isTagDetected = false;
private boolean isTagProcessed = false;
private Pose2d lastVisionPose = new Pose2d();
public Pose2d lastVisionPose = new Pose2d();
private Pose2d lastPhysOdomPose = new Pose2d();
private Matrix<N3, N1> curStdDevs;
@@ -265,7 +265,8 @@ public class Vision extends Subsystem {
public Pose2d getPose2d() {
if(isTagDetected && isTagProcessed)
return lastVisionPose;
// return lastVisionPose;
return lastPhysOdomPose;
else
return lastPhysOdomPose;
}