Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

second commit

Browse Source
This commit is contained in:
Mehooliu
2026-02-20 16:27:48 -05:00
parent 5adb6549b5 a50d67d7f5
commit aa50a664e5
19 changed files with 909 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
View File

@@ -3,6 +3,10 @@ package frc.robot.subsystems;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.Servo;
<<<<<<< HEAD
=======
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
@@ -37,8 +41,24 @@ public class ClimberSubsystem extends SubsystemBase{
public static void toggleRatchet(boolean toggle) {
if (toggle == true) {
<<<<<<< HEAD
climberRatchet.set(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
} else
climberRatchet.set(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
=======
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
} else
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
}
public Command toggleRatchetCommand(boolean toggle) {
return runOnce(() -> toggleRatchet(toggle));
}
@Override
public void periodic()
{
SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}
}

107
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -78,3 +79,109 @@ public class IntakeSubsystem extends SubsystemBase {
// Shuffleboard.getTab("Intake Rotator Motor").add("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
}
}
=======
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Constants;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.ControlType;
public class IntakeSubsystem extends SubsystemBase {
private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
MotorType.kBrushless);
private static SparkClosedLoopController intakeRotatorPIDController;
public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
private static SparkClosedLoopController intakeWheelsMotorPIDController;
public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
public IntakeSubsystem() {
intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
intakeWheelsMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
}
public void startIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity);
}
public void reverseIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity);
}
public void stopIntakeMotor() {
intakeWheelsMotor.set(0);
}
public Command startIntakeMotorCommand() {
return runOnce(() -> startIntakeMotor());
}
public Command reverseIntakeMotorCommand() {
return runOnce(() -> reverseIntakeMotor());
}
public Command stopIntakeMotorCommand() {
return runOnce(() -> stopIntakeMotor());
}
public void deployIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
ControlType.kPosition);
}
public Command deployintakeCommand() {
return runOnce(() -> deployIntake());
}
public void retractIntake() {
intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition);
}
public Command retractIntakeCommand() {
return runOnce(() -> retractIntake());
}
public void assistFuelIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
ControlType.kPosition);
}
public Command assistFuelIntakeCommand() {
return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
.andThen(new WaitCommand(2));
}
@Override
public void periodic() {
SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
}
}
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f

171
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.DoubleSolenoid;
@@ -119,3 +120,173 @@ public class ShooterSubsystem extends SubsystemBase {
currentPotentiometerPosition = leftPotentiometer.get();
}
}
=======
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import java.util.function.BooleanSupplier;
import com.ctre.phoenix6.controls.Follower;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkBase;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkBaseConfig;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.LimelightHelpers;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
public class ShooterSubsystem extends SubsystemBase {
private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
MotorType.kBrushless);
private static SparkClosedLoopController centerShooterMotorPIDController;
public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
private static SparkClosedLoopController leftShooterMotorPIDController;
public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
private static SparkClosedLoopController rightShooterMotorPIDController;
public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
private static SparkClosedLoopController indexerAndRampMotorPIDController;
public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
public ShooterSubsystem() {
centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController();
rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
}
//private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
// MotorType.kBrushless);
//private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
//MotorType.kBrushless);
//private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
//private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
private static double currentPotentiometerPosition; // might need second value for the right potentiometer
public void setShooterMotorsRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
}
public double getShooterMotorRPM() {
return leftShooterMotor.getEncoder().getVelocity();
}
public void setIndexerAndRampMotorRPM() {
// if (LimelightHelpers.getTX("limelight") < 1.5 &&
// LimelightHelpers.getTX("limelight") > -1.5) {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity);
// } else
// indexerMotor.set(0);
}
/* public Command shootFuelCommand() {
return run(() -> startShooterMotors())
.until(() -> {
return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY);
})
.andThen(() -> startIndexerMotor());
} */
public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
.andThen(() -> setIndexerAndRampMotorRPM());
};
public void stopShooters() {
centerShooterMotor.set(0);
leftShooterMotor.set(0);
rightShooterMotor.set(0);
indexerAndRampMotor.set(0);
}
public Command stopShooterCommand() {
return runOnce(() -> stopShooters());
}
public void moveActuator(double desiredPotentiometerPosition) {
if (desiredPotentiometerPosition > currentPotentiometerPosition) {
//TODO: Test for positive or negative power
//leftActuatorMotor.set(0.1);
//rightActuatorMotor.set(0.1);
} else {
//leftActuatorMotor.set(-0.1);
//rightActuatorMotor.set(-0.1);
}
}
public void stopActuator() {
//leftActuatorMotor.set(0);
//rightActuatorMotor.set(0);
}
public Command moveActuatorCommand(double desiredPotentiometerPosition) {
return run(() -> moveActuator(desiredPotentiometerPosition))
.until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
.andThen(() -> stopActuator());
}
@Override
public void periodic() {
/* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
currentPotentiometerPosition = leftPotentiometer.get(); */
}
}
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f

61
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
View File

@@ -38,6 +38,10 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
<<<<<<< HEAD
=======
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -45,9 +49,13 @@ import frc.robot.RobotContainer;
import frc.robot.Constants;
public class TargetingSubsystems extends SubsystemBase {
<<<<<<< HEAD
RobotContainer m_RobotContainer = new RobotContainer();
=======
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera");
Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(
@@ -63,10 +71,17 @@ public class TargetingSubsystems extends SubsystemBase {
public List<Waypoint> rightClimbWaypoints;
<<<<<<< HEAD
public Command pathPlanToRightClimbPoseCommand() {
GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
currentRobotPose = m_RobotContainer.getSwerveDriveBase().getPose();
=======
public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) {
GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
currentRobotPose = swerveDrive.getPose();
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
rightClimbWaypoints = PathPlannerPath.waypointsFromPoses(
currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE);
@@ -74,12 +89,21 @@ public class TargetingSubsystems extends SubsystemBase {
goalEndState);
goToClimbPath.preventFlipping = true;
<<<<<<< HEAD
return m_RobotContainer.getSwerveDriveBase().getAutonomousCommand("goToClimbPath");
}
public Command aimAndRangeToPose(Pose2d desiredPose) {
return new RunCommand(() -> {
currentRobotPose = m_RobotContainer.getSwerveDriveBase().getPose();
=======
return swerveDrive.getAutonomousCommand("goToClimbPath");
}
public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
return new RunCommand(() -> {
currentRobotPose = swerveDrive.getPose();
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
@@ -98,11 +122,20 @@ public class TargetingSubsystems extends SubsystemBase {
double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
desiredPose.getRotation().getRadians());
<<<<<<< HEAD
m_RobotContainer.getSwerveDriveBase().drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
}, m_RobotContainer.getSwerveDriveBase());
}
Command photonAimAtClimb = new RunCommand(() -> {
=======
swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
}, swerveDrive);
}
Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
double rot = 0.0;
var result = photonVision.getLatestResult();
if (result.hasTargets()) {
@@ -112,9 +145,16 @@ public class TargetingSubsystems extends SubsystemBase {
rot = MathUtil.clamp(rot, -3.0, 3.0);
<<<<<<< HEAD
m_RobotContainer.getSwerveDriveBase().drive(new Translation2d(m_RobotContainer.getDriverXbox().getLeftY() * -1,
m_RobotContainer.getDriverXbox().getLeftX() * -1), rot, true);
}, m_RobotContainer.getSwerveDriveBase());
=======
swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
driverXbox.getLeftX() * -1), rot, true);
}, swerveDrive);
}
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
public PhotonPoseEstimator getPhotonPoseEstimator() {
@@ -150,6 +190,7 @@ public class TargetingSubsystems extends SubsystemBase {
return distanceFromLimelightToGoalInches;
}
<<<<<<< HEAD
@Override
public void periodic() {
@@ -160,6 +201,20 @@ public class TargetingSubsystems extends SubsystemBase {
m_RobotContainer.getSwerveDriveBase().getSwerveDrive()
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
}
=======
public void updateRobotPose(SwerveSubsystem swerveDrive) {
Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
if (result.isPresent()) {
EstimatedRobotPose estimatedPose = result.get();
swerveDrive.getSwerveDrive()
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
}
}
@Override
public void periodic() {
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
/*
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
* photonVision.getLatestResult().getBestTarget().getYaw());
@@ -174,5 +229,11 @@ public class TargetingSubsystems extends SubsystemBase {
* "Arducam_OV9281_USB_Camera",
* "http://photonvision.local:5800");
*/
<<<<<<< HEAD
}
}
=======
}
}
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f

4
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View File

@@ -62,7 +62,11 @@ public class SwerveSubsystem extends SubsystemBase {
/**
* Enable vision odometry updates while driving.
*/
<<<<<<< HEAD
private final boolean visionDriveTest = true;
=======
private final boolean visionDriveTest = false;
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
/**
* PhotonVision class to keep an accurate odometry.