mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 00:38:02 -06:00
Transfer code
This commit is contained in:
Michael Mikovsky
@@ -15,7 +15,9 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||
|
||||
import edu.wpi.first.wpilibj.Alert;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.Alert.AlertType;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.robot.constants.BuildConstants;
|
||||
@@ -57,6 +59,8 @@ public class Robot extends LoggedRobot {
|
||||
// FaultReporter.startThread();
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* This function is called every robot packet, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
@@ -77,7 +81,6 @@ public class Robot extends LoggedRobot {
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode.
|
||||
* You can use it to reset any subsystem information you want to clear when
|
||||
@@ -125,6 +128,7 @@ public class Robot extends LoggedRobot {
|
||||
public void autonomousPeriodic() {
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.stop();
|
||||
@@ -137,10 +141,7 @@ public class Robot extends LoggedRobot {
|
||||
CommandScheduler.getInstance().cancel(m_autonomousCommand);
|
||||
m_autonomousCommand.cancel();
|
||||
m_autonomousCommand.end(true);
|
||||
System.out.println("NOT Null!!");
|
||||
|
||||
} else {
|
||||
System.out.println("Null!!");
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
|
||||
@@ -51,11 +51,12 @@ import frc4388.robot.constants.Constants.OIConstants;
|
||||
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
// Subsystems
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.Elevator.CoordinationState;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
// import frc4388.robot.subsystems.Endeffector;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
@@ -560,7 +561,7 @@ public class RobotContainer {
|
||||
NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive,
|
||||
new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
|
||||
// NamedCommands.registerCommand("feed-driveback", Commands.none());
|
||||
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
|
||||
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped()));
|
||||
|
||||
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
@@ -694,8 +695,8 @@ public class RobotContainer {
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;}))
|
||||
.onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;}));
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;}))
|
||||
.onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;}));
|
||||
|
||||
new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))
|
||||
@@ -890,6 +891,7 @@ public class RobotContainer {
|
||||
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
|
||||
}
|
||||
|
||||
public boolean autoChooserUpdated = false;
|
||||
public void makeAutoChooser() {
|
||||
autoChooser = new SendableChooser<String>();
|
||||
|
||||
@@ -913,6 +915,7 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
autoChooser.onChange((filename) -> {
|
||||
autoChooserUpdated = true;
|
||||
if (filename.equals("Taxi")) {
|
||||
autoCommand = new SequentialCommandGroup(
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
|
||||
@@ -131,7 +131,7 @@ public class DriveToReef extends Command {
|
||||
(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)
|
||||
(!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
|
||||
);
|
||||
// System.out.println(finished);
|
||||
|
||||
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2025RidgeScape";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 169;
|
||||
public static final String GIT_SHA = "aaef829ad2830d39cc9f9e05e61e973658d7784d";
|
||||
public static final String GIT_DATE = "2025-07-15 09:33:40 MDT";
|
||||
public static final int GIT_REVISION = 170;
|
||||
public static final String GIT_SHA = "2a38f94d5eef00a093f47df192f7c5c8a2b8cf8d";
|
||||
public static final String GIT_DATE = "2025-07-15 10:24:11 MDT";
|
||||
public static final String GIT_BRANCH = "advantagekit";
|
||||
public static final String BUILD_DATE = "2025-07-15 09:49:13 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752594553460L;
|
||||
public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752599069523L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
@@ -25,24 +27,29 @@ public class Elevator extends SubsystemBase implements Queryable {
|
||||
private TalonFX endeffectorMotor;
|
||||
private LED led;
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private long wait = 0;
|
||||
private long maxWait = 1000;
|
||||
// @AutoLog
|
||||
// private class ElevatorState {
|
||||
@SuppressWarnings("unused")
|
||||
public long wait = 0;
|
||||
public long maxWait = 1000;
|
||||
|
||||
private double elevatorRefrence = 0;
|
||||
private double endeffectorRefrence = 0;
|
||||
public double elevatorRefrence = 0;
|
||||
public double endeffectorRefrence = 0;
|
||||
|
||||
private boolean elevatorManualStop = true;
|
||||
private boolean endefectorManualStop = true;
|
||||
public boolean elevatorManualStop = true;
|
||||
public boolean endefectorManualStop = true;
|
||||
|
||||
private boolean disableAutoIntake = false;
|
||||
public boolean disableAutoIntake = false;
|
||||
|
||||
private boolean seededZeroEndefector = false;
|
||||
private boolean seededZeroElevator = false;
|
||||
public boolean seededZeroEndefector = false;
|
||||
public boolean seededZeroElevator = false;
|
||||
|
||||
private DigitalInput basinBeamBreak;
|
||||
private DigitalInput endeffectorLimitSwitch;
|
||||
private DigitalInput intakeIR;
|
||||
public DigitalInput basinBeamBreak;
|
||||
public DigitalInput endeffectorLimitSwitch;
|
||||
public DigitalInput intakeIR;
|
||||
// }
|
||||
|
||||
// private ElevatorState state = new ElevatorState();
|
||||
|
||||
public enum CoordinationState {
|
||||
Waiting, // for coral into the though
|
||||
|
||||
@@ -60,11 +60,6 @@ public class LED extends SubsystemBase implements Queryable {
|
||||
return "LEDs";
|
||||
}
|
||||
|
||||
// @Override
|
||||
// public void queryStatus() {
|
||||
// SmartDashboard.putString("LED status", mode.name());
|
||||
// }
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
@@ -51,10 +51,6 @@ public class Lidar extends SubsystemBase implements Queryable {
|
||||
return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
|
||||
}
|
||||
|
||||
ShuffleboardLayout subsystemLayout;
|
||||
GenericEntry sbDistance;
|
||||
GenericEntry sbWithinDistance;
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Lidar " + name;
|
||||
|
||||
@@ -6,6 +6,7 @@ package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
@@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.constants.DriveConstants;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.status.Status;
|
||||
@@ -40,27 +42,30 @@ import com.pathplanner.lib.config.RobotConfig;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
private Vision vision;
|
||||
|
||||
private int gear_index = DriveConstants.STARTING_GEAR;
|
||||
private boolean stopped = false;
|
||||
@SuppressWarnings("unused")
|
||||
private boolean robotKnowsWhereItIs = false;
|
||||
@AutoLog
|
||||
public class SwerveDriveState {
|
||||
public int gear_index = DriveConstants.STARTING_GEAR;
|
||||
public boolean stopped = false;
|
||||
public boolean robotKnowsWhereItIs = false;
|
||||
|
||||
public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
|
||||
// 25%
|
||||
|
||||
public double lastOdomSpeed;
|
||||
|
||||
public Pose2d initalPose2d = null;
|
||||
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
}
|
||||
|
||||
public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
|
||||
// 25%
|
||||
|
||||
public double lastOdomSpeed;
|
||||
|
||||
public Pose2d initalPose2d = null;
|
||||
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
public SwerveDriveState state = new SwerveDriveState();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
|
||||
@@ -81,7 +86,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// DoubleSupplier a = () -> 1.d;
|
||||
AutoBuilder.configure(
|
||||
() -> {
|
||||
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
|
||||
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d);
|
||||
}, // Robot pose supplier
|
||||
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
|
||||
// pose)
|
||||
@@ -138,7 +143,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
public void setOdoPose(Pose2d pose) {
|
||||
if (pose == null) return;
|
||||
initalPose2d = pose;
|
||||
state.initalPose2d = pose;
|
||||
swerveDriveTrain.resetPose(pose);
|
||||
}
|
||||
|
||||
@@ -157,7 +162,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// }
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
|
||||
@@ -165,7 +170,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
stopped = false;
|
||||
state.stopped = false;
|
||||
if (fieldRelative) {
|
||||
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
@@ -173,18 +178,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) {
|
||||
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
|
||||
state.rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
|
||||
// .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
} else {
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(state.rotTarget));
|
||||
ctrl.HeadingController.setPID(
|
||||
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
|
||||
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
|
||||
@@ -197,20 +202,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
} else { // Create robot-relative speeds.
|
||||
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(-leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(-leftStick.getY() * state.speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
|
||||
}
|
||||
}
|
||||
|
||||
public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
|
||||
stopped = false;
|
||||
state.stopped = false;
|
||||
// Create robot-relative speeds.
|
||||
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
|
||||
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
.withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
|
||||
|
||||
}
|
||||
|
||||
@@ -220,7 +225,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// relitive version of
|
||||
// this, and no pre
|
||||
// provided version
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve
|
||||
// drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
@@ -230,8 +235,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withTargetDirection(rightStick.getAngle()));
|
||||
}
|
||||
|
||||
@@ -239,8 +244,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
ctrl.HeadingController.setPID(
|
||||
DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
@@ -254,8 +259,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick = leftStick.rotateBy(heading);
|
||||
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
// ctrl.HeadingController.setPID(
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
@@ -298,6 +303,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
return false;
|
||||
}
|
||||
|
||||
public boolean isStopped() {
|
||||
return state.lastOdomSpeed < AutoConstants.STOP_VELOCITY;
|
||||
}
|
||||
|
||||
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
|
||||
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
|
||||
// swerve drive is still going:
|
||||
@@ -309,8 +318,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * -speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * -state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withTargetDirection(rot));
|
||||
// double
|
||||
}
|
||||
@@ -320,19 +329,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
|
||||
return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d);
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
swerveDriveTrain.tareEverything();
|
||||
robotKnowsWhereItIs = false;
|
||||
rotTarget = 0;
|
||||
state.robotKnowsWhereItIs = false;
|
||||
state.rotTarget = 0;
|
||||
// vision.resetRotations();
|
||||
}
|
||||
|
||||
|
||||
public void softStop() {
|
||||
stopped = true;
|
||||
state.stopped = true;
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
@@ -350,7 +359,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
SmartDashboard.putNumber("RotTartget", state.rotTarget);
|
||||
|
||||
double time = Vision.getTime();
|
||||
double freq = swerveDriveTrain.getOdometryFrequency();
|
||||
@@ -367,82 +376,80 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
|
||||
state.gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
|
||||
// robot start in?)
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
|
||||
if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
int i = state.gear_index - 1;
|
||||
if (i == -1)
|
||||
i = 0;
|
||||
setPercentOutput(DriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
state.gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
|
||||
if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
int i = state.gear_index + 1;
|
||||
if (i == DriveConstants.GEARS.length)
|
||||
i = DriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(DriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
state.gear_index = i;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
state.gear_index = -1;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
setPercentOutput(DriveConstants.SLOW_SPEED);
|
||||
gear_index = 0;
|
||||
state.gear_index = 0;
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
setPercentOutput(DriveConstants.FAST_SPEED);
|
||||
gear_index = 1;
|
||||
state.gear_index = 1;
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
setPercentOutput(DriveConstants.TURBO_SPEED);
|
||||
gear_index = 2;
|
||||
state.gear_index = 2;
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
|
||||
state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
|
||||
state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
private int tmp_gear_index = DriveConstants.STARTING_GEAR;
|
||||
|
||||
public void startSlowPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
tmp_gear_index = state.gear_index;
|
||||
setToSlow();
|
||||
}
|
||||
|
||||
public void startTurboPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
tmp_gear_index = state.gear_index;
|
||||
setToTurbo();
|
||||
}
|
||||
|
||||
public void endSlowPeriod() {
|
||||
setPercentOutput(DriveConstants.GEARS[tmp_gear_index]);
|
||||
gear_index = tmp_gear_index;
|
||||
state.gear_index = tmp_gear_index;
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
|
||||
if(curPose.isPresent() && lastPose.isPresent()){
|
||||
this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
|
||||
|
||||
SmartDashboard.putNumber("Speed", lastOdomSpeed);
|
||||
state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -453,28 +460,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
GenericEntry sbGyro = subsystemLayout
|
||||
.add("Gyro angle", 0)
|
||||
.withWidget(BuiltInWidgets.kGyro)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbShiftState = subsystemLayout
|
||||
.add("Shift State", 0)
|
||||
.withWidget(BuiltInWidgets.kNumberBar)
|
||||
.getEntry();
|
||||
|
||||
// @Override
|
||||
// public void queryStatus() {
|
||||
// sbGyro.setDouble(getGyroAngle());
|
||||
// sbShiftState.setDouble(this.speedAdjust);
|
||||
|
||||
// // TODO: Add more status things
|
||||
// }
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
@@ -10,6 +10,7 @@ import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
@@ -35,44 +36,22 @@ import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
public class Vision extends SubsystemBase implements Queryable {
|
||||
|
||||
// private PhotonCamera leftCamera;
|
||||
// private PhotonCamera rightCamera;
|
||||
|
||||
private PhotonCamera[] cameras;
|
||||
private PhotonPoseEstimator[] estimators;
|
||||
private List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
|
||||
private boolean isTagDetected = false;
|
||||
private boolean isTagProcessed = false;
|
||||
|
||||
private double lastLatency = 0;
|
||||
|
||||
public double getLastLatency() {
|
||||
return lastLatency;
|
||||
@AutoLog
|
||||
public class VisionState {
|
||||
public boolean isTagDetected = false;
|
||||
public boolean isTagProcessed = false;
|
||||
public List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
public double latency = 0;
|
||||
public Pose2d lastVisionPose = new Pose2d();
|
||||
public Pose2d lastPhysOdomPose = new Pose2d();
|
||||
}
|
||||
|
||||
public Pose2d lastVisionPose = new Pose2d();
|
||||
private Pose2d lastPhysOdomPose = new Pose2d();
|
||||
|
||||
private Matrix<N3, N1> curStdDevs;
|
||||
private VisionState state = new VisionState();
|
||||
|
||||
private Field2d field = new Field2d();
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
GenericEntry sbTagDetected = subsystemLayout
|
||||
.add("Tag Detected", false)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbTagProcessed = subsystemLayout
|
||||
.add("Tag Processed", false)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
|
||||
public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
|
||||
FaultReporter.register(this);
|
||||
SmartDashboard.putData(field);
|
||||
@@ -98,17 +77,17 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
|
||||
|
||||
private void update() {
|
||||
isTagProcessed = false;
|
||||
isTagDetected = false;
|
||||
state.isTagProcessed = false;
|
||||
state.isTagDetected = false;
|
||||
|
||||
// Instant now = Instant.now();
|
||||
|
||||
// int cams = 0;
|
||||
|
||||
// double latency = 0;
|
||||
double latency = 0;
|
||||
|
||||
// Pose2d pose = null;
|
||||
poses.clear();
|
||||
state.poses.clear();
|
||||
|
||||
for(int i = 0; i < cameras.length; i++){
|
||||
PhotonCamera camera = cameras[i];
|
||||
@@ -122,9 +101,9 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
|
||||
|
||||
var result = results.get(results.size()-1);
|
||||
// latency += result.getTimestampSeconds();
|
||||
latency += result.getTimestampSeconds();
|
||||
|
||||
isTagDetected = isTagDetected | result.hasTargets();
|
||||
state.isTagDetected = state.isTagDetected | result.hasTargets();
|
||||
|
||||
// If there are no tags
|
||||
if(!result.hasTargets())
|
||||
@@ -136,7 +115,7 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
if(estimatedRobotPose.isEmpty())
|
||||
continue;
|
||||
|
||||
poses.add(estimatedRobotPose.get());
|
||||
state.poses.add(estimatedRobotPose.get());
|
||||
|
||||
// if(pose == null)
|
||||
// pose = estimatedRobotPose.get().estimatedPose.toPose2d();
|
||||
@@ -150,7 +129,7 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
// Yaw += (pose.getRotation().getDegrees() + 180) % 360;
|
||||
// cams++;
|
||||
|
||||
isTagProcessed = true;
|
||||
state.isTagProcessed = true;
|
||||
|
||||
|
||||
}
|
||||
@@ -244,22 +223,13 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
// }
|
||||
// }
|
||||
|
||||
/**
|
||||
* Returns the latest standard deviations of the estimated pose from {@link
|
||||
* #getEstimatedGlobalPose()}, for use with {@link
|
||||
* edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
|
||||
* only be used when there are targets visible.
|
||||
*/
|
||||
public Matrix<N3, N1> getEstimationStdDevs() {
|
||||
return curStdDevs;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
public void setLastOdomPose(Optional<Pose2d> pose){
|
||||
if(pose.isPresent())
|
||||
lastPhysOdomPose = pose.get();
|
||||
state.lastPhysOdomPose = pose.get();
|
||||
}
|
||||
|
||||
// public double getLastOdomSpeed(){
|
||||
@@ -267,8 +237,8 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
// }
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
if(lastPhysOdomPose != null)
|
||||
return lastPhysOdomPose;
|
||||
if(state.lastPhysOdomPose != null)
|
||||
return state.lastPhysOdomPose;
|
||||
|
||||
// if(lastVisionPose != null)
|
||||
// return lastVisionPose;
|
||||
@@ -281,12 +251,12 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
public boolean isTag(){
|
||||
return isTagDetected && isTagProcessed;
|
||||
return state.isTagDetected && state.isTagProcessed;
|
||||
}
|
||||
|
||||
|
||||
public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
|
||||
for(EstimatedRobotPose pose : poses){
|
||||
for(EstimatedRobotPose pose : state.poses){
|
||||
drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
|
||||
}
|
||||
}
|
||||
@@ -303,19 +273,6 @@ public class Vision extends SubsystemBase implements Queryable {
|
||||
return "Vision";
|
||||
}
|
||||
|
||||
// GenericEntry sbShiftState = subsystemLayout
|
||||
// .add("Shift State", 0)
|
||||
// .withWidget(BuiltInWidgets.kNumberBar)
|
||||
// .getEntry();
|
||||
|
||||
|
||||
// @Override
|
||||
// public void queryStatus() {
|
||||
// sbTagDetected.setBoolean(isTagDetected);
|
||||
// sbTagProcessed.setBoolean(isTagProcessed);
|
||||
// // field.setRobotPose(getPose2d());
|
||||
// }
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
return new Status();
|
||||
|
||||
Reference in New Issue
Block a user