FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' of github.com:FairportRobotics/Castor2026

This commit is contained in:
Team578 Driver Station 2026-04-09 20:48:11 -04:00
parent 647ad41a9f ad65dc07ce
commit 833b3f2a7e
19 changed files with 1016 additions and 388 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
simgui-ds.json
View File

@ -1,4 +1,9 @@
{
"Keyboard 0 Settings": {
"window": {
"visible": true
}
},
"keyboardJoysticks": [
{
"axisConfig": [
@ -7,17 +12,20 @@
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
"decKey": 83,
"incKey": 87
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"decayRate": 0.009999999776482582,
"keyRate": 0.009999999776482582
},
{},
{
"decKey": 81,
"incKey": 69
}
],
"axisCount": 3,
"axisCount": 5,
"buttonCount": 4,
"buttonKeys": [
90,
@ -91,7 +99,7 @@
],
"robotJoysticks": [
{
"guid": "030000004c050000cc09000011810000"
"guid": "Keyboard0"
}
]
}

19
src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto
View File

@ -9,6 +9,25 @@
"data": {
"pathName": "S. Hub. shoot 8. E-Hub"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 10.0
}
},
{
"type": "named",
"data": {
"name": "ChassisAlignShoot"
}
}
]
}
}
]
}

8
src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path
View File

@ -32,12 +32,12 @@
},
{
"anchor": {
"x": 3.473,
"y": 4.013
"x": 2.9483700538099855,
"y": 4.00221686746988
},
"prevControl": {
"x": 3.1233132530120478,
"y": 4.034855421686746
"x": 2.5986833068220334,
"y": 4.024072289156626
},
"nextControl": null,
"isLocked": false,

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

@ -1,124 +1,167 @@
// 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;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import com.ctre.phoenix6.signals.InvertedValue;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.units.measure.Angle;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean constants. This class should not be used for any other
* purpose. All constants should be declared globally (i.e. public static). Do
* not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
* <p>
* It is advised to statically import this class (or one of its inner classes)
* wherever the constants are needed, to reduce verbosity.
*/
public final class Constants {
public static class OperatorConstants {
public static final int kDriverControllerPort = 0;
}
public static class CameraConstanst {
public static class OperatorConstants {
public static class IDFilters {
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 };
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 };
public static final int kDriverControllerPort = 0;
}
public static final String BACK_CAMERA_NAME = "limelight-back";
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
public static class CameraConstanst {
public static final String FRONT_CAMERA_NAME = "limelight-front";
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
}
public static class IDFilters {
public static class AdvantageKitConstants{
public enum RobotType{
SIM,
REAL,
REPLAY
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
}
public static final String BACK_CAMERA_NAME = "limelight-back";
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
public static final String FRONT_CAMERA_NAME = "limelight-front";
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final double[] BLUE_ZONE_COOR = new double[]{1};
public static final double[] RED_ZONE_COOR = new double[]{1};
public static final double[] NEUTRAL_ZONE_COOR = new double[]{1};
}
public static final RobotType CURRENT_MODE = RobotType.REAL;
}
// Swerve offsets as of 3/2/26
public static class SwerveDriveOffsets {
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
}
/*
public static class AdvantageKitConstants {
public enum RobotType {
SIM,
REAL,
REPLAY
}
public static final RobotType CURRENT_MODE = RobotType.REAL;
}
// Swerve offsets as of 3/2/26
public static class SwerveDriveOffsets {
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
}
/*
* Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
* Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
* Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot
* Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot
* Laters numbers will be added for more subsystems
* Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
*/
public static class SwerveDriveIDs {
public static final int FRONT_LEFT_STEER_ID = 11;
public static final int FRONT_LEFT_DRIVE_ID = 12;
public static final int FRONT_LEFT_ENCODER_ID = 13;
public static final int FRONT_RIGHT_STEER_ID = 21;
public static final int FRONT_RIGHT_DRIVE_ID = 22;
public static final int FRONT_RIGHT_ENCODER_ID = 23;
public static final int BACK_LEFT_STEER_ID = 31;
public static final int BACK_LEFT_DRIVE_ID = 32;
public static final int BACK_LEFT_ENCODER_ID = 33;
public static final int BACK_RIGHT_STEER_ID = 41;
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
//Bird
public static class ExtraIDEntities {
public static final int PIGEON_ID = 1;
}
public static class IntakeConstants {
public static final int DEPLOY_MOTOR_ID = 17;
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
*/
public static class SwerveDriveIDs {
public static final int INTAKE_MOTOR_ID = 2;
public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
}
public static final int FRONT_LEFT_STEER_ID = 11;
public static final int FRONT_LEFT_DRIVE_ID = 12;
public static final int FRONT_LEFT_ENCODER_ID = 13;
public static final int FRONT_RIGHT_STEER_ID = 21;
public static final int FRONT_RIGHT_DRIVE_ID = 22;
public static final int FRONT_RIGHT_ENCODER_ID = 23;
public static final int BACK_LEFT_STEER_ID = 31;
public static final int BACK_LEFT_DRIVE_ID = 32;
public static final int BACK_LEFT_ENCODER_ID = 33;
public static final int BACK_RIGHT_STEER_ID = 41;
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
public static class ShooterConstants {
public static final int LAUNCHER_MOTOR_ID = 3;
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
public static class RobotChassisLimits {
// Max velocity in meters per second
public static final int TURRET_MOTOR_ID = 4;
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final int TURRET_LIMIT_CHANNEL = 9;
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
public static final double HOMING_SPEED = .35;
public static final double TURRET_GEAR_RATIO = 1;
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
// In meters per second squared
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
public static final int HOOD_SERVO_CHANNEL = 0;
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
public static final int HOOD_ENCODER_ID = 4;
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 300;
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
}
public static class HopperConstants {
public static final int KICKER_MOTOR_ID = 9;
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
// In radians per second
public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
// In radians per second squared
public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
}
public static final int SPINDEXER_MOTOR_ID = 8;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
}
//Bird
public static class ExtraIDEntities {
public static final int PIGEON_ID = 1;
}
public static class IntakeConstants {
public static final int DEPLOY_MOTOR_ID = 17;
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
public static final int INTAKE_MOTOR_ID = 2;
public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
}
public static class ShooterConstants {
public static final int LAUNCHER_MOTOR_ID = 3;
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
public static final int TURRET_MOTOR_ID = 4;
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final int TURRET_LIMIT_CHANNEL = 9;
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
public static final double HOMING_SPEED = .35;
public static final double TURRET_GEAR_RATIO = 6;
public static final int HOOD_SERVO_CHANNEL = 0;
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
public static final int HOOD_ENCODER_ID = 4;
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
}
public static class HopperConstants {
public static final int KICKER_MOTOR_ID = 9;
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
public static final int SPINDEXER_MOTOR_ID = 8;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
}
public static class FieldPoses {
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
}
}

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

@ -1,78 +1,101 @@
// 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;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.OperatorConstants;
import frc.robot.commands.AutoShootCommandChassisTurret;
import frc.robot.commands.ManualHopperCommand;
import frc.robot.commands.ManualIntake;
import frc.robot.commands.ManualShootCommand;
import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
import frc.robot.commands.RelayCommand;
import frc.robot.commands.SetDeflectorCommand;
import frc.robot.commands.ZoneCheck;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TurretSubsystem;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and trigger mappings) should be declared here.
* This class is where the bulk of the robot should be declared. Since
* Command-based is a "declarative" paradigm, very little robot logic should
* actually be handled in the {@link Robot} periodic methods (other than the
* scheduler calls). Instead, the structure of the robot (including subsystems,
* commands, and trigger mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
// Replace with CommandPS4Controller or CommandJoystick if needed
private final CommandXboxController m_driverController =
new CommandXboxController(OperatorConstants.kDriverControllerPort);
// The robot's subsystems and commands are defined here...
// Replace with CommandPS4Controller or CommandJoystick if needed
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
private final CommandXboxController m_driverController
= new CommandXboxController(OperatorConstants.kDriverControllerPort);
public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
public final TurretSubsystem turretSubsystem = new TurretSubsystem();
public final TurretSubsystem turretSubsystem = new TurretSubsystem();
public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
}
public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
/**
* 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}.
*/
/**
* The container for the robot. Contains subsystems, OI devices, and
* commands.
*/
public RobotContainer() {
private void configureBindings() {
turretSubsystem.startHoming();
m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
// turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
// turretSubsystem.homeTurret();
// Configure the trigger bindings
configureBindings();
m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
}
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return driveSubsystem.getAutoCommand();
}
}
/**
* 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() {
m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
//m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
// m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
m_driverController.rightBumper().onTrue(intakeSubsystem.intake());
m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
m_driverController.b().toggleOnTrue(new ZoneCheck());
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> {
turretSubsystem.homeTurret();
}, turretSubsystem), driveSubsystem.getAutoCommand());
}
}

25
src/main/java/frc/robot/Utils.java Normal file
View File

@ -0,0 +1,25 @@
package frc.robot;
public class Utils {
/**
* Calculates the needed angle of the turret for a distance from the HUB.
* @param distanceToTag
* @return number of rotations of the turret, robot relative
*/
public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
// Calculate turret Angle
double cameraDistanceToTarget = distanceToTag + .6;
double cameraToShootDistance = 0.26035;
// a^2 + b^2 = c^2
double shooterDistanceToTarget = Math
.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
double robotTurretRotations = (robotTurretAngle / Math.PI);
return robotTurretRotations;
}
}

144
src/main/java/frc/robot/commands/AnyTarget.java Normal file
View File

@ -0,0 +1,144 @@
package frc.robot.commands;
import java.util.Arrays;
import java.util.List;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class AnyTarget extends Command {
// For Chassis Control
private DriveSubsystem driveSubsystem;
private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem;
private PIDController cameraAutoCenterController;
private PIDController deadreckoningAutoCenterController;
private int[] tagFilters;
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Pose3d closestTag;
private final double chassisRotateSpeed = Math.PI * 0.1;
public AnyTarget(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) {
this.driveSubsystem = driveSubsystem;
this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem;
this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0);
cameraAutoCenterController.setTolerance(0.2); // Units degrees
cameraAutoCenterController.setSetpoint(0);
deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0);
deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations
deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI);
}
@Override
public void initialize() {
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
} else {
tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
}
});
turretSubsystem.setLauncher(6000);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
hopperSubsystem.spindexerOn();
cameraAutoCenterController.calculate(-100);
// Change to HUB tracking pipeline
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER);
LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
Pose3d botPose = driveSubsystem.getBotPose();
// Find closest target to us
List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList();
closestTag = botPose.nearest(tagPoses);
Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closestTag);
Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians());
}
@Override
public void execute() {
// Need to compute shoot speed and angle
Logger.recordOutput("AutoAlign-CenteringError", cameraAutoCenterController.getError());
// No april tag in camera view. Rotate towards our theoretical closest tag
if (!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)) {
Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
Pose3d botPose = driveSubsystem.getBotPose();
//driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
} else // We have an april tag, center it to the camera frame
{
Logger.recordOutput("AutoAlignState-Tracking", "Camera");
// [tx, ty, tz, pitch, yaw, roll]
double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
if (targetsPose.length >= 1) {
Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
// Calculate turret Angle
double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
turretSubsystem.setTurretMotorRotation(-turretAngle);
driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
}
}
// If robot is centered on HUB and shooter is up to speed. Run the kicker
if (turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()) {
Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
hopperSubsystem.feedKicker();
} else {
Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
turretSubsystem.setLauncher(0);
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
driveSubsystem.stopDrive();
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
}
}

30
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.TurretSubsystem;
@ -40,7 +41,7 @@ public class AutoShootCommandChassisTurret extends Command{
private int[] tagFilters;
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Pose3d closestTag;
@ -54,8 +55,8 @@ public class AutoShootCommandChassisTurret extends Command{
this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
cameraAutoCenterController = new PIDController(0.1, 0, 0.0);
cameraAutoCenterController.setTolerance(0.2); // Units is meters
cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0);
cameraAutoCenterController.setTolerance(0.2); // Units degrees
cameraAutoCenterController.setSetpoint(0);
@ -74,12 +75,13 @@ public class AutoShootCommandChassisTurret extends Command{
}
});
turretSubsystem.setLauncher(2500);
turretSubsystem.setLauncher(6000);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
hopperSubsystem.spindexerOn();
cameraAutoCenterController.calculate(-100);
// Change to HUB tracking pipeline
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER);
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER);
LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
Pose3d botPose = driveSubsystem.getBotPose();
@ -91,7 +93,7 @@ public class AutoShootCommandChassisTurret extends Command{
Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
deadreckoningAutoCenterController.setSetpoint(delta.getAngle().getRadians());
deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians());
}
@Override
@ -102,25 +104,33 @@ public class AutoShootCommandChassisTurret extends Command{
// No april tag in camera view. Rotate towards our theoretical closest tag
if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
Logger.recordOutput("AutoAlignState", "Look for tag");
Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
Pose3d botPose = driveSubsystem.getBotPose();
driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this
//driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
}
else // We have an april tag, center it to the camera frame
{
Logger.recordOutput("AutoAlignState", "Center on tag");
Logger.recordOutput("AutoAlignState-Tracking", "Camera");
// [tx, ty, tz, pitch, yaw, roll]
double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(targetsPose.length >= 1){
Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
// Calculate turret Angle
double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
turretSubsystem.setTurretMotorRotation(-turretAngle);
driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
}
}
// If robot is centered on HUB and shooter is up to speed. Run the kicker
if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){
Logger.recordOutput("AutoAlignState", "FIRE!");
Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
hopperSubsystem.feedKicker();
}else{
Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
}
}

130
src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java Normal file
View File

@ -0,0 +1,130 @@
package frc.robot.commands;
import java.util.Arrays;
import java.util.List;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathConstraints;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.Constants;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class AutoShootCommandChassisTurretPathPlanner extends Command {
// For Chassis Control
private DriveSubsystem driveSubsystem;
private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem;
private int[] tagFilters;
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Pose3d closestTag;
private Pose2d targetPPPose;
private Command pathFollowCommand;
public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem,
TurretSubsystem turretSubsystem) {
this.driveSubsystem = driveSubsystem;
this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem;
this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
}
@Override
public void initialize() {
DriverStation.getAlliance().ifPresent((aliance) -> {
Pose3d targetHub;
if (aliance == Alliance.Blue) {
tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
targetHub = Constants.FieldPoses.BLUE_HUB_POSE;
} else {
tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
targetHub = Constants.FieldPoses.RED_HUB_POSE;
}
Pose3d botPose = driveSubsystem.getBotPose();
// Find closest target to us
List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get())
.toList();
closestTag = botPose.nearest(tagPoses);
Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag);
Translation2d delta = botPose.getTranslation().toTranslation2d()
.minus(closestTag.getTranslation().toTranslation2d());
Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta);
// Add 180 because we want the back of the robot to face the HUB
// deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
// Translate back 1 meter from HUB
Translation2d newTrans = targetHub.getTranslation().toTranslation2d()
.plus(new Translation2d(1.5, delta.getAngle()));
targetPPPose = new Pose2d(newTrans, delta.getAngle());
Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose);
pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose,
new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY,
Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL,
Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY,
Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL));
CommandScheduler.getInstance().schedule(pathFollowCommand);
});
turretSubsystem.setLauncher(5500);
hopperSubsystem.spindexerOn();
}
@Override
public void execute() {
// If robot is centered on HUB and shooter is up to speed. Run the kicker
if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) {
Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
hopperSubsystem.feedKicker();
} else {
Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
CommandScheduler.getInstance().cancel(pathFollowCommand);
turretSubsystem.setLauncher(0);
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
driveSubsystem.stopDrive();
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME,
Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
}
}

88
src/main/java/frc/robot/commands/AutoTurretCommand.java Normal file
View File

@ -0,0 +1,88 @@
package frc.robot.commands;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class AutoTurretCommand extends Command {
private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736);
private TurretSubsystem turretSubsystem;
private DriveSubsystem driveSubsystem;
private int[] shootingTagFilters;
private Pose3d closestTag;
private Pose3d scoringHubPose;
public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
this.turretSubsystem = turretSubsystem;
this.driveSubsystem = driveSubsystem;
addRequirements(turretSubsystem); // Not adding drive as we don't wan't the drive command to be de-scheduled
}
@Override
public void initialize() {
// Perform auto-home
turretSubsystem.homeTurret();
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE;
} else {
shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
scoringHubPose = Constants.FieldPoses.RED_HUB_POSE;
}
Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
});
}
@Override
public void execute() {
Pose3d botPose = driveSubsystem.getBotPose();
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative()))));
Logger.recordOutput("AutoTurret-TurretPose", turretPose);
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurret-Translation", targetTranslation);
if(turretSubsystem.isTurretReady()){
// turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
}
}

19
src/main/java/frc/robot/commands/ManualIntake.java → src/main/java/frc/robot/commands/ExampleCommand.java
View File

@ -4,25 +4,21 @@
package frc.robot.commands;
import frc.robot.subsystems.IntakeSubsystem;
import edu.wpi.first.wpilibj.XboxController;
import frc.robot.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. */
public class ManualIntake extends Command {
public class ExampleCommand extends Command {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final IntakeSubsystem m_subsystem;
private final XboxController m_XboxController;
private final ExampleSubsystem m_subsystem;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) {
public ExampleCommand(ExampleSubsystem subsystem) {
m_subsystem = subsystem;
m_XboxController = xboxController;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
@ -33,12 +29,7 @@ public class ManualIntake extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_XboxController.getYButton()==true)
{
m_subsystem.extend();
}
}
public void execute() {}
// Called once the command ends or is interrupted.
@Override

58
src/main/java/frc/robot/commands/ManualHopperCommand.java
View File

@ -1,58 +0,0 @@
// 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.commands;
import frc.robot.subsystems.HopperSubsystem;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. */
public class ManualHopperCommand extends Command {
private final HopperSubsystem m_subsystem;
private final XboxController m_xboxController;
public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) {
m_subsystem = subsystem;
m_xboxController = xboxController;
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_subsystem.spindexerOn();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
{
m_subsystem.feedKicker();
}
else
{
m_subsystem.stopKicker();
}
if(m_xboxController.getRightBumper())
{
m_subsystem.spindexerOn();
}
if(m_xboxController.getLeftBumper())
{
m_subsystem.spindexerOff();
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

48
src/main/java/frc/robot/commands/ManualShootCommand.java
View File

@ -1,48 +0,0 @@
// 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.commands;
import frc.robot.subsystems.TurretSubsystem;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. */
public class ManualShootCommand extends Command {
private final TurretSubsystem m_subsystem;
private final XboxController m_controller;
public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) {
m_subsystem = subsystem;
m_controller = controller;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0)
{
m_subsystem.startRotate(-.5);
m_controller.setRumble(RumbleType.kRightRumble, 1);
}
else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0)
{
m_subsystem.startRotate(.5);
m_controller.setRumble(RumbleType.kLeftRumble, 1);
}
else
{
m_subsystem.startRotate(0);
m_controller.setRumble(RumbleType.kBothRumble, 0);
}
}
@Override
public boolean isFinished() {
return false;
}
}

63
src/main/java/frc/robot/commands/RelayCommand.java Normal file
View File

@ -0,0 +1,63 @@
package frc.robot.commands;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class RelayCommand extends Command{
private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem;
private IntakeSubsystem intakeSubsystem;
private Command waitCommand = Commands.waitSeconds(1.5);
public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){
this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem;
this.intakeSubsystem = intakeSubsystem;
addRequirements(hopperSubsystem, turretSubsystem, intakeSubsystem);
}
@Override
public void initialize() {
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
turretSubsystem.setLauncher(6000);
hopperSubsystem.spindexerOn();
CommandScheduler.getInstance().schedule(waitCommand);
turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY
}
@Override
public void execute() {
if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
hopperSubsystem.feedKicker();
}else{
hopperSubsystem.stopKicker();
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
waitCommand.cancel();
hopperSubsystem.spindexerOff();
hopperSubsystem.stopKicker();
turretSubsystem.setLauncher(0);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}

79
src/main/java/frc/robot/commands/ZoneCheck.java Normal file
View File

@ -0,0 +1,79 @@
package frc.robot.commands;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
public class ZoneCheck extends Command {
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Zone zone;
public enum Zone {
Blue,
Red,
Neutral;
}
public ZoneCheck() {
getBotPose();
}
@Override
public void initialize() {
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
switch (zone) {
case Blue:
System.out.println("Im Home");
case Neutral:
System.out.println("Uhhhhhhhh not red");
;
default:
System.out.println("BOOOOO, BOOOO RED");
;
}
} else {
switch (zone) {
case Blue:
System.out.println("Screw Blue");
;
case Neutral:
System.out.println("Uhhhhhhhh not red");
;
default:
System.out.println("Im Home");
}
}
});
}
@Override
public void execute() {
}
@Override
public boolean isFinished() {
return false;
}
private double[] getBotPose() {
return LimelightHelpers.getBotPose_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
}
private void getZone() {
if (getBotPose()[0] >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
zone = Zone.Red;
} else if (getBotPose()[0] >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
zone = Zone.Neutral;
} else {
zone = Zone.Blue;
}
}
}

25
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -10,6 +10,8 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
@ -50,8 +52,8 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem = swerveBuilder
.withCanbusName("Drive")
.withPigeonId(Constants.ExtraIDEntities.PIGEON_ID)
.withMaxLinearVelocity(3.0)
.withMaxAngularVelocity(Math.PI)
.withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY)
.withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY)
.withSwerveModule(
swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID)
@ -133,9 +135,8 @@ public class DriveSubsystem extends TestableSubsystem {
.build())
.build();
// Set vision measurement confidence values
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999));
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));
this.setDefaultCommand(Commands.run(new Runnable() {
@ -163,8 +164,8 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem::getRobotRelativeSpeeds,
(speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
new PPHolonomicDriveController(
new PIDConstants(5.0, 0, 0),
new PIDConstants(5.0, 0, 0)),
new PIDConstants(1.5, 0, 0),
new PIDConstants(Math.PI / 2, 0, 0)),
config,
() -> {
var alliance = DriverStation.getAlliance();
@ -193,12 +194,12 @@ public class DriveSubsystem extends TestableSubsystem {
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
if(frontCameraPose.tagCount >= 1){
if (frontCameraPose.tagCount >= 1) {
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
}
LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(backCameraPose.tagCount >= 1){
if (backCameraPose.tagCount >= 1) {
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
}
@ -210,15 +211,15 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem.simulationPeriodic();
}
public Command getAutoCommand(){
public Command getAutoCommand() {
return autoChooser.getSelected();
}
public void stopDrive(){
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
public void stopDrive() {
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
}
public void rotateChassis(double rotSpeed){
public void rotateChassis(double rotSpeed) {
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
}

58
src/main/java/frc/robot/subsystems/HopperSubsystem.java
View File

@ -6,36 +6,84 @@ package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem;
import org.fairportrobotics.frc.posty.test.PostTest;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.AngularVelocityUnit;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.AngularVelocity;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import frc.robot.Constants;
public class HopperSubsystem extends TestableSubsystem {
private TalonFX spindexerMotor;
private TalonFX kickerMotor;
private VelocityVoltage kickerControl = new VelocityVoltage(0);
private StatusSignal<AngularVelocity> kickerVelocity;
/** Creates a new HopperSubsystem. */
public HopperSubsystem() {
spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION));
spindexerMotor.getConfigurator().apply(
new TalonFXConfiguration()
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
.withSlot0(
new Slot0Configs()
.withKP(0)
.withKI(0)
.withKD(0)
.withKS(0)));
kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION));
kickerMotor.getConfigurator().apply(
new TalonFXConfiguration()
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
)
.withSlot0(new Slot0Configs()
.withKP(0)
.withKI(0)
.withKD(0)
.withKS(0)
));
kickerVelocity = kickerMotor.getVelocity();
}
public void feedKicker() {kickerMotor.set(0.5);}
public void feedKicker() {
kickerMotor.setControl(kickerControl.withVelocity(1000));
// kickerMotor.set(0.75);
}
public void reverseKicker() {kickerMotor.set(-0.5);}
public void reverseKicker() {
kickerMotor.setControl(kickerControl.withVelocity(-500));
// kickerMotor.set(-0.5);
}
public void stopKicker() {kickerMotor.stopMotor();}
public void spindexerOn() { spindexerMotor.set(0.5);}
public void spindexerOn() { spindexerMotor.set(0.75);}
public void spindexerOff() { spindexerMotor.stopMotor();}
@Override
public void periodic() {
Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
}
@PostTest(enabled = true)
public void hopperSystem_CANDevicesConnected(){
assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();

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

@ -17,6 +17,7 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import static org.fairportrobotics.frc.robolib.motors.Utils.*;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
public class IntakeSubsystem extends TestableSubsystem {
/** Creates a new ExampleSubsystem. */
@ -35,9 +36,21 @@ public class IntakeSubsystem extends TestableSubsystem {
deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
}
public void extend()
public Command deploy()
{
deployMotor.set(1);
return Commands.sequence(
Commands.deadline(
Commands.waitSeconds(2),
this.runOnce(
() -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
)
),
this.runOnce(() -> deployMotor.stopMotor())
);
}
public Command resetDeploy(){
return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
}
public void setSpeed(double speed)
@ -45,19 +58,14 @@ public class IntakeSubsystem extends TestableSubsystem {
intakeMotor.set(speed);
}
public Command startSpeedCommand()
public Command intake()
{
return this.runOnce(() -> setSpeed(-.5));
return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
}
public Command killSpeedCommand()
public Command reverseIntake()
{
return this.runOnce(() -> setSpeed(0));
}
public Command revSpeedCommand()
{
return this.runOnce(() -> setSpeed(.5));
return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
}
@PostTest(enabled = true)

206
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -11,15 +11,22 @@ import org.littletonrobotics.junction.Logger;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.ForwardLimitValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.ReverseLimitValue;
import com.ctre.phoenix6.sim.TalonFXSimState;
import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
@ -27,29 +34,36 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkMaxConfig;
import frc.robot.Constants;
import edu.wpi.first.units.AngleUnit;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
public class TurretSubsystem extends TestableSubsystem {
public enum TurretState {
INIT,
HOMING,
MANUAL;
READY;
}
private final boolean LOGGING = true;
private TurretState currentState;
private TurretState turretState;
private DigitalInput turretLimitSwitch;
private TalonFX turretMotor;
private PositionVoltage turretControlMode = new PositionVoltage(0);
private TalonFXSimState turretSim;
private StatusSignal<Angle> turretMotorPosition;
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
private SparkMax launcherMotor;
private SparkClosedLoopController launcherControler;
private double turretOffsetInRotations = 0;
private Servo hood;
private Angle limitPos;
private Angle limitNeg;
private boolean turretGoingPos;
public TurretSubsystem() {
@ -60,9 +74,10 @@ public class TurretSubsystem extends TestableSubsystem {
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
.withKP(1.0)
.withKP(6)
.withKI(0)
.withKD(0)
.withKD(0.3)
.withKS(0)
)
.withMotorOutput(
new MotorOutputConfigs()
@ -70,35 +85,53 @@ public class TurretSubsystem extends TestableSubsystem {
)
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(50)
)
.withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false))
.withFeedback(
new FeedbackConfigs()
.withFeedbackRotorOffset(0)
)
);
turretMotorPosition = turretMotor.getPosition();
turretMotorForwardLimit = turretMotor.getForwardLimit();
turretMotorReverseLimit = turretMotor.getReverseLimit();
turretSim = turretMotor.getSimState();
// Launcher configuration
launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
config.closedLoop.p(1).i(0).d(0.1);
config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
config.voltageCompensation(10);
config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001
config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00);
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
config.closedLoop.positionWrappingEnabled(false);
// config.closedLoopRampRate(0.5);
// config.smartCurrentLimit(5, 50);
launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
launcherControler = launcherMotor.getClosedLoopController();
// Hood configuration
hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL);
currentState = TurretState.INIT;
limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG;
limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS;
turretGoingPos = false;
turretState = TurretState.INIT;
}
public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);}
public void setLauncher(double speed) {
if(speed == 0)
{
launcherMotor.stopMotor();
return;
}
launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0);
}
public boolean isLauncherUpToSpeed() {
return launcherControler.isAtSetpoint();
}
public void turretControl(double position)
{
turretMotor.setControl(new PositionVoltage(position));
// Valid if velcity is within 10% of setpoint.
// May need to lower this
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
}
public void setTargetElevation(double elev)
@ -113,97 +146,118 @@ public class TurretSubsystem extends TestableSubsystem {
return this.runOnce(() -> setLauncher(-.5));
}
public void startHoming()
public Command setShooter(int speedRPM){
return Commands.runEnd(() -> {
setLauncher(speedRPM);
}, () -> {
launcherMotor.stopMotor();
}, this);
}
public void homeTurret()
{
if(turretState != TurretState.INIT){
return; // Early return, we don't want to home if we already have
}
turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
currentState = TurretState.HOMING;
turretState = TurretState.HOMING;
}
public void startRotate(double speed)
public boolean isTurretReady(){
return turretState == TurretState.READY;
}
public void turretRotateDir(double speed)
{
turretGoingPos=speed>0;
if(!passedLimit())
{
turretMotor.set(speed);
}
else
{
turretMotor.set(0);
}
turretMotor.set(speed);
}
public boolean passedLimit(){
return (getTurretAngle().gt(limitPos) && turretGoingPos) || (getTurretAngle().lt(limitNeg) && !turretGoingPos);
}
public Angle getTurretAngle()
public double getTurretAngleRobotRelative()
{
return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
}
private double getTurretMotorPosition(){
return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
switch (currentState) {
case HOMING:
periodicHoming();
break;
case MANUAL:
periodicManual();
break;
default:
break;
if(turretState == TurretState.HOMING)
{
turretMotor.set(0.05);
if(turretLimitSwitch.get())
{ // Turret has tripped the switch
turretMotor.stopMotor();
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
turretState = TurretState.READY;
// Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
// Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
// Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
// Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs()
// .withForwardSoftLimitEnable(true)
// .withForwardSoftLimitThreshold(forwardLimit)
// .withReverseSoftLimitEnable(true)
// .withReverseSoftLimitThreshold(reverseLimit)
);
// setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}
if(LOGGING)
{
Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
}
}
private void periodicHoming()
public void setTurretFieldRelative(double pos)
{
if(turretLimitSwitch.get())
if(turretState != TurretState.READY)
{
currentState = TurretState.MANUAL;
turretMotor.set(0);
turretMotor.setPosition(0);
goTo(limitPos);
// Not ready to set angle
return;
}
}
private void goTo(Angle pos)
public void setTurretRobotRelative(double pos)
{
if(pos.gt(limitNeg) && pos.lt(limitPos))
if(turretState != TurretState.READY)
{
while(getTurretAngle()!=pos)
{
if(getTurretAngle().gt(pos))
{
turretMotor.set(.5);
}
else
{
turretMotor.set(-.5);
}
}
// Not ready to set angle
return;
}
setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
}
private void periodicManual()
{
if(passedLimit())
{
turretMotor.set(0);
}
public void setTurretMotorRotation(double pos){
turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations)));
turretSim.setRawRotorPosition(pos + turretOffsetInRotations);
}
private double getRequestedPosTurretRelative() {
return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
}
@PostTest()
public void TurretSubsystem_CANDevicesConnected(){
assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0);
assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();
assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
}