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

70
src/main/java/frc/robot/Constants.java
View File

@@ -33,7 +33,11 @@ public final class Constants {
private static ShuffleboardTab programmingTab = Shuffleboard.getTab("Programming");
<<<<<<< HEAD
public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
=======
public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag
public static final double MAX_SPEED = Units.feetToMeters(14.5);
@@ -63,6 +67,7 @@ public final class Constants {
}
public static class ShooterConstants {
<<<<<<< HEAD
private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter Velocity", -0.5)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
@@ -70,15 +75,44 @@ public final class Constants {
public static void getShooterVelocity() {
SHOOTER_VELOCITY = shooterVelocity.getDouble(-0.5);
=======
private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter RPM", -1000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double SHOOTER_RPM = -0.6;
public static double SHOOTER_POWER = -0.45;
public static void getShooterVelocity() {
SHOOTER_RPM = shooterVelocity.getDouble(-1000);
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}
public static final int CENTER_SHOOTER_MOTOR_ID = 42;
public static final int LEFT_SHOOTER_MOTOR_ID = 41;
public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
public static final int INDEXER_MOTOR_ID = 43;
<<<<<<< HEAD
public static final double INDEXER_MOTOR_SPEED = 0.6;
public static final int LEFT_ACTUATOR_ID = 44;
public static final int RIGHT_ACTUATOR_ID = 45;
=======
public static double INDEXER_AND_RAMP_MOTOR_RPM;
public static double SHOOTER_MOTOR_P = 0.001;
public static double SHOOTER_MOTOR_I = 0;
public static double SHOOTER_MOTOR_D = 0;
private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer Speed", 1000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
// this method called in robot periodic so values updated in elastic are
// constantly read and applied to RAMP_MOTOR_SPEED
public static void getIndexerAndRampMotorRPM() {
INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(1000);
}
public static final int LEFT_ACTUATOR_PWM_PORT = 1;
public static final int RIGHT_ACTUATOR_PWM_PORT = 3;
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
public static double DESIRED_POTENTIOMETER_DISTANCE;
@@ -86,6 +120,7 @@ public final class Constants {
}
public static class IntakeConstants {
<<<<<<< HEAD
private static GenericEntry intakeSpeed = programmingTab.add("Desired Intake Speed", -0.65)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
@@ -93,10 +128,19 @@ public final class Constants {
public static void getIntakeWheelsSpeed() {
INTAKE_WHEELS_MOTOR_SPEED = intakeSpeed.getDouble(-0.65);
=======
private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double INTAKE_WHEELS_MOTOR_RPM;
public static void getIntakeWheelsSpeed() {
INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}
public static final int INTAKE_WHEELS_MOTOR_ID = 50;
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
<<<<<<< HEAD
public static final double INTAKE_COLLECT_ENCODER_VALUE = 0;
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
@@ -104,12 +148,23 @@ public final class Constants {
public static final double INTAKE_ROTATOR_P = 0.01;
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0;
=======
public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744;
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
public static class IntakeRotatorPID {
public static final double INTAKE_ROTATOR_P = 0.05;
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0.001;
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}
}
public static class RampConstants {
public static final int RAMP_MOTOR_ID = 45;
<<<<<<< HEAD
public static double RAMP_MOTOR_SPEED;
// create object and a new widget under programming tab in Elastic where object
@@ -122,6 +177,13 @@ public final class Constants {
public static void getRampMotorSpeed() {
RAMP_MOTOR_SPEED = rampSpeed.getDouble(0.4);
}
=======
public static double RAMP_MOTOR_SPEED = .6;
// create object and a new widget under programming tab in Elastic where object
// retrieves value from widget
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}
public static class LimeLight {
@@ -151,11 +213,19 @@ public final class Constants {
public static class ClimberConstants {
public static final int CLIMB_MOTOR_ID = 60;
<<<<<<< HEAD
public static final int RATCHET_PWM_PORT = 1;
public static final double RATCHET_UNLOCK_ANGLE = 0.3;
public static final double RATCHET_LOCK_ANGLE = 0.15;
public static final double CLIMBER_SPEED = .1;
=======
public static final int RATCHET_PWM_PORT = 0;
public static final double RATCHET_UNLOCK_ANGLE = 0;
public static final double RATCHET_LOCK_ANGLE = 180;
public static final double CLIMBER_SPEED = .5;
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}
}

8
src/main/java/frc/robot/Robot.java
View File

@@ -89,11 +89,19 @@ public class Robot extends TimedRobot {
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
<<<<<<< HEAD
Constants.RampConstants.getRampMotorSpeed();
Constants.IntakeConstants.getIntakeWheelsSpeed();
Constants.ShooterConstants.getShooterVelocity();
Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
=======
// Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
Constants.IntakeConstants.getIntakeWheelsSpeed();
Constants.ShooterConstants.getShooterVelocity();
Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
//distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}
/**

131
src/main/java/frc/robot/RobotContainer.java
View File

@@ -36,6 +36,10 @@ import java.util.function.DoubleSupplier;
import frc.robot.subsystems.TargetingSubsystems;
import swervelib.SwerveInputStream;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
<<<<<<< HEAD
=======
import frc.robot.subsystems.ClimberSubsystem;
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
/**
* This class is where the bulk of the robot should be declared. Since
@@ -57,9 +61,18 @@ public class RobotContainer {
// SmartDashboard, allowing selection of desired auto
private final SendableChooser<Command> autoChooser;
<<<<<<< HEAD
private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
private final TargetingSubsystems m_TargetingSubsystems = new TargetingSubsystems();
private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
=======
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
/**
* Converts driver input into a field-relative ChassisSpeeds that is controlled
* by angular velocity.
@@ -142,6 +155,7 @@ public class RobotContainer {
}
<<<<<<< HEAD
/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
@@ -234,6 +248,116 @@ public class RobotContainer {
}
}
=======
/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
* an arbitrary predicate, or via the
* named factories in
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
* for
* {@link CommandXboxController
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
* Flight joysticks}.
*/
private void configureBindings() {
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngle);
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngleKeyboard);
driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand()));
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
// () -> -driverXbox.getLeftX()));
if (driverXbox.getRightY() < -0.4) {
m_ClimberSubsystem.liftRobotCommand();
} else if (driverXbox.getRightY() > 0.4) {
m_ClimberSubsystem.lowerRobotCommand();
} else {
m_ClimberSubsystem.stopClimberCommand();
}
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
if (Robot.isSimulation()) {
Pose2d target = new Pose2d(new Translation2d(1, 4),
Rotation2d.fromDegrees(90));
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
driveDirectAngleKeyboard.driveToPose(() -> target,
new ProfiledPIDController(5,
0,
0,
new Constraints(5, 2)),
new ProfiledPIDController(5,
0,
0,
new Constraints(Units.degreesToRadians(360),
Units.degreesToRadians(180))));
driverXbox.start()
.onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
// driverXbox.b().whileTrue(
// drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
// );
}
if (DriverStation.isTest()) {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above!
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
driverXbox.leftBumper().onTrue(Commands.none());
driverXbox.rightBumper().onTrue(Commands.none());
} else {
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.start().whileTrue(Commands.none());
driverXbox.back().whileTrue(Commands.none());
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.rightBumper().onTrue(Commands.none());
}
}
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
@@ -250,6 +374,7 @@ public class RobotContainer {
drivebase.setMotorBrake(brake);
}
<<<<<<< HEAD
public Command aimAtHopperCommand(DoubleSupplier xSup, DoubleSupplier ySup) {
try (PIDController aimPIDs = new PIDController(0.3, 0, 0.001)) {
aimPIDs.setTolerance(1.0);
@@ -284,4 +409,10 @@ public class RobotContainer {
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
m_shooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_shooterSubsystem.shootFuelCommand());
=======
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
}

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.