diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3704390..7bfce4a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,7 @@ package frc4388.robot; +import edu.wpi.first.math.geometry.Translation2d; // Drive Systems import edu.wpi.first.wpilibj.DriverStation; @@ -25,8 +26,10 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Autos import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.wait.waitSupplier; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.subsystems.differential.DiffDrive; @@ -89,8 +92,8 @@ public class RobotContainer { configureVirtualButtonBindings(); DeferredBlock.addBlock(() -> { // Called on first robot enable - // m_robotSwerveDrive.resetGyro(); - }, false); + m_DiffDrive.resetOdometry(); + }, true); DeferredBlock.addBlock(() -> { // Called on every robot enable TimesNegativeOne.update(); }, true); @@ -214,7 +217,14 @@ public class RobotContainer { // return autoCommand; // } catch (Exception e) { // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); - return autoCommand; + return new SequentialCommandGroup( + new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, -0.5), new Translation2d(), 2000), + new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, 0.5), new Translation2d(), 1000), + new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, -1), new Translation2d(), 500), + new MoveForTimeCommand(m_DiffDrive, new Translation2d(), new Translation2d(-1, 0), 250), + new MoveForTimeCommand(m_DiffDrive, new Translation2d(), new Translation2d(1, 0), 500), + new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, 0.5), new Translation2d(-0.5, 0), 500) + ); // } // return new PathPlannerAuto("Line-up-no-arm"); // zach told me to do the below comment diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9186e8d..e5c2cbc 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -12,9 +12,11 @@ import com.ctre.phoenix6.hardware.Pigeon2; import frc4388.robot.subsystems.differential.DiffConstants; import frc4388.robot.subsystems.differential.DiffIO; -import frc4388.robot.subsystems.differential.DiffTalonSRX; +import frc4388.robot.subsystems.differential.DiffReal; +import frc4388.robot.subsystems.differential.DiffSim; import frc4388.robot.subsystems.differential.GyroIO; import frc4388.robot.subsystems.differential.GyroPigeon2; +import frc4388.robot.subsystems.differential.GyroSim; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -35,7 +37,7 @@ public class RobotMap { TalonSRX m_leftRear = new TalonSRX(DiffConstants.IDs.REAR_LEFT_MOTOR.id); TalonSRX m_rightRear = new TalonSRX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id); - map.m_DiffDrive = new DiffTalonSRX( + map.m_DiffDrive = new DiffReal( m_leftFront, m_rightFront, m_leftRear, m_rightRear ); @@ -45,5 +47,15 @@ public class RobotMap { return map; } - + + public static RobotMap configureSim() { + RobotMap map = new RobotMap(); + + DiffSim sim = new DiffSim(); + map.m_DiffDrive = sim; + map.m_gyro = new GyroSim(sim.getSimAngle()); + + return map; + } + } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java index d3ad236..d7833cf 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -1,49 +1,47 @@ -// package frc4388.robot.commands; +package frc4388.robot.commands; -// import java.time.Instant; +import java.time.Instant; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc4388.robot.subsystems.SwerveDrive; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.differential.DiffDrive; +import frc4388.utility.structs.Drivebase; -// // Command to repeat a joystick movement for a specific time. -// public class MoveForTimeCommand extends Command { -// private final SwerveDrive swerveDrive; -// private final Translation2d leftStick; -// private final Translation2d rightStick; -// private final long duration; -// private final boolean robotRelative; +// Command to repeat a joystick movement for a specific time. +public class MoveForTimeCommand extends Command { + private final DiffDrive drivebase; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final long duration; -// private Instant startTime; + private Instant startTime; -// public MoveForTimeCommand( -// SwerveDrive swerveDrive, -// Translation2d leftStick, -// Translation2d rightStick, -// long millis, -// boolean robotRelative) { + public MoveForTimeCommand( + DiffDrive drivebase, + Translation2d leftStick, + Translation2d rightStick, + long millis) { -// addRequirements(swerveDrive); + addRequirements(drivebase); -// this.swerveDrive = swerveDrive; -// this.leftStick = leftStick; -// this.rightStick = rightStick; -// this.duration = millis; -// this.robotRelative = robotRelative; -// } + this.drivebase = drivebase; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.duration = millis; + } -// @Override -// public void initialize() { -// startTime = Instant.now(); -// } + @Override + public void initialize() { + startTime = Instant.now(); + } -// @Override -// public void execute() { -// swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); -// } + @Override + public void execute() { + drivebase.arcadeDrive(leftStick, rightStick); + } -// @Override -// public boolean isFinished() { -// return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration; -// } -// } \ No newline at end of file + @Override + public boolean isFinished() { + return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index e81a70e..a5def5d 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Vision-Minibot"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 5; - public static final String GIT_SHA = "a9e45fcf9ba356f63af7c567d51115d255bb269b"; - public static final String GIT_DATE = "2024-12-16 00:39:59 MST"; + public static final int GIT_REVISION = 6; + public static final String GIT_SHA = "0bb2a774d711afb09921bee415ca946821a45335"; + public static final String GIT_DATE = "2025-07-16 15:09:08 MDT"; public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2025-07-16 14:12:56 MDT"; - public static final long BUILD_UNIX_TIME = 1752696776235L; + public static final String BUILD_DATE = "2025-07-18 12:15:43 MDT"; + public static final long BUILD_UNIX_TIME = 1752862543497L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java b/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java index 74fcf3e..fce855b 100644 --- a/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java @@ -9,10 +9,13 @@ import frc4388.utility.structs.Gains; public class DiffConstants { - public static final Distance WHEEL_RADIUS_TO_ARC = Inches.of(2.5).times(Math.PI * 2); //meters + public static final Distance WHEEL_RADIUS = Inches.of(2.5); + public static final Distance WHEEL_RADIUS_TO_ARC = WHEEL_RADIUS.times(Math.PI * 2); //meters public static final Distance TRACK_DISPLACEMENT = Inches.of(6.5); //meters public static final Distance TRACK_WIDTH = TRACK_DISPLACEMENT.times(2); + public static final int ENCODER_TICKS_PER_ROTATION = 1100; + public static final Gains ROT_GAINS = new Gains(20, 0, 0); public static final class IDs { diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java index 76554dd..d3b096f 100644 --- a/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java @@ -11,10 +11,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.status.Queryable; import frc4388.utility.status.Status; import frc4388.utility.structs.Drivebase; +import pabeles.concurrency.IntOperatorTask.Max; public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { DiffIO io; @@ -39,8 +41,8 @@ public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { Logger.processInputs("gyro", gyroState); var speeds = new DifferentialDriveWheelPositions( - DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.leftOutputPosition), - DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.rightOutputPosition) + DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.leftOutputPosition), + DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.rightOutputPosition) ); odometry.update(gyroState.yaw, speeds); @@ -54,28 +56,30 @@ public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { ); } - private PID rotPid = new PID(DiffConstants.ROT_GAINS); + private PID rotPid = new PID(DiffConstants.ROT_GAINS); @Override public void driveFieldRelative(Translation2d left, Translation2d right) { + double magnatude = right.getNorm(); // In case the driver's stick is inside the deadband - if(right.getNorm() == 0) { + if(magnatude == 0) { io.driveWithInput(0, 0); return; }; - double targetAngle = right.getAngle().getRotations(); + double targetAngle = right.getAngle().getRotations() + 0.25; double curAngle = gyroState.yaw.getRotations(); - double error = targetAngle - curAngle - signedFloor(curAngle); + double error = targetAngle - signedFloor(curAngle); if(error > 0.5) { error -= 1; }else if(error < -0.5) { error += 1; } + SmartDashboard.putNumber("error", error); - double move = 0; - double rot = rotPid.update(targetAngle - curAngle); + double move = (0.5 - Math.abs(error))*2 * magnatude; + double rot = Math.max(rotPid.update(error) * magnatude, magnatude); io.driveWithInput( move + rot, diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffTalonSRX.java b/src/main/java/frc4388/robot/subsystems/differential/DiffReal.java similarity index 79% rename from src/main/java/frc4388/robot/subsystems/differential/DiffTalonSRX.java rename to src/main/java/frc4388/robot/subsystems/differential/DiffReal.java index ffc412d..137eb33 100644 --- a/src/main/java/frc4388/robot/subsystems/differential/DiffTalonSRX.java +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffReal.java @@ -7,13 +7,13 @@ import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -public class DiffTalonSRX implements DiffIO { +public class DiffReal implements DiffIO { TalonSRX m_leftFront; TalonSRX m_rightFront; TalonSRX m_leftRear; TalonSRX m_rightRear; - public DiffTalonSRX(TalonSRX m_leftFront, TalonSRX m_rightFront, TalonSRX m_leftRear, TalonSRX m_rightRear) { + public DiffReal(TalonSRX m_leftFront, TalonSRX m_rightFront, TalonSRX m_leftRear, TalonSRX m_rightRear) { this.m_leftFront = m_leftFront; this.m_rightFront = m_rightFront; this.m_leftRear = m_leftRear; @@ -31,12 +31,12 @@ public class DiffTalonSRX implements DiffIO { @Override public void updateInputs(DiffState state) { state.leftCurrent = new double[] {m_leftFront.getStatorCurrent(), m_leftRear.getStatorCurrent()}; - state.leftOutputPosition = (m_leftFront.getSelectedSensorPosition() + m_leftRear.getSelectedSensorPosition()) / 2 / 1100; - state.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / 1100; + state.leftOutputPosition = (m_leftFront.getSelectedSensorPosition() + m_leftRear.getSelectedSensorPosition()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION; + state.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION; state.rightCurrent = new double[] {m_rightFront.getStatorCurrent(), m_rightRear.getStatorCurrent()}; - state.rightOutputPosition = (m_rightFront.getSelectedSensorPosition() + m_rightFront.getSelectedSensorPosition()) / 2 / 1100; - state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / 1100; + state.rightOutputPosition = (m_rightFront.getSelectedSensorPosition() + m_rightFront.getSelectedSensorPosition()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION; + state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION; } @Override @@ -44,9 +44,4 @@ public class DiffTalonSRX implements DiffIO { m_leftFront.set(TalonSRXControlMode.PercentOutput, leftInput); m_rightFront.set(TalonSRXControlMode.PercentOutput, rightInput); } - - @AutoLogOutput - public double test() { - return m_leftFront.getSelectedSensorPosition(); - } } diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffSim.java b/src/main/java/frc4388/robot/subsystems/differential/DiffSim.java new file mode 100644 index 0000000..c804425 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffSim.java @@ -0,0 +1,79 @@ +package frc4388.robot.subsystems.differential; + +import static edu.wpi.first.units.Units.Meters; + +import java.time.Duration; +import java.time.Instant; +import java.util.function.Supplier; + +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix6.StatusSignal; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; + +public class DiffSim implements DiffIO { + private static final DCMotor MOTOR_SIM = new DCMotor( + 12.3, + 3, + -1.325, + -0.5, + -5.5, + 2); + + public DifferentialDrivetrainSim sim = new DifferentialDrivetrainSim(MOTOR_SIM, + 1 / (double) DiffConstants.ENCODER_TICKS_PER_ROTATION, + 1, + 1, + DiffConstants.WHEEL_RADIUS.in(Meters), + DiffConstants.TRACK_WIDTH.in(Meters), + null + ); + + private Supplier currentRot = new Supplier() { + @Override + public Rotation2d get() { + return sim.getHeading(); + } + }; + + private double clamp(double x, double min, double max) { + return Math.max(Math.min(x, max), min); + } + + @Override + public void driveWithInput(double leftInput, double rightInput) { + sim.setInputs( + clamp(leftInput, -1, 1) * 12, + clamp(rightInput, -1, 1) * 12 + ); + } + + Instant lastUpdate = Instant.now(); + + private double getTiemDelta() { + Instant now = Instant.now(); + double elapsedSeconds = ((double) Duration.between(lastUpdate, now).toMillis()) / 1000; + // System.out.println(elapsedSeconds);% + lastUpdate = now; + return elapsedSeconds; + } + + @Override + public void updateInputs(DiffState state) { + sim.update(getTiemDelta()); + + state.leftOutputPosition = sim.getLeftPositionMeters() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters); + state.leftOutputVelocity = sim.getLeftVelocityMetersPerSecond() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters); + state.leftCurrent = new double[]{sim.getLeftCurrentDrawAmps()}; + + state.rightOutputPosition = sim.getRightPositionMeters() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters); + state.rightOutputVelocity = sim.getRightVelocityMetersPerSecond() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters); + state.rightCurrent = new double[]{sim.getRightCurrentDrawAmps()}; + } + + public Supplier getSimAngle() { + return currentRot; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/differential/GyroSim.java b/src/main/java/frc4388/robot/subsystems/differential/GyroSim.java new file mode 100644 index 0000000..5321e6f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/GyroSim.java @@ -0,0 +1,30 @@ +package frc4388.robot.subsystems.differential; + +import static edu.wpi.first.units.Units.Volts; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Voltage; + +public class GyroSim implements GyroIO { + private Supplier diffRot; + + public GyroSim(Supplier diffRot){ + this.diffRot = diffRot; + } + + private Rotation2d offsetRot = new Rotation2d(); + + @Override + public void updateInputs(GyroState inputs) { + inputs.connected = true; + inputs.voltage = Volts.of(12); + inputs.yaw = diffRot.get().rotateBy(offsetRot); + } + + @Override + public void reset() { + offsetRot = diffRot.get().unaryMinus(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java similarity index 91% rename from src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java rename to src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java index 3851050..3bf33d5 100644 --- a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java @@ -4,12 +4,12 @@ import edu.wpi.first.wpilibj.Counter; import frc4388.robot.constants.Constants.LiDARConstants; // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface -public class LidarLiteV2 implements LidarIO { +public class LidarReal implements LidarIO { private Counter LidarPWM; - public LidarLiteV2(int port) { + public LidarReal(int port) { LidarPWM = new Counter(port); LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java similarity index 97% rename from src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java rename to src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 888dc12..442d9ed 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -11,7 +11,7 @@ import org.photonvision.targeting.PhotonPipelineResult; import frc4388.robot.constants.Constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; -public class VisionPhotonvision implements VisionIO { +public class VisionReal implements VisionIO { // private PhotonCamera[] cameras; // private PhotonPoseEstimator[] estimators; @@ -21,7 +21,7 @@ public class VisionPhotonvision implements VisionIO { // public List poses = new ArrayList<>(); - public VisionPhotonvision(PhotonCamera camera, Transform3d position){ + public VisionReal(PhotonCamera camera, Transform3d position){ this.camera = camera; estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); @@ -34,7 +34,7 @@ public class VisionPhotonvision implements VisionIO { state.isTagProcessed = false; state.isTagDetected = false; - state.lastEstimatedPose = null; + // state.lastEstimatedPose = null; var results = camera.getAllUnreadResults();