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

Added relay command, and many attempts to fix auto align

This commit is contained in:
Tyler Wilcox 2026-04-02 19:29:42 -04:00
parent fa9d628c9c
commit 0ced1e4d59
16 changed files with 324 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12
Subproject commit 0584a07e801a1823e4113c16e42b4d97f147cc51

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"
}
}
]
}
}
]
}

2
src/main/deploy/pathplanner/paths/M. Depot. .path
View File

@ -36,7 +36,7 @@
"y": 7.138469879518072
},
"prevControl": {
"x": 0.4124120733073493,
"x": 0.41241207330734936,
"y": 6.876706525398199
},
"nextControl": {

8
src/main/deploy/pathplanner/paths/Relay. R-Tr..path
View File

@ -37,7 +37,7 @@
},
"prevControl": {
"x": 7.691609840125085,
"y": 7.387443181134998
"y": 7.387443181134999
},
"nextControl": {
"x": 7.64634196710383,
@ -56,7 +56,7 @@
"y": 7.138469879518072
},
"nextControl": {
"x": 6.511975903614461,
"x": 6.511975903614458,
"y": 7.138469879518072
},
"isLocked": false,
@ -85,11 +85,11 @@
},
"prevControl": {
"x": 6.020824888067715,
"y": 0.8531028227756468
"y": 0.8531028227756466
},
"nextControl": {
"x": 5.667271497474452,
"y": 1.2066562133689314
"y": 1.2066562133689316
},
"isLocked": false,
"linkedName": null

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,

2
src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path
View File

@ -24,7 +24,7 @@
"y": 3.77441480327739
},
"nextControl": {
"x": 2.348744281004639,
"x": 2.3487442810046395,
"y": 4.0114647147949
},
"isLocked": false,

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

@ -4,6 +4,12 @@
package frc.robot;
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.Transform3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
@ -76,6 +82,19 @@ public final class Constants {
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
public static class RobotChassisLimits{
// Max velocity in meters per second
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
// In meters per second squared
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
// 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;
}
//Bird
public static class ExtraIDEntities {
public static final int PIGEON_ID = 1;
@ -121,4 +140,12 @@ public final class Constants {
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)));
}
}

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

@ -4,6 +4,8 @@
package frc.robot;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
@ -11,10 +13,12 @@ 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.AutoShootCommandChassisTurretPathPlanner;
import frc.robot.commands.AutoTurretCommand;
import frc.robot.commands.ManualHopperCommand;
import frc.robot.commands.ManualIntake;
import frc.robot.commands.ManualShootCommand;
import frc.robot.commands.RelayCommand;
import frc.robot.commands.SetDeflectorCommand;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
@ -45,11 +49,14 @@ public class RobotContainer {
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
// turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
// turretSubsystem.homeTurret();
// Configure the trigger bindings
configureBindings();
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
}
/**
@ -68,8 +75,14 @@ public class RobotContainer {
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.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().onTrue(intakeSubsystem.deploy());
m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
}
@ -80,6 +93,6 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return Commands.parallel(Commands.run(()-> {/*turretSubsystem.homeTurret();*/}, turretSubsystem), driveSubsystem.getAutoCommand());
return Commands.parallel(intakeSubsystem.deploy(), Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem), driveSubsystem.getAutoCommand());
}
}

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

@ -75,6 +75,7 @@ public class AutoShootCommandChassisTurret extends Command{
});
turretSubsystem.setLauncher(5500);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
hopperSubsystem.spindexerOn();
cameraAutoCenterController.calculate(-100);
@ -91,9 +92,13 @@ public class AutoShootCommandChassisTurret extends Command{
Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
// Add 180 because we want the back of the robot to face the HUB
deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
}
});
}
@Override
public void execute() {

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) {
pathFollowCommand.cancel();
turretSubsystem.setLauncher(0);
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
driveSubsystem.stopDrive();
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME,
Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
}
}

12
src/main/java/frc/robot/commands/AutoTurretCommand.java
View File

@ -29,8 +29,6 @@ public class AutoTurretCommand extends Command {
private int[] shootingTagFilters;
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Pose3d closestTag;
private Pose3d scoringHubPose;
@ -49,14 +47,10 @@ public class AutoTurretCommand extends Command {
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
scoringHubPose = fieldTags.getTagPose(26).get();
scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE;
} else {
shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
scoringHubPose = fieldTags.getTagPose(10).get();
scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
scoringHubPose = Constants.FieldPoses.RED_HUB_POSE;
}
Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
});
@ -76,7 +70,7 @@ public class AutoTurretCommand extends Command {
if(turretSubsystem.isTurretReady()){
turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
}

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

@ -0,0 +1,59 @@
package frc.robot.commands;
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(5500);
hopperSubsystem.spindexerOn();
CommandScheduler.getInstance().schedule(waitCommand);
}
@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);
}
}

12
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)
@ -135,7 +137,7 @@ public class DriveSubsystem extends TestableSubsystem {
// Set vision measurement confidence values
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7));
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));
this.setDefaultCommand(Commands.run(new Runnable() {
@ -163,8 +165,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();

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

@ -41,21 +41,18 @@ public class IntakeSubsystem extends TestableSubsystem {
return Commands.sequence(
Commands.deadline(
Commands.waitSeconds(2),
this.runEnd(
() -> {deployMotor.set(0.5);}, // Bump this speed up if intake doesn't deploy
() -> {deployMotor.stopMotor();}
this.runOnce(
() -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
)
),
Commands.deadline(
Commands.waitSeconds(2),
this.runEnd(
() -> { deployMotor.set(-0.5); }, // Same here
() -> { deployMotor.stopMotor(); }
)
)
this.runOnce(() -> deployMotor.stopMotor())
);
}
public Command resetDeploy(){
return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
}
public void setSpeed(double speed)
{
intakeMotor.set(speed);
@ -63,7 +60,7 @@ public class IntakeSubsystem extends TestableSubsystem {
public Command intake()
{
return this.runOnce(() -> {if(intakeMotor.get()!=0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
}
public Command reverseIntake()

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

@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem {
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
.withKP(2)
.withKP(3)
.withKI(1)
.withKD(0)
.withKS(0)
.withKS(0.3)
)
.withMotorOutput(
new MotorOutputConfigs()
@ -134,11 +134,6 @@ public class TurretSubsystem extends TestableSubsystem {
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1;
}
public void turretControl(double position)
{
turretMotor.setControl(new PositionVoltage(position));
}
public void setTargetElevation(double elev)
{
double pos = Math.abs((elev - 300)/300);
@ -161,6 +156,9 @@ public class TurretSubsystem extends TestableSubsystem {
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);
turretState = TurretState.HOMING;
}
@ -193,7 +191,7 @@ public class TurretSubsystem extends TestableSubsystem {
if(turretLimitSwitch.get())
{ // Turret has tripped the switch
turretMotor.stopMotor();
turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
turretOffset = turretMotorPosition.refresh().getValue().minus(Angle.ofRelativeUnits(65, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
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);
@ -201,12 +199,13 @@ public class TurretSubsystem extends TestableSubsystem {
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs()
.withForwardSoftLimitEnable(true)
// .withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(forwardLimit)
.withReverseSoftLimitEnable(true)
// .withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit)
);
//setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees));
// setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
setTurretMotorRotation(Angle.ofRelativeUnits(0, Units.Degrees));
}
}
@ -226,7 +225,16 @@ public class TurretSubsystem extends TestableSubsystem {
}
public void setTurretRotation(Angle pos)
public void setTurretFieldRelative(Angle pos)
{
if(turretState != TurretState.READY)
{
// Not ready to set angle
return;
}
}
public void setTurretRobotRelative(Angle pos)
{
if(turretState != TurretState.READY)
{
@ -237,8 +245,8 @@ public class TurretSubsystem extends TestableSubsystem {
setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
}
public void setTurretMotorRotation(Angle pos){
turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
private void setTurretMotorRotation(Angle pos){
turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1)));
turretSim.setRawRotorPosition(pos.plus(turretOffset));
}