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

Update

Browse Source
This commit is contained in:
Zach Wilke
2025-10-30 17:19:46 -06:00
parent 56841636f9
commit f2946c11a9
12 changed files with 13 additions and 497 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+2 -1
View File
@@ -58,5 +58,6 @@
"edu.wpi.first.math.**.struct.*",
],
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable",
"wpilib.autoStartRioLog": false
"wpilib.autoStartRioLog": false,
"java.debug.settings.onBuildFailureProceed": true
}
src/main/deploy/pathplanner/navgrid.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/RobotContainer.java
-14
View File
@@ -37,12 +37,6 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.MoveUntilSuply;
import frc4388.robot.commands.alignment.DriveToReef;
import frc4388.robot.commands.alignment.DriveUntilLiDAR;
import frc4388.robot.commands.alignment.LidarAlign;
import frc4388.robot.commands.wait.waitElevatorRefrence;
import frc4388.robot.commands.wait.waitEndefectorRefrence;
import frc4388.robot.commands.wait.waitFeedCoral;
import frc4388.robot.commands.wait.waitSupplier;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.Constants.AutoConstants;
@@ -55,15 +49,12 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
// Subsystems
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.elevator.Elevator;
import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
// Utilites
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.compute.ReefPositionHelper.Side;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -80,14 +71,9 @@ public class RobotContainer {
/* Subsystems */
public final LED m_robotLED = new LED();
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
src/main/java/frc4388/robot/RobotMap.java
-24
View File
@@ -17,12 +17,9 @@ import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.constants.Constants.ElevatorConstants;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.robot.constants.Constants.SimConstants;
import frc4388.robot.constants.Constants.VisionConstants;
import frc4388.robot.subsystems.elevator.ElevatorIO;
import frc4388.robot.subsystems.elevator.ElevatorReal;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.lidar.LidarIO;
import frc4388.robot.subsystems.lidar.LidarReal;
@@ -59,9 +56,6 @@ public class RobotMap {
/* Swreve Drive Subsystem */
public final SwerveIO swerveDrivetrain;
/* Elevator Subsystem */
public final ElevatorIO elevatorIO;
public RobotMap(SimConstants.Mode mode) {
switch (mode) {
case REAL:
@@ -88,25 +82,8 @@ public class RobotMap {
swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
// Configure elevator
TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
// Fault
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
FaultTalonFX.addDevice(elevator, "Elevator");
FaultTalonFX.addDevice(endeffector, "Endeffector");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
@@ -130,7 +107,6 @@ public class RobotMap {
reefLidar = new LidarIO() {};
reverseLidar = new LidarIO() {};
swerveDrivetrain = new SwerveIO() {};
elevatorIO = new ElevatorIO() {};
break;
}
}
src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.autos;
package frc4388.robot.commands.Autos;
// package frc4388.robot.commands.Autos;
// import java.io.File;
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
+1 -1
View File
@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.swerve;
package frc4388.robot.commands.Swerve;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
src/main/java/frc4388/robot/commands/alignment/DriveToReef.java
-188
View File
@@ -1,188 +0,0 @@
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.ReefPositionHelper;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.compute.ReefPositionHelper.Side;
import frc4388.utility.structs.Gains;
public class DriveToReef extends Command {
// private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
// private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
// private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
SwerveDrive swerveDrive;
Vision vision;
double distance;
Side side;
boolean waitVelocityZero;
/**
* Command to drive robot to position.
* @param SwerveDrive m_robotSwerveDrive
*/
public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.distance = distance;
this.side = side;
this.waitVelocityZero = waitVelocityZero && false;
addRequirements(swerveDrive);
}
public static double tagRelativeXError = -1;
private static void setTagRelativeXError(double val){
tagRelativeXError = val;
}
@Override
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(
this.vision.getPose2d(),
side,
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
);
}
double xerr;
double yerr;
double roterr;
double xoutput;
double youtput;
double rotoutput;
@Override
public void execute() {
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
// xerr = targetpos.getX() - vision.getPose2d().getX();
// yerr = targetpos.getX() - vision.getPose2d().getY();
// roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
if(roterr > 180){
roterr -= 360;
}else if(roterr < -180){
roterr += 360;
}
// SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
// SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
// SmartDashboard.putNumber("Rotational PID error", roterr);
SmartDashboard.putNumber("PID X Error", xerr);
SmartDashboard.putNumber("PID Y Error", yerr);
SmartDashboard.putNumber("PID Rot Error", 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;
// rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
Math.max(Math.min(-xoutput, 1), -1)
// 0,0
);
// Translation2d rightStick = new Translation2d(
// Math.max(Math.min(rotoutput, 1), -1),
// 0
// );
setTagRelativeXError(
new Translation2d(xerr, yerr).getAngle()
.rotateBy(targetpos.getRotation())
.getCos());
swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
// swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
}
@Override
public final boolean isFinished() {
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) &&
(!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
);
// System.out.println(finished);
if(finished)
swerveDrive.softStop();
return finished;
// this statement is a boolean in and of itself
}
private class PID {
protected Gains gains;
private double output = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(Gains gains, double tolerance) {
this.gains = gains;
}
// Called when the command is initially scheduled.
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
public double update(double error) {
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
return output;
}
}
}
src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java
-48
View File
@@ -1,48 +0,0 @@
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class DriveUntilLiDAR extends Command {
private final SwerveDrive swerveDrive;
private final Translation2d leftStick;
private final Translation2d rightStick;
private final LiDAR m_lidar;
private final double mindistance;
public DriveUntilLiDAR(
SwerveDrive swerveDrive,
Translation2d leftStick,
Translation2d rightStick,
LiDAR lidar,
double mindistance) {
addRequirements(swerveDrive);
this.swerveDrive = swerveDrive;
this.leftStick = leftStick;
this.rightStick = rightStick;
this.m_lidar = lidar;
this.mindistance = mindistance;
}
@Override
public void initialize() {
}
@Override
public void execute() {
swerveDrive.driveFine(leftStick, rightStick, 0.3);
}
@Override
public boolean isFinished() {
if (Math.abs(m_lidar.getDistance()) < mindistance) {
swerveDrive.softStop();
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/alignment/LidarAlign.java
-107
View File
@@ -1,107 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class LidarAlign extends Command {
private SwerveDrive swerveDrive;
private LiDAR lidar;
private int currentFinderTick;
// private int tickFoundPipe;
private boolean foundReef;
private boolean headedRight;
private double speed;
private int bounces;
private double additionalDistance = 0;
// private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */
public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerveDrive = swerveDrive;
this.lidar = lidar;
addRequirements(swerveDrive, lidar);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.currentFinderTick = 0;
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = (DriveToReef.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (lidar.withinDistance()) {
swerveDrive.softStop();
foundReef = true;
return;
}
if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
headedRight = !headedRight;
currentFinderTick *= -1;
bounces++;
additionalDistance += 5;
if (bounces == 5) return;
}
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
SmartDashboard.putNumber("Rel Angle", relAngle);
SmartDashboard.putNumber("heading", currentHeading);
if (!headedRight) {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
} else {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
}
currentFinderTick++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (lidar.getDistance() < 4) {
swerveDrive.stopModules();
return true;
} else if (foundReef && lidar.withinDistance()) { // spot on
swerveDrive.stopModules();
return true;
} else if (foundReef && !lidar.withinDistance()) { // over shot
speed = speed / 2;
headedRight = !headedRight;
currentFinderTick = 0;
foundReef = false;
return false;
} else if (bounces >= 3) {
swerveDrive.stopModules();
return true;
} else {
return false;
}
}
}
src/main/java/frc4388/robot/constants/BuildConstants.java
+7 -7
View File
@@ -5,14 +5,14 @@ package frc4388.robot.constants;
*/
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2025RidgeScape";
public static final String MAVEN_NAME = "Robot-Essentials";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 173;
public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513";
public static final String GIT_DATE = "2025-07-17 09:15:19 MDT";
public static final String GIT_BRANCH = "advantagekit";
public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT";
public static final long BUILD_UNIX_TIME = 1752774931371L;
public static final int GIT_REVISION = 123;
public static final String GIT_SHA = "56841636f984f1a691134b4f1ae8862353846b54";
public static final String GIT_DATE = "2025-10-30 16:40:04 MDT";
public static final String GIT_BRANCH = "calibration-system";
public static final String BUILD_DATE = "2025-10-30 17:18:25 MDT";
public static final long BUILD_UNIX_TIME = 1761866305954L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/constants/Constants.java
+1
View File
@@ -69,6 +69,7 @@ public final class Constants {
public static final double MIN_ROT_PID_OUTPUT = 0.0;
public static final double VELOCITY_THRESHHOLD = 0.01;
public static final double STOP_VELOCITY = 2;
}
src/main/java/frc4388/utility/compute/ReefPositionHelper.java
-106
View File
@@ -1,106 +0,0 @@
package frc4388.utility.compute;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.FieldConstants;
public class ReefPositionHelper {
public enum Side {
LEFT,
RIGHT,
CENTER,
FAR_LEFT
}
public static final Pose2d[] RED_TAGS = {
FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
};
public static final Pose2d[] BLUE_TAGS = {
FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(22).get().toPose2d()
};
public static double distanceTo(Pose2d first, Pose2d second){
return Math.sqrt(Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2));
}
/*
* Function to loop through a list of tag locations to figure out closest one
*/
public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){
if(locations.length <= 0) return new Pose2d();
Pose2d minPos = locations[0];
double minDistance = distanceTo(position,minPos);
for(int i = 1; i < locations.length; i++){
double dist = distanceTo(locations[i],position);
if(dist < minDistance){
minPos = locations[i];
minDistance = dist;
}
}
System.out.println(minPos.getRotation().getDegrees());
return minPos;
}
/*
* Function to find closest tag location based on side
*/
public static Pose2d getNearestTag(Pose2d position) {
if(TimesNegativeOne.isRed)
return getNearestTag(RED_TAGS, position);
else
return getNearestTag(BLUE_TAGS, position);
}
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
return offset(getNearestTag(position),
getSide(side) + xtrim,
ydistance);
}
public static double getSide(Side side){
switch(side) {
case LEFT:
return -(AutoConstants.X_SCORING_POSITION_OFFSET);
case FAR_LEFT:
return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
case RIGHT:
return (AutoConstants.X_SCORING_POSITION_OFFSET);
case CENTER:
return 0;
}
assert false;
return 0;
}
public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
Translation2d oldTranslation = oldPose.getTranslation();
double rot = oldPose.getRotation().getRadians();
return new Pose2d(new Translation2d(
oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
}
}