astatin3/Vision-Minibot
1
0
Fork 0
mirror of https://github.com/Astatin3/Vision-Minibot.git synced 2026-06-08 16:18:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Pnumatics working

Browse Source
This commit is contained in:
Michael Mikovsky
2025-07-31 09:51:31 -07:00
parent 849125882e
commit a7cbe7e64e
7 changed files with 127 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+10 -1
View File
@@ -53,7 +53,7 @@ public class RobotContainer {
public final RobotMap m_robotMap = RobotMap.configureReal(); public final RobotMap m_robotMap = RobotMap.configureReal();
/* Subsystems */ /* 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 LED m_robotLED = new LED();
// public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); // 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); // 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(); 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)*/ // ? /* Programer Buttons (Controller 3)*/
// * /* Auto Recording */ // * /* Auto Recording */
src/main/java/frc4388/robot/RobotMap.java
+15 -10
View File
@@ -7,9 +7,12 @@
package frc4388.robot; 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.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.DiffConstants;
import frc4388.robot.subsystems.differential.DiffIO; import frc4388.robot.subsystems.differential.DiffIO;
import frc4388.robot.subsystems.differential.DiffReal; import frc4388.robot.subsystems.differential.DiffReal;
@@ -27,23 +30,25 @@ public class RobotMap {
// public RobotGyro gyro = new RobotGyro(m_pigeon2); // public RobotGyro gyro = new RobotGyro(m_pigeon2);
public DiffIO m_DiffDrive; public DiffIO m_DiffDrive;
public GyroIO m_gyro; // public GyroIO m_gyro;
public static RobotMap configureReal() { public static RobotMap configureReal() {
RobotMap map = new RobotMap(); RobotMap map = new RobotMap();
TalonSRX m_leftFront = new TalonSRX(DiffConstants.IDs.FRONT_LEFT_MOTOR.id); TalonFX m_leftFront = new TalonFX(DiffConstants.IDs.FRONT_LEFT_MOTOR.id);
TalonSRX m_rightFront = new TalonSRX(DiffConstants.IDs.FRONT_RIGHT_MOTOR.id); TalonFX m_rightFront = new TalonFX(DiffConstants.IDs.FRONT_RIGHT_MOTOR.id);
TalonSRX m_leftRear = new TalonSRX(DiffConstants.IDs.REAR_LEFT_MOTOR.id); TalonFX m_leftRear = new TalonFX(DiffConstants.IDs.REAR_LEFT_MOTOR.id);
TalonSRX m_rightRear = new TalonSRX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id); TalonFX m_rightRear = new TalonFX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id);
PneumaticsControlModule m_Hub = new PneumaticsControlModule();
map.m_DiffDrive = new DiffReal( map.m_DiffDrive = new DiffReal(
m_leftFront, m_rightFront, m_leftFront, m_rightFront,
m_leftRear, m_rightRear m_leftRear, m_rightRear,
m_Hub
); );
Pigeon2 pigeon2 = new Pigeon2(10); // Pigeon2 pigeon2 = new Pigeon2(10);
map.m_gyro = new GyroPigeon2(pigeon2); // map.m_gyro = new GyroPigeon2(pigeon2);%
return map; return map;
} }
@@ -53,7 +58,7 @@ public class RobotMap {
DiffSim sim = new DiffSim(); DiffSim sim = new DiffSim();
map.m_DiffDrive = sim; map.m_DiffDrive = sim;
map.m_gyro = new GyroSim(sim.getSimAngle()); // map.m_gyro = new GyroSim(sim.getSimAngle());
return map; return map;
} }
src/main/java/frc4388/robot/constants/BuildConstants.java
+6 -6
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = ""; public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "Vision-Minibot"; public static final String MAVEN_NAME = "Vision-Minibot";
public static final String VERSION = "unspecified"; public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 6; public static final int GIT_REVISION = 7;
public static final String GIT_SHA = "0bb2a774d711afb09921bee415ca946821a45335"; public static final String GIT_SHA = "849125882e6f9540eb67d7a12f9abf3ec591e675";
public static final String GIT_DATE = "2025-07-16 15:09:08 MDT"; public static final String GIT_DATE = "2025-07-29 11:02:59 MDT";
public static final String GIT_BRANCH = "master"; public static final String GIT_BRANCH = "diff-drive";
public static final String BUILD_DATE = "2025-07-18 12:15:43 MDT"; public static final String BUILD_DATE = "2025-07-31 10:48:10 MDT";
public static final long BUILD_UNIX_TIME = 1752862543497L; public static final long BUILD_UNIX_TIME = 1753980490846L;
public static final int DIRTY = 1; public static final int DIRTY = 1;
private BuildConstants(){} private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java
+4 -4
View File
@@ -19,10 +19,10 @@ public class DiffConstants {
public static final Gains ROT_GAINS = new Gains(20, 0, 0); public static final Gains ROT_GAINS = new Gains(20, 0, 0);
public static final class IDs { public static final class IDs {
public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 4); 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", 1); 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", 2); 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", 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_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 3);
// public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 2); // public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 2);
src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java
+41 -32
View File
@@ -21,31 +21,31 @@ import pabeles.concurrency.IntOperatorTask.Max;
public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { public class DiffDrive extends SubsystemBase implements Drivebase, Queryable {
DiffIO io; DiffIO io;
DiffStateAutoLogged state = new DiffStateAutoLogged(); DiffStateAutoLogged state = new DiffStateAutoLogged();
GyroIO gyroIO; // GyroIO gyroIO;
GyroStateAutoLogged gyroState = new GyroStateAutoLogged(); // GyroStateAutoLogged gyroState = new GyroStateAutoLogged();
DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DiffConstants.TRACK_WIDTH); DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DiffConstants.TRACK_WIDTH);
DifferentialDrivePoseEstimator odometry = new DifferentialDrivePoseEstimator(kinematics, new Rotation2d(), 0, 0, new Pose2d()); 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.io = io;
this.gyroIO = gyroIO; // this.gyroIO = gyroIO;
} }
@Override @Override
public void periodic() { public void periodic() {
io.updateInputs(state); io.updateInputs(state);
Logger.processInputs(getName(), state); Logger.processInputs(getName(), state);
gyroIO.updateInputs(gyroState); // gyroIO.updateInputs(gyroState);
Logger.processInputs("gyro", gyroState); // Logger.processInputs("gyro", gyroState);
var speeds = new DifferentialDriveWheelPositions( var speeds = new DifferentialDriveWheelPositions(
DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.leftOutputPosition), DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.leftOutputPosition),
DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.rightOutputPosition) DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.rightOutputPosition)
); );
odometry.update(gyroState.yaw, speeds); // odometry.update(gyroState.yaw, speeds);
} }
@Override @Override
@@ -58,34 +58,34 @@ 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 // @Override
public void driveFieldRelative(Translation2d left, Translation2d right) { // public void driveFieldRelative(Translation2d left, Translation2d right) {
double magnatude = right.getNorm(); // double magnatude = right.getNorm();
// In case the driver's stick is inside the deadband // // In case the driver's stick is inside the deadband
if(magnatude == 0) { // if(magnatude == 0) {
io.driveWithInput(0, 0); // io.driveWithInput(0, 0);
return; // return;
}; // };
double targetAngle = right.getAngle().getRotations() + 0.25; // double targetAngle = right.getAngle().getRotations() + 0.25;
double curAngle = gyroState.yaw.getRotations(); // double curAngle = gyroState.yaw.getRotations();
double error = targetAngle - signedFloor(curAngle); // double error = targetAngle - signedFloor(curAngle);
if(error > 0.5) { // if(error > 0.5) {
error -= 1; // error -= 1;
}else if(error < -0.5) { // }else if(error < -0.5) {
error += 1; // error += 1;
} // }
SmartDashboard.putNumber("error", error); // SmartDashboard.putNumber("error", error);
double move = (0.5 - Math.abs(error))*2 * magnatude; // double move = (0.5 - Math.abs(error))*2 * magnatude;
double rot = Math.max(rotPid.update(error) * magnatude, magnatude); // double rot = Math.max(rotPid.update(error) * magnatude, magnatude);
io.driveWithInput( // io.driveWithInput(
move + rot, // move + rot,
move - rot // move - rot
); // );
} // }
private double signedFloor(double x){ private double signedFloor(double x){
if(x > 0) { if(x > 0) {
@@ -97,11 +97,20 @@ public class DiffDrive extends SubsystemBase implements Drivebase, Queryable {
@Override @Override
public void resetOdometry() { public void resetOdometry() {
gyroIO.reset(); // gyroIO.reset();
odometry.resetPose(new Pose2d()); odometry.resetPose(new Pose2d());
} }
public void shiftUp() {
io.shiftUp();
}
public void shiftDown() {
io.shiftDown();
}
@AutoLogOutput @AutoLogOutput
public Pose2d estimatedPose() { public Pose2d estimatedPose() {
src/main/java/frc4388/robot/subsystems/differential/DiffIO.java
+3
View File
@@ -17,4 +17,7 @@ public interface DiffIO {
public default void driveWithInput(double leftInput, double rightInput) {} public default void driveWithInput(double leftInput, double rightInput) {}
public default void updateInputs(DiffState state) {} public default void updateInputs(DiffState state) {}
public default void shiftDown() {}
public default void shiftUp() {}
} }
src/main/java/frc4388/robot/subsystems/differential/DiffReal.java
+48 -17
View File
@@ -6,42 +6,73 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; 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 { public class DiffReal implements DiffIO {
TalonSRX m_leftFront; TalonFX m_leftFront;
TalonSRX m_rightFront; TalonFX m_rightFront;
TalonSRX m_leftRear; TalonFX m_leftRear;
TalonSRX m_rightRear; 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_leftFront = m_leftFront;
this.m_rightFront = m_rightFront; this.m_rightFront = m_rightFront;
this.m_leftRear = m_leftRear; this.m_leftRear = m_leftRear;
this.m_rightRear = m_rightRear; 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_rightFront.setInverted(true);
this.m_rightRear.setInverted(true); // this.m_rightRear.setInverted(true);
} }
@Override @Override
public void updateInputs(DiffState state) { public void updateInputs(DiffState state) {
state.leftCurrent = new double[] {m_leftFront.getStatorCurrent(), m_leftRear.getStatorCurrent()}; // 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.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.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION;
state.rightCurrent = new double[] {m_rightFront.getStatorCurrent(), m_rightRear.getStatorCurrent()}; // 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.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.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION;
} }
@Override @Override
public void driveWithInput(double leftInput, double rightInput) { public void driveWithInput(double leftInput, double rightInput) {
m_leftFront.set(TalonSRXControlMode.PercentOutput, leftInput); m_leftFront.set(leftInput);
m_rightFront.set(TalonSRXControlMode.PercentOutput, rightInput); 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);
} }
} }