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

More automation and new IDs

This commit is contained in:
Tyler Wilcox 2026-03-22 16:03:07 -04:00
parent c89b0f299b
commit 5009913ea4
8 changed files with 166 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit eea76939a3cbb76aba47874adf15eaf49a8333de
Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee

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

@ -4,11 +4,7 @@
package frc.robot;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Unit;
import edu.wpi.first.units.measure.Angle;
import java.security.PublicKey;
import java.util.Queue;
import com.ctre.phoenix6.signals.InvertedValue;
@ -25,6 +21,16 @@ public final class Constants {
public static final int kDriverControllerPort = 0;
}
public static class CameraConstanst {
public static final String BACK_CAMERA_NAME = "back";
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final int BACK_HUB_TRACKING_PIPELINE_NUMBER = 1;
public static final String FRONT_CAMERA_NAME = "front";
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
}
public static class AdvantageKitConstants{
public enum RobotType{
SIM,
@ -32,7 +38,6 @@ public final class Constants {
REPLAY
}
public static final RobotType CURRENT_MODE = RobotType.REAL;
}
// Swerve offsets as of 3/2/26
public static class SwerveDriveOffsets {
@ -72,7 +77,7 @@ public final class Constants {
public static final int DEPLOY_MOTOR_ID = 17;
public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this
public static final int INTAKE_MOTOR_ID = 0;
public static final int INTAKE_MOTOR_ID = 2;
public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
}
@ -80,7 +85,7 @@ public final class Constants {
public static final int LAUNCHER_MOTOR_ID = 2;
public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this
public static final int TURRET_MOTOR_ID = 0;
public static final int TURRET_MOTOR_ID = 3;
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final int TURRET_LIMIT_CHANNEL = 0;
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265);
@ -88,7 +93,7 @@ public final class Constants {
public static final double HOMING_SPEED = .35;
public static final double TURRET_GEAR_RATIO = 0;
public static final int HOOD_SERVO_CHANNEL = 9;
public static final int HOOD_SERVO_CHANNEL = 0;
public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this
public static final int HOOD_ENCODER_ID = 0;
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5);
@ -103,10 +108,9 @@ public final class Constants {
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 InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final int SPINDEXER_MOTOR_ID = 8;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final double SPINDEXER_MOTOR_SPEED = .6;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
}
}

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

@ -79,9 +79,6 @@ public class RobotContainer {
m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand());
m_driverController.b().onTrue(hopperSubsystem.revSpeedCommand());
m_driverController.b().onTrue(turretSubsystem.revSpeedCommand());
intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
}

75
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java Normal file
View File

@ -0,0 +1,75 @@
package frc.robot.commands;
import java.lang.constant.Constable;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers.LimelightTarget_Fiducial;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class AutoShootCommandChassisTurret extends Command{
// For Chassis Control
private DriveSubsystem driveSubsystem;
private HopperSubsystem hopperSubsystem;
private IntakeSubsystem intakeSubsystem;
private TurretSubsystem turretSubsystem;
private PIDController autoCenterController;
public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, IntakeSubsystem intakeSubsystem, TurretSubsystem turretSubsystem)
{
this.driveSubsystem = driveSubsystem;
this.hopperSubsystem = hopperSubsystem;
this.intakeSubsystem = intakeSubsystem;
this.turretSubsystem = turretSubsystem;
autoCenterController = new PIDController(0, 0, 0);
autoCenterController.setTolerance(1);
autoCenterController.setSetpoint(0);
}
@Override
public void initialize() {
turretSubsystem.setLauncher(2500);
hopperSubsystem.spindexerOn();
}
@Override
public void execute() {
// Need to compute shoot speed and angle
// Get apriltags
// Change to HUB tracking pipeline
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_PIPELINE_NUMBER);
LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){
hopperSubsystem.feedKicker();
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
turretSubsystem.setLauncher(0);
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
}
}

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

@ -23,7 +23,7 @@ public class ManualHopperCommand extends Command {
// 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.
@ -31,19 +31,19 @@ public class ManualHopperCommand extends Command {
public void execute() {
if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
{
m_subsystem.feed();
m_subsystem.feedKicker();
}
else
{
m_subsystem.stop();
m_subsystem.stopKicker();
}
if(m_xboxController.getRightBumper())
{
m_subsystem.spin(Constants.HopperConstants.SPINDEXER_MOTOR_SPEED);
m_subsystem.spindexerOn();
}
if(m_xboxController.getLeftBumper())
{
m_subsystem.spin(0);
m_subsystem.spindexerOff();
}
}

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

@ -28,26 +28,20 @@ public class HopperSubsystem extends TestableSubsystem {
SetMotorDirection(kickerMotor, Constants.HopperConstants.KICKER_MOTOR_DIRECTION);
}
public void feed() {kickerMotor.set(0.5);}
public void feedKicker() {kickerMotor.set(0.5);}
public void reverse() {kickerMotor.set(-0.5);}
public void reverseKicker() {kickerMotor.set(-0.5);}
public void stop() {kickerMotor.set(0);}
public void stopKicker() {kickerMotor.stopMotor();}
public void spin(double speed) {spindexerMotor.set(speed);}
public void spindexerOn() { spindexerMotor.set(0.5);}
public Command revSpeedCommand()
{
return this.runOnce(() -> reverse());
}
@PostTest(name = "A friendly name", enabled = true)
public void myFailingPostTest(){
assertThat(true).isFalse();
}
public void spindexerOff() { spindexerMotor.stopMotor();}
@PostTest(enabled = true)
public void myPassingPostTest(){
assertThat("Hello World").endWith("World");
public void hopperSystem_CANDevicesConnected(){
assertThat(kickerMotor.isConnected()).isTrue().as("Kicker motor not connected!");
assertThat(spindexerMotor.isConnected()).isTrue().as("Spindexer motor not connected!");
}
}

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

@ -61,4 +61,10 @@ public class IntakeSubsystem extends TestableSubsystem {
{
return this.runOnce(() -> setSpeed(.5));
}
@PostTest(enabled = true)
public void IntakeSubsystem_CANDevicesConnected(){
assertThat(intakeMotor.isConnected()).isTrue().as("Intake motor not connected!");
assertThat(deployMotor.isAlive()).isTrue().as("Deploy controller not connected!");
}
}

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

@ -5,24 +5,28 @@
package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem;
import org.fairportrobotics.frc.posty.test.PostTest;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkMaxConfig;
import frc.robot.Constants;
import static org.fairportrobotics.frc.robolib.motors.Utils.*;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.Velocity;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj2.command.Command;
@ -38,21 +42,46 @@ public class TurretSubsystem extends TestableSubsystem {
private DigitalInput turretLimitSwitch;
private TalonFX turretMotor;
private SparkMax launcherMotor;
private SparkClosedLoopController launcherControler;
private Servo hood;
private Angle limitPos;
private Angle limitNeg;
private boolean turretGoingPos;
public TurretSubsystem() {
turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
//turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
//SetMotorDirection(turretMotor, Constants.ShooterConstants.TURRET_MOTOR_DIRECTION);
// Turret configuration
turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
turretMotor.getConfigurator().apply(
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
.withKP(1.0)
.withKI(0)
.withKD(0)
)
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(InvertedValue.Clockwise_Positive)
)
.withCurrentLimits(
new CurrentLimitsConfigs()
)
);
// Launcher configuration
launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
SparkMaxConfig config = new SparkMaxConfig()
.apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO_2);
.apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
config.closedLoop.p(1).i(0).d(0).outputRange(0, 5000);
config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
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;
@ -60,7 +89,11 @@ public class TurretSubsystem extends TestableSubsystem {
turretGoingPos = false;
}
public void setLauncher(double speed) {launcherMotor.set(speed);}
public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);}
public boolean isLauncherUpToSpeed() {
return launcherControler.isAtSetpoint();
}
public void turretControl(double position)
{
@ -108,8 +141,7 @@ public class TurretSubsystem extends TestableSubsystem {
public Angle getTurretAngle()
{
// return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
return Units.Degrees.of(0.0);
return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
}
@Override
@ -147,4 +179,11 @@ public class TurretSubsystem extends TestableSubsystem {
//turretMotor.set(0);
}*/
}
@PostTest()
public void TurretSubsystem_CANDevicesConnected(){
assertThat(launcherMotor.getDeviceId()).isGreaterThan(0).as("Launcher motor not connected!");
assertThat(turretMotor.isConnected()).isTrue().as("Turret motor not connected!");
}
}