mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
updated to 2026
removed 2025
This commit is contained in:
C4llSqin
@@ -37,12 +37,12 @@ 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.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.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 +55,15 @@ 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.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;
|
||||
// import frc4388.utility.compute.ReefPositionHelper.Side;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
@@ -79,13 +79,13 @@ 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 Vision m_vision = new Vision();
|
||||
// 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");
|
||||
// public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
|
||||
// public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
|
||||
|
||||
|
||||
/* Controllers */
|
||||
|
||||
@@ -17,12 +17,12 @@ 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.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.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;
|
||||
@@ -44,13 +44,13 @@ public class RobotMap {
|
||||
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
|
||||
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
public final VisionIO leftCamera;
|
||||
public final VisionIO rightCamera;
|
||||
// public final VisionIO leftCamera;
|
||||
// public final VisionIO rightCamera;
|
||||
|
||||
// public final LiDAR lidar = new
|
||||
|
||||
public final LidarIO reefLidar;
|
||||
public final LidarIO reverseLidar;
|
||||
// public final LidarIO reefLidar;
|
||||
// public final LidarIO reverseLidar;
|
||||
|
||||
|
||||
/* LED Subsystem */
|
||||
@@ -59,25 +59,25 @@ public class RobotMap {
|
||||
/* Swreve Drive Subsystem */
|
||||
public final SwerveIO swerveDrivetrain;
|
||||
|
||||
/* Elevator Subsystem */
|
||||
public final ElevatorIO elevatorIO;
|
||||
// /* Elevator Subsystem */
|
||||
// public final ElevatorIO elevatorIO;
|
||||
|
||||
public RobotMap(SimConstants.Mode mode) {
|
||||
switch (mode) {
|
||||
case REAL:
|
||||
// Configure cameras
|
||||
PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
|
||||
PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
|
||||
// // Configure cameras
|
||||
// PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
|
||||
// PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
|
||||
|
||||
leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
|
||||
rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
|
||||
// leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
|
||||
// rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
|
||||
|
||||
FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
|
||||
FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
|
||||
// FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
|
||||
// FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
|
||||
|
||||
// Configure LiDAR
|
||||
reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
|
||||
reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
|
||||
// // Configure LiDAR
|
||||
// reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
|
||||
// reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
|
||||
|
||||
// Configure swerve drive train
|
||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
|
||||
@@ -90,23 +90,23 @@ public class RobotMap {
|
||||
|
||||
// Configure elevator
|
||||
|
||||
TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
|
||||
TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
|
||||
// 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);
|
||||
// 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);
|
||||
// elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
|
||||
|
||||
|
||||
|
||||
// Fault
|
||||
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
|
||||
|
||||
FaultTalonFX.addDevice(elevator, "Elevator");
|
||||
FaultTalonFX.addDevice(endeffector, "Endeffector");
|
||||
// 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");
|
||||
@@ -125,12 +125,12 @@ public class RobotMap {
|
||||
// case SIM:
|
||||
// break;
|
||||
default:
|
||||
leftCamera = new VisionIO() {};
|
||||
rightCamera = new VisionIO() {};
|
||||
reefLidar = new LidarIO() {};
|
||||
reverseLidar = new LidarIO() {};
|
||||
// leftCamera = new VisionIO() {};
|
||||
// rightCamera = new VisionIO() {};
|
||||
// reefLidar = new LidarIO() {};
|
||||
// reverseLidar = new LidarIO() {};
|
||||
swerveDrivetrain = new SwerveIO() {};
|
||||
elevatorIO = new ElevatorIO() {};
|
||||
// elevatorIO = new ElevatorIO() {};
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands.autos;
|
||||
package frc4388.robot.commands.Autos;
|
||||
// package frc4388.robot.commands.Autos;
|
||||
|
||||
// import java.io.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;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands.swerve;
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileInputStream;
|
||||
import java.util.ArrayList;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands.swerve;
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileOutputStream;
|
||||
import java.util.ArrayList;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
|
||||
this.currentFinderTick = 0;
|
||||
this.speed = 0.4; // TODO: find good speed for this
|
||||
this.foundReef = false;
|
||||
this.headedRight = (DriveToReef.tagRelativeXError < 0);
|
||||
//this.headedRight = (DriveToReef.tagRelativeXError < 0);
|
||||
this.additionalDistance = 0;
|
||||
this.bounces = 0;
|
||||
}
|
||||
|
||||
@@ -1,36 +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.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* 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 waitElevatorRefrence extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitElevatorRefrence(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// 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() {
|
||||
return elevator.elevatorAtReference();
|
||||
}
|
||||
}
|
||||
@@ -1,36 +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.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* 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 waitEndefectorRefrence extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitEndefectorRefrence(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// 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() {
|
||||
return elevator.endeffectorAtReference();
|
||||
}
|
||||
}
|
||||
@@ -1,36 +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.wait;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.elevator.Elevator;
|
||||
|
||||
/* 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 waitFeedCoral extends Command {
|
||||
/** Creates a new waitElevatorRefrence. */
|
||||
private Elevator elevator;
|
||||
public waitFeedCoral(Elevator elevator) {
|
||||
this.elevator = elevator;
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// 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() {
|
||||
return elevator.hasCoral();
|
||||
}
|
||||
}
|
||||
@@ -5,15 +5,15 @@ 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 = "2026KPopRobotHunters";
|
||||
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 DIRTY = 1;
|
||||
public static final int GIT_REVISION = -1;
|
||||
public static final String GIT_SHA = "UNKNOWN";
|
||||
public static final String GIT_DATE = "UNKNOWN";
|
||||
public static final String GIT_BRANCH = "UNKNOWN";
|
||||
public static final String BUILD_DATE = "2026-01-08 19:31:25 MST";
|
||||
public static final long BUILD_UNIX_TIME = 1767925885671L;
|
||||
public static final int DIRTY = 129;
|
||||
|
||||
private BuildConstants(){}
|
||||
}
|
||||
|
||||
@@ -69,6 +69,9 @@ 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 = 0.;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -20,8 +20,9 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
//import edu.wpi.first.units.measure.measure.Distance;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
import frc4388.utility.structs.Gains;
|
||||
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user