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

Changed code for robot

Browse Source
This commit is contained in:
Mehooliu
2026-03-02 16:25:15 -05:00
parent 69279812f3
commit 1b84701184
10 changed files with 407 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -82,37 +82,47 @@ public final class Constants {
public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
public static final int INDEXER_MOTOR_ID = 43;
public static double SHOOTER_MOTOR_P = 0.0018;
public static double SHOOTER_MOTOR_P = 0.001;
public static double SHOOTER_MOTOR_I = 0;
public static double SHOOTER_MOTOR_D = 0;
public static double SHOOTER_MOTOR_D = 0.001;
public static double SHOOTER_MOTOR_S = 0.2;
public static double SHOOTER_MOTOR_V = 0.0015;
public static double INDEXER_MOTOR_P = 0.0001;
public static double INDEXER_MOTOR_I = 0;
public static double INDEXER_MOTOR_D = 0;
/*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();*/
/*
* private static GenericEntry indexerAndRampRPM =
* programmingTab.add("Desired Ramp + Indexer RPM", 2000)
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
*/
public static double INDEXER_AND_RAMP_MOTOR_RPM = 10000;
// this method called in robot periodic so values updated in elastic are
// constantly read and applied to RAMP_MOTOR_SPEED
/*public static void updateIndexerAndRampMotorRPM() {
INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000);
}*/
/*
* public static void updateIndexerAndRampMotorRPM() {
* INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000);
* }
*/
}
public static class IntakeConstants {
/* private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry(); */
/*
* private static GenericEntry intakeRPM =
* programmingTab.add("Desired Intake RPM", -1000)
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
*/
public static double INTAKE_WHEELS_MOTOR_RPM = -7000;
/*public static void updateIntakeWheelsRPM() {
INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
}*/
/*
* public static void updateIntakeWheelsRPM() {
* INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
* }
*/
public static final int INTAKE_WHEELS_MOTOR_ID = 50;
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
@@ -122,61 +132,70 @@ public final class Constants {
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0;
}
public static final double INTAKE_MOTOR_P = 0.0001;
public static final double INTAKE_MOTOR_I = 0;
public static final double INTAKE_MOTOR_D = 0;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 5;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5;
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 0;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3;
public static final double INTAKE_RETRACT_ENCODER_VALUE = -5;
}
// create object and a new widget under programming tab in Elastic where object
// retrieves value from widget
// create object and a new widget under programming tab in Elastic where object
// retrieves value from widget
public static class TargetingConstants {
public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera");
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera");
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Back Left Camera");
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Back Right Camera");
public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera");
public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera");
public static final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d());
public static Rotation2d HUB_ROTATION_BLUE;
public static Rotation2d HUB_ROTATION_RED;
public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
public static final double HUB_X_POSE_BLUE = 4.625;
public static final double HUB_Y_POSE_BLUE = 4.03;
public static final double HUB_X_POSE_RED = 4.625;
public static final double HUB_Y_POSE_RED = 0;
public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d(
new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25), Units.inchesToMeters(13.1875)),
new Rotation3d(0, 0, Math.toRadians(-205)));
public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d(
new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)),
new Rotation3d(0, 0, Math.toRadians(200)));
public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d(
new Translation3d(Units.inchesToMeters(23.375), 0, Units.inchesToMeters(13)),
new Rotation3d(0, 0, 0));
public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
ORANGE_ROBOT_TO_CAM);
public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
BLACK_ROBOT_TO_CAM);
public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
RED_ROBOT_TO_CAM);
public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
PURPLE_ROBOT_TO_CAM);
public static final Transform3d ROBOT_TO_FRONT_RIGHT_CAM = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
}
public static class ClimberConstants {
public static final int CLIMB_MOTOR_ID = 60;
public static final int RATCHET_PWM_PORT = 9;
public static final int CLIMB_MOTOR_ID = 52;
public static final double RATCHET_UNLOCK_ANGLE = 0;
public static final double RATCHET_LOCK_ANGLE = 180;
public static final double CLIMBER_SPEED = .5;
public static final double CLIMBER_PID_P = 5;
public static final double CLIMBER_PID_I = 0;
public static final double CLIMBER_PID_D = 0;
public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 45;
public static final double CLIMBER_LOWERED_SETPOINT_VALUE = 0;
}
public static class LEDConstants {
public static final int LED_DIO_PORT = 0;
}
}

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

@@ -19,6 +19,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.Constants.IntakeConstants;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TargetingSubsystems;
/**
@@ -94,16 +96,8 @@ public class Robot extends TimedRobot {
//Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
//Constants.IntakeConstants.updateIntakeWheelsRPM();
Constants.ShooterConstants.updateShooterRPM();
TargetingSubsystems.getHubPoseTheta(m_robotContainer.getSwerveDrive());
//Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM,
Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM,
Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM,
Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM,
Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
}
@@ -132,6 +126,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousInit() {
IntakeSubsystem.resetIntakeRotationEncoder();
m_robotContainer.setMotorBrake(true);
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -142,6 +137,9 @@ public class Robot extends TimedRobot {
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**

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

@@ -40,6 +40,7 @@ import frc.robot.subsystems.TargetingSubsystems;
import swervelib.SwerveInputStream;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.swervedrive.Vision;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -64,8 +65,8 @@ public class RobotContainer {
private final SendableChooser<Command> autoChooser;
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
//private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
@@ -136,6 +137,10 @@ public class RobotContainer {
// Create the NamedCommands that will be used in PathPlanner
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
NamedCommands.registerCommand("Shoot_Fuel_Command", m_ShooterSubsystem.shootFuelCommand().andThen(m_IntakeSubsystem.startIntakeMotorCommand()).andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(2)));
NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployintakeCommand());
NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand());
NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand());
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
@@ -150,6 +155,8 @@ public class RobotContainer {
// Put the autoChooser on the SmartDashboard
SmartDashboard.putData("Auto Chooser", autoChooser);
}
/**
@@ -183,20 +190,15 @@ public class RobotContainer {
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()));
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
// .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
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.y().onTrue(m_ClimberSubsystem.lowerRobotCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand());
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.rightStick().onTrue(climbCommand());
//driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand()
@@ -289,18 +291,6 @@ public class RobotContainer {
return drivebase;
}
public Command climbCommand() {
if (driverXbox.getRightY() > -0.5) {
return m_ClimberSubsystem.lowerRobotCommand();
} else if (driverXbox.getRightX() < 0.5) {
return m_ClimberSubsystem.liftRobotCommand();
} else
return m_ClimberSubsystem.stopClimberCommand();
}
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_ShooterSubsystem.shootFuelCommand(),
m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
}

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

@@ -1,6 +1,12 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.ClosedLoopConfig;
import com.revrobotics.spark.config.SparkFlexConfig;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -8,48 +14,53 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class ClimberSubsystem extends SubsystemBase{
private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT);
public class ClimberSubsystem extends SubsystemBase {
// private static TalonFX climberMotor = new
// TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
private static SparkFlex climberMotor = new SparkFlex(Constants.ClimberConstants.CLIMB_MOTOR_ID,
MotorType.kBrushless);
private static SparkClosedLoopController climberMotorPIDController;
public static SparkFlexConfig climberMotorConfig = new SparkFlexConfig();
public void liftRobot() {
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED);
}
public void lowerRobot() {
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
}
public void stopClimber() {
climberMotor.set(0);
}
public Command liftRobotCommand() {
return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot());
}
public Command lowerRobotCommand() {
return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot());
}
public Command stopClimberCommand() {
return runOnce(() -> stopClimber());
}
public static void toggleRatchet(boolean toggle) {
if (toggle == true) {
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
} else
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
public ClimberSubsystem() {
climberMotorConfig.closedLoop.pid(Constants.ClimberConstants.CLIMBER_PID_P,
Constants.ClimberConstants.CLIMBER_PID_I,
Constants.ClimberConstants.CLIMBER_PID_D);
climberMotorConfig.smartCurrentLimit(80);
climberMotorConfig.openLoopRampRate(0);
climberMotorConfig.closedLoopRampRate(0);
climberMotor.configure(climberMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
climberMotorPIDController = climberMotor.getClosedLoopController();
}
public Command toggleRatchetCommand(boolean toggle) {
return runOnce(() -> toggleRatchet(toggle));
public void liftRobot() {
climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LIFTED_SETPOINT_VALUE, ControlType.kPosition);
}
public void lowerRobot() {
climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LOWERED_SETPOINT_VALUE, ControlType.kPosition);
}
public void stopClimber() {
climberMotor.set(0);
}
public Command liftRobotCommand() {
return runOnce(() -> liftRobot());
}
public Command lowerRobotCommand() {
return runOnce(() -> lowerRobot());
}
public Command stopClimberCommand() {
return runOnce(() -> stopClimber());
}
@Override
public void periodic()
{
SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getAngle());
public void periodic() {
SmartDashboard.putNumber("Climber Motor Encoder", climberMotor.getEncoder().getPosition());
}
}

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

@@ -36,11 +36,12 @@ public class IntakeSubsystem extends SubsystemBase {
.i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
.d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
.p(.06, ClosedLoopSlot.kSlot1)
.p(.13, ClosedLoopSlot.kSlot1)
.i(0, ClosedLoopSlot.kSlot1)
.d(0, ClosedLoopSlot.kSlot1);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorConfig.smartCurrentLimit(40);
intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
@@ -51,6 +52,11 @@ public class IntakeSubsystem extends SubsystemBase {
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
}
public static void resetIntakeRotationEncoder()
{
intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE);
}
public void startIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity);
}
@@ -93,12 +99,12 @@ public class IntakeSubsystem extends SubsystemBase {
public void assistFuelIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
ControlType.kPosition);
ControlType.kPosition, ClosedLoopSlot.kSlot1);
}
public Command assistFuelIntakeCommand() {
return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
.andThen(new WaitCommand(2));
return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(1.5)).andThen(deployintakeCommand())
.andThen(new WaitCommand(1.5));
}
@Override

117
src/main/java/frc/robot/subsystems/LEDSubsystem.java Normal file
View File

@@ -0,0 +1,117 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class LEDSubsystem extends SubsystemBase {
/** Creates a new LEDSubsystem. */
public LEDSubsystem() {
}
private double matchTime;
private boolean isHubActive;
public void setLEDPeriod() {
if (matchTime <= 140 && matchTime > 130 && !DriverStation.isAutonomous()) // transition
{
}
if (matchTime <= 130 && matchTime > 30 && !DriverStation.isAutonomous()) // shifts
{
}
if (matchTime <= 30 && !DriverStation.isAutonomous()) // endgame
{
}
}
public void setLEDHubActive() {
if (isHubActive) {
}
else {
}
}
public boolean isHubActive() {
Optional<Alliance> alliance = DriverStation.getAlliance();
// If we have no alliance, we cannot be enabled, therefore no hub.
if (alliance.isEmpty()) {
return false;
}
// Hub is always enabled in autonomous.
if (DriverStation.isAutonomousEnabled()) {
return true;
}
// At this point, if we're not teleop enabled, there is no hub.
if (!DriverStation.isTeleopEnabled()) {
return false;
}
// We're teleop enabled, compute.
double matchTime = DriverStation.getMatchTime();
String gameData = DriverStation.getGameSpecificMessage();
// If we have no game data, we cannot compute, assume hub is active, as its
// likely early in teleop.
if (gameData.isEmpty()) {
return true;
}
boolean redInactiveFirst = false;
switch (gameData.charAt(0)) {
case 'R' -> redInactiveFirst = true;
case 'B' -> redInactiveFirst = false;
default -> {
// If we have invalid game data, assume hub is active.
return true;
}
}
// Shift was is active for blue if red won auto, or red if blue won auto.
boolean shift1Active = switch (alliance.get()) {
case Red -> !redInactiveFirst;
case Blue -> redInactiveFirst;
};
if (matchTime > 130) {
// Transition shift, hub is active.
return true;
} else if (matchTime > 105) {
// Shift 1
return shift1Active;
} else if (matchTime > 80) {
// Shift 2
return !shift1Active;
} else if (matchTime > 55) {
// Shift 3
return shift1Active;
} else if (matchTime > 30) {
// Shift 4
return !shift1Active;
} else {
// End game, hub always active.
return true;
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run
matchTime = DriverStation.getMatchTime();
isHubActive = isHubActive();
}
}

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

@@ -11,11 +11,13 @@ import frc.robot.Constants;
import java.util.function.BooleanSupplier;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.swerve.utility.WheelForceCalculator.Feedforwards;
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.FeedbackSensor;
import com.revrobotics.spark.SparkBase;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
@@ -23,8 +25,8 @@ 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 edu.wpi.first.wpilibj2.command.WaitUntilCommand;
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,
@@ -52,28 +54,52 @@ public class ShooterSubsystem extends SubsystemBase {
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);
centerShooterMotorConfig
.voltageCompensation(12.0)
.closedLoop
.p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
.i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
.d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
.feedForward
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
centerShooterMotorConfig.smartCurrentLimit(80);
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);
leftShooterMotorConfig
.voltageCompensation(12.0)
.closedLoop
.p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
.i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
.d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
.feedForward
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
leftShooterMotorConfig.smartCurrentLimit(80);
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);
rightShooterMotorConfig
.voltageCompensation(12.0)
.closedLoop
.p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
.i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
.d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
.feedForward
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
rightShooterMotorConfig.smartCurrentLimit(80);
rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
indexerAndRampMotorConfig.closedLoop.pid(0.0001,
indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.INDEXER_MOTOR_P,
0,
0);
indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
@@ -81,78 +107,81 @@ public class ShooterSubsystem extends SubsystemBase {
indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
}
// private static SparkMax leftActuatorMotor = new
// SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
// MotorType.kBrushless);
//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
// private static SparkMax rightActuatorMotor = new
// SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
// MotorType.kBrushless);
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);
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
}
//test individual motor code
// test individual motor code
public void setLeftShooterMotorRPM() {
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
}
public Command testLeftShooterCommand() {
return runOnce(() -> setLeftShooterMotorRPM());
}
public void setRightShooterMotorRPM() {
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
}
public Command testRightShooterCommand() {
return runOnce(() -> setRightShooterMotorRPM());
}
public void setCenterShooterMotorRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
}
public Command testCenterShooterCommand() {
return runOnce(() -> setCenterShooterMotorRPM());
}
public void stopLeftShooterMotorRPM() {
leftShooterMotor.set(0);
leftShooterMotor.set(0);
}
public Command stopLeftShooterCommand() {
return runOnce(() -> stopLeftShooterMotorRPM());
}
public void stopCenterShooterMotorRPM() {
centerShooterMotor.set(0);
centerShooterMotor.set(0);
}
public Command stopCenterShooterCommand() {
return runOnce(() -> stopCenterShooterMotorRPM());
}
public void stopRightShooterMotorRPM() {
rightShooterMotor.set(0);
rightShooterMotor.set(0);
}
public Command stopRightShooterCommand() {
return runOnce(() -> stopRightShooterMotorRPM());
}
public double getShooterMotorRPM() {
return rightShooterMotor.getEncoder().getVelocity();
}
public void setIndexerAndRampMotorRPM() {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity);
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM,
ControlType.kVelocity);
}
public void reverseIndexerAndRampMotorRPM() {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity);
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1,
ControlType.kVelocity);
}
public Command reverseIndexerAndRampMotorRPMCommand() {
@@ -164,20 +193,30 @@ public class ShooterSubsystem extends SubsystemBase {
}
public Command stopIndexerAndRampMotorCommand() {
return runOnce(()-> indexerAndRampMotor.set(0));
return runOnce(() -> indexerAndRampMotor.set(0));
}
/*public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM())
.until(() -> {return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM * 0.9);})
.andThen(() -> setIndexerAndRampMotorRPM());
} */
.until(() -> {
return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM);
})
.andThen(() -> setIndexerAndRampMotorRPM());
}*/
public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
.andThen(() -> setIndexerAndRampMotorRPM());
};
//public Command shootFuelCommand() {
// return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
// .andThen(() -> setIndexerAndRampMotorRPM());
// };
public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM())
.andThen(new WaitUntilCommand(() -> {
return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM);
}))
.andThen(() -> setIndexerAndRampMotorRPM());
};
public void stopShooters() {
@@ -193,34 +232,10 @@ public class ShooterSubsystem extends SubsystemBase {
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("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity());
/* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
currentPotentiometerPosition = leftPotentiometer.get(); */
}
}

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

@@ -1,9 +1,11 @@
package frc.robot.subsystems;
import java.lang.StackWalker.Option;
import java.util.List;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.dyn4j.geometry.Rotation;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
@@ -35,6 +37,8 @@ import edu.wpi.first.networktables.NetworkTableInstance;
import frc.robot.Constants;
import frc.robot.LimelightHelpers;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -51,8 +55,13 @@ public class TargetingSubsystems extends SubsystemBase {
PIDController photonAimPIDController = new PIDController(3, 0, 0.001);
static Pose2d allianceHubPose;
public static Rotation2d hubThetaPose = new Rotation2d();
public static Optional<Alliance> alliance = DriverStation.getAlliance();
public TargetingSubsystems() {
photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI);
}
Pose2d currentRobotPose;
@@ -95,14 +104,17 @@ public class TargetingSubsystems extends SubsystemBase {
}, swerveDrive);
}
public Command aimAtHub(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
public Command aimAtHubPose(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
currentRobotPose = swerveDrive.getPose();
// Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose,
Constants.TargetingConstants.HUB_POSE);
allianceHubPose);
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
angleDifference.getRadians());
@@ -110,11 +122,11 @@ public class TargetingSubsystems extends SubsystemBase {
angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0);
swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, -driverXbox.getLeftY() * -1), angleSpeed,
false);
true);
}, swerveDrive);
}
Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
Command photonAimAtAprilTag(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
double rot = 0.0;
var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult();
@@ -130,65 +142,29 @@ public class TargetingSubsystems extends SubsystemBase {
}, swerveDrive);
}
public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) {
return poseEstimator;
public static void getHubPoseTheta(SwerveSubsystem swerveDrive)
{
if(alliance.isPresent()){
if (alliance.get() == Alliance.Blue){
hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX()));
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE, Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose);
}
else{
hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX()));
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED, Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose);
}
}
}
/*
* // static public NetworkTable table =
* //
* NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME
* );
* // static public NetworkTableEntry ty = table.getEntry("ty");
* // static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
*
* // how many degrees back is your limelight rotated from perfectly vertical?
* static double limelightMountAngleDegrees = 25.0;
*
* // distance from the center of the Limelight lens to the floor
* static double limelightLensHeightInches = 27.5;
*
* // distance from the target to the floor
* static double goalHeightInches = 44;
*
* static double angleToGoalDegrees = limelightMountAngleDegrees +
* Constants.LimeLight.LIMELIGHT_TY;
* static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
*
* // calculate distance
* static double distanceFromLimelightToGoalInches = (goalHeightInches -
* limelightLensHeightInches)
* / Math.tan(angleToGoalRadians);
*
* public static double getDistanceFromAprilTag() {
* angleToGoalDegrees = limelightMountAngleDegrees +
* Constants.LimeLight.LIMELIGHT_TY;
* angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
* distanceFromLimelightToGoalInches = (goalHeightInches -
* limelightLensHeightInches)
* / Math.tan(angleToGoalRadians);
* return distanceFromLimelightToGoalInches;
* }
*/
public static Optional<EstimatedRobotPose> visionEst = Optional.empty();
public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) {
/* for (var result : camera.getAllUnreadResults()) {
visionEst = poseEstimator.estimateCoprocMultiTagPose(result);
if (visionEst.isPresent()) {
EstimatedRobotPose estimatedPose = visionEst.get();
swerveDrive.getSwerveDrive()
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
}
}*/
}
@Override
public void periodic() {
SmartDashboard.putString("Target Hub Pose", allianceHubPose.getX() + "\n" + allianceHubPose.getY() + "\n" + allianceHubPose.getRotation()) ;
/*
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
* photonVision.getLatestResult().getBestTarget().getYaw());

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

@@ -32,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import frc.robot.Constants;
import frc.robot.subsystems.TargetingSubsystems;
import frc.robot.subsystems.swervedrive.Vision.Cameras;
import java.io.File;
import java.io.IOException;
@@ -146,7 +147,7 @@ public class SwerveSubsystem extends SubsystemBase {
// When vision is enabled we must manually update odometry in SwerveDrive
if (visionDriveTest) {
swerveDrive.updateOdometry();
//vision.updatePoseEstimation(swerveDrive);
vision.updatePoseEstimation(swerveDrive);
}
}
@@ -188,9 +189,9 @@ public class SwerveSubsystem extends SubsystemBase {
new PPHolonomicDriveController(
// PPHolonomicController is the built in path following controller for holonomic
// drive trains
new PIDConstants(5.0, 0.0, 0.0),
new PIDConstants(3.6, 0.0, 0.0),
// Translation PID constants
new PIDConstants(3.8, 0.0, 0.0)
new PIDConstants(3.675, 0.0, 0.00)
// Rotation PID constants
),
config,

39
src/main/java/frc/robot/subsystems/swervedrive/Vision.java
View File

@@ -39,6 +39,7 @@ import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import swervelib.SwerveDrive;
import swervelib.telemetry.SwerveDriveTelemetry;
import frc.robot.Constants;
/**
* Example PhotonVision class to aid in the pursuit of accurate odometry. Taken
@@ -306,33 +307,35 @@ public class Vision {
*/
enum Cameras {
/**
* Left Camera
* Back Left Camera
*/
LEFT_CAM("left",
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(10.981),
Units.inchesToMeters(8.44)),
BACK_LEFT_CAMERA("Rear Left Camera",
Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
/**
* Right Camera
* Back Right Camera
*/
RIGHT_CAM("right",
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(-10.981),
Units.inchesToMeters(8.44)),
BACK_RIGHT_CAM("Rear Right Camera",
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
/**
* Center Camera
* Front Left Camera
*/
CENTER_CAM("center",
new Rotation3d(0, Units.degreesToRadians(18), 0),
new Translation3d(Units.inchesToMeters(-4.628),
Units.inchesToMeters(-10.687),
Units.inchesToMeters(16.129)),
FRONT_LEFT_CAM("Front Left Camera",
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
/**
* Front Right Camera
*/
/*PURPLE_PHOTON_CAM("Front Right Camera",
Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/
/**
* Latency alert to use when high latency is detected.
*/