From a7cbe7e64e96eab29b0f5b7801308a7e47695725 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 31 Jul 2025 09:51:31 -0700 Subject: [PATCH] Pnumatics working --- .../java/frc4388/robot/RobotContainer.java | 11 ++- src/main/java/frc4388/robot/RobotMap.java | 25 ++++--- .../robot/constants/BuildConstants.java | 12 +-- .../differential/DiffConstants.java | 8 +- .../subsystems/differential/DiffDrive.java | 73 +++++++++++-------- .../robot/subsystems/differential/DiffIO.java | 3 + .../subsystems/differential/DiffReal.java | 65 ++++++++++++----- 7 files changed, 127 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7bfce4a..7a0f4ba 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -53,7 +53,7 @@ public class RobotContainer { public final RobotMap m_robotMap = RobotMap.configureReal(); /* Subsystems */ - public final DiffDrive m_DiffDrive = new DiffDrive(m_robotMap.m_DiffDrive, m_robotMap.m_gyro); + public final DiffDrive m_DiffDrive = new DiffDrive(m_robotMap.m_DiffDrive); // 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.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); @@ -157,6 +157,15 @@ public class RobotContainer { m_DiffDrive.resetOdometry(); })); + new JoystickButton(m_driverXbox, XboxController.LEFT_BUMPER_BUTTON).onTrue(new InstantCommand(() -> { + m_DiffDrive.shiftUp(); + })); + + + new JoystickButton(m_driverXbox, XboxController.RIGHT_BUMPER_BUTTON).onTrue(new InstantCommand(() -> { + m_DiffDrive.shiftDown(); + })); + // ? /* Programer Buttons (Controller 3)*/ // * /* Auto Recording */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index e5c2cbc..f9c54b2 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,9 +7,12 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +// import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.PneumaticHub; +import edu.wpi.first.wpilibj.PneumaticsControlModule; import frc4388.robot.subsystems.differential.DiffConstants; import frc4388.robot.subsystems.differential.DiffIO; import frc4388.robot.subsystems.differential.DiffReal; @@ -27,23 +30,25 @@ public class RobotMap { // public RobotGyro gyro = new RobotGyro(m_pigeon2); public DiffIO m_DiffDrive; - public GyroIO m_gyro; + // public GyroIO m_gyro; public static RobotMap configureReal() { RobotMap map = new RobotMap(); - TalonSRX m_leftFront = new TalonSRX(DiffConstants.IDs.FRONT_LEFT_MOTOR.id); - TalonSRX m_rightFront = new TalonSRX(DiffConstants.IDs.FRONT_RIGHT_MOTOR.id); - TalonSRX m_leftRear = new TalonSRX(DiffConstants.IDs.REAR_LEFT_MOTOR.id); - TalonSRX m_rightRear = new TalonSRX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id); + TalonFX m_leftFront = new TalonFX(DiffConstants.IDs.FRONT_LEFT_MOTOR.id); + TalonFX m_rightFront = new TalonFX(DiffConstants.IDs.FRONT_RIGHT_MOTOR.id); + TalonFX m_leftRear = new TalonFX(DiffConstants.IDs.REAR_LEFT_MOTOR.id); + TalonFX m_rightRear = new TalonFX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id); + PneumaticsControlModule m_Hub = new PneumaticsControlModule(); map.m_DiffDrive = new DiffReal( m_leftFront, m_rightFront, - m_leftRear, m_rightRear + m_leftRear, m_rightRear, + m_Hub ); - Pigeon2 pigeon2 = new Pigeon2(10); - map.m_gyro = new GyroPigeon2(pigeon2); + // Pigeon2 pigeon2 = new Pigeon2(10); + // map.m_gyro = new GyroPigeon2(pigeon2);% return map; } @@ -53,7 +58,7 @@ public class RobotMap { DiffSim sim = new DiffSim(); map.m_DiffDrive = sim; - map.m_gyro = new GyroSim(sim.getSimAngle()); + // map.m_gyro = new GyroSim(sim.getSimAngle()); return map; } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index a5def5d..ca1d2f2 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 = 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-18 12:15:43 MDT"; - public static final long BUILD_UNIX_TIME = 1752862543497L; + public static final int GIT_REVISION = 7; + public static final String GIT_SHA = "849125882e6f9540eb67d7a12f9abf3ec591e675"; + public static final String GIT_DATE = "2025-07-29 11:02:59 MDT"; + public static final String GIT_BRANCH = "diff-drive"; + public static final String BUILD_DATE = "2025-07-31 10:48:10 MDT"; + public static final long BUILD_UNIX_TIME = 1753980490846L; 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 fce855b..a8e683f 100644 --- a/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java @@ -19,10 +19,10 @@ public class DiffConstants { public static final Gains ROT_GAINS = new Gains(20, 0, 0); public static final class IDs { - public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 4); - public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 1); - public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 2); - public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 3); + public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 2); + public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 4); + public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 3); + public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 5); // public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 3); // public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 2); diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java index d3b096f..d757761 100644 --- a/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java @@ -21,31 +21,31 @@ import pabeles.concurrency.IntOperatorTask.Max; public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { DiffIO io; DiffStateAutoLogged state = new DiffStateAutoLogged(); - GyroIO gyroIO; - GyroStateAutoLogged gyroState = new GyroStateAutoLogged(); + // GyroIO gyroIO; + // GyroStateAutoLogged gyroState = new GyroStateAutoLogged(); DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DiffConstants.TRACK_WIDTH); DifferentialDrivePoseEstimator odometry = new DifferentialDrivePoseEstimator(kinematics, new Rotation2d(), 0, 0, new Pose2d()); - public DiffDrive(DiffIO io, GyroIO gyroIO) { + public DiffDrive(DiffIO io) { this.io = io; - this.gyroIO = gyroIO; + // this.gyroIO = gyroIO; } @Override public void periodic() { io.updateInputs(state); Logger.processInputs(getName(), state); - gyroIO.updateInputs(gyroState); - Logger.processInputs("gyro", gyroState); + // gyroIO.updateInputs(gyroState); + // Logger.processInputs("gyro", gyroState); var speeds = new DifferentialDriveWheelPositions( DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.leftOutputPosition), DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.rightOutputPosition) ); - odometry.update(gyroState.yaw, speeds); + // odometry.update(gyroState.yaw, speeds); } @Override @@ -58,34 +58,34 @@ public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { 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(magnatude == 0) { - io.driveWithInput(0, 0); - return; - }; + // @Override + // public void driveFieldRelative(Translation2d left, Translation2d right) { + // double magnatude = right.getNorm(); + // // In case the driver's stick is inside the deadband + // if(magnatude == 0) { + // io.driveWithInput(0, 0); + // return; + // }; - double targetAngle = right.getAngle().getRotations() + 0.25; - double curAngle = gyroState.yaw.getRotations(); + // double targetAngle = right.getAngle().getRotations() + 0.25; + // double curAngle = gyroState.yaw.getRotations(); - double error = targetAngle - signedFloor(curAngle); - if(error > 0.5) { - error -= 1; - }else if(error < -0.5) { - error += 1; - } - SmartDashboard.putNumber("error", error); + // 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.5 - Math.abs(error))*2 * magnatude; - double rot = Math.max(rotPid.update(error) * magnatude, magnatude); + // double move = (0.5 - Math.abs(error))*2 * magnatude; + // double rot = Math.max(rotPid.update(error) * magnatude, magnatude); - io.driveWithInput( - move + rot, - move - rot - ); - } + // io.driveWithInput( + // move + rot, + // move - rot + // ); + // } private double signedFloor(double x){ if(x > 0) { @@ -97,11 +97,20 @@ public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { @Override public void resetOdometry() { - gyroIO.reset(); + // gyroIO.reset(); odometry.resetPose(new Pose2d()); } + public void shiftUp() { + io.shiftUp(); + } + + public void shiftDown() { + io.shiftDown(); + } + + @AutoLogOutput public Pose2d estimatedPose() { diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffIO.java b/src/main/java/frc4388/robot/subsystems/differential/DiffIO.java index 13e1fef..bea1eca 100644 --- a/src/main/java/frc4388/robot/subsystems/differential/DiffIO.java +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffIO.java @@ -17,4 +17,7 @@ public interface DiffIO { public default void driveWithInput(double leftInput, double rightInput) {} public default void updateInputs(DiffState state) {} + + public default void shiftDown() {} + public default void shiftUp() {} } diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffReal.java b/src/main/java/frc4388/robot/subsystems/differential/DiffReal.java index 137eb33..775917b 100644 --- a/src/main/java/frc4388/robot/subsystems/differential/DiffReal.java +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffReal.java @@ -6,42 +6,73 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticHub; +import edu.wpi.first.wpilibj.PneumaticsControlModule; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; public class DiffReal implements DiffIO { - TalonSRX m_leftFront; - TalonSRX m_rightFront; - TalonSRX m_leftRear; - TalonSRX m_rightRear; + TalonFX m_leftFront; + TalonFX m_rightFront; + TalonFX m_leftRear; + TalonFX m_rightRear; + PneumaticsControlModule m_pneumaticHub; - public DiffReal(TalonSRX m_leftFront, TalonSRX m_rightFront, TalonSRX m_leftRear, TalonSRX m_rightRear) { + DoubleSolenoid m_Solenoid; + // DoubleSolenoid m_leftSolenoid; + + public DiffReal(TalonFX m_leftFront, TalonFX m_rightFront, TalonFX m_leftRear, TalonFX m_rightRear, PneumaticsControlModule pneumaticHub) { this.m_leftFront = m_leftFront; this.m_rightFront = m_rightFront; this.m_leftRear = m_leftRear; this.m_rightRear = m_rightRear; + this.m_pneumaticHub = pneumaticHub; + + this.m_Solenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 0, 1); + // this.m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 2, 3); - m_leftRear.follow(m_leftFront); - m_rightRear.follow(m_rightFront); + + m_leftRear.setControl(new Follower(m_leftFront.getDeviceID(), false)); + m_rightRear.setControl(new Follower(m_rightFront.getDeviceID(), false)); + // m_rightRear.follow(m_rightFront); - this.m_rightFront.setInverted(true); - this.m_rightRear.setInverted(true); + // this.m_rightFront.setInverted(true); + // this.m_rightRear.setInverted(true); } @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 / DiffConstants.ENCODER_TICKS_PER_ROTATION; - state.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION; + // state.leftCurrent = new double[] {m_leftFront.getStatorCurrent(), m_leftRear.getStatorCurrent()}; + // 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 / DiffConstants.ENCODER_TICKS_PER_ROTATION; - state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.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 / DiffConstants.ENCODER_TICKS_PER_ROTATION; + // state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION; } @Override public void driveWithInput(double leftInput, double rightInput) { - m_leftFront.set(TalonSRXControlMode.PercentOutput, leftInput); - m_rightFront.set(TalonSRXControlMode.PercentOutput, rightInput); + m_leftFront.set(leftInput); + m_rightFront.set(rightInput); + } + + @Override + public void shiftUp() { + m_Solenoid.set(Value.kForward); + // m_rightSolenoid.set(Value.kForward); + } + + @Override + public void shiftDown() { + m_Solenoid.set(Value.kReverse); + // m_rightSolenoid.set(Value.kReverse); } }