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();
/* 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 */
src/main/java/frc4388/robot/RobotMap.java
+15 -10
View File
@@ -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;
}
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_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(){}
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 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);
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 {
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() {
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 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.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);
}
}