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-13 20:46:15 -04:00
parent 2ada8c9408 343884052f
commit e0ecdb42f7
6 changed files with 362 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,6 +9,7 @@ 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.units.measure.Angle;
@ -170,5 +171,50 @@ public final class Constants {
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)));
// These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side.
// Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
// Rotation are zero because they don't matter.
public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d());
public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d());
public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
}
// These represent the dividing lines for the regions in our shooting strategy.
public static class ShootingRegionDimensions {
public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0;
public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5;
public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0;
public static double BLUE_RIGHT_REGION_Y_MAX = 3.0;
public static double RED_LEFT_REGION_Y_MAX = 3.0; // same meaning as the previous one, but from red perspective
public static double BLUE_LEFT_REGION_Y_MIN = 5.0;
public static double RED_RIGHT_REGION_Y_MIN = 5.0; // same meaning as the previous one, but from red perspective
}
// Regions that define the AutoTurret shooting behaviors.
// Color ID in the name reflects robot's alliance since region definitions vary by alliance.
// Left/Right are from driver's perspective.
public enum ShootingRegion {
NON_SHOOTING_REGION, // Any part of the field that's not in one of the regions
BLUE_OWN_ALLIANCE_ZONE,
BLUE_LEFT_NEUTRAL_ZONE,
BLUE_RIGHT_NEUTRAL_ZONE,
BLUE_LEFT_OPPONENT_ALLIANCE_ZONE,
BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE,
RED_OWN_ALLIANCE_ZONE,
RED_LEFT_NEUTRAL_ZONE,
RED_RIGHT_NEUTRAL_ZONE,
RED_LEFT_OPPONENT_ALLIANCE_ZONE,
RED_RIGHT_OPPONENT_ALLIANCE_ZONE
}
}

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

@ -13,9 +13,7 @@ import frc.robot.Constants.OperatorConstants;
import frc.robot.commands.AutoShootCommandChassisTurret;
import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
import frc.robot.commands.AutoTurretCommand;
import frc.robot.commands.RelayCommand;
import frc.robot.commands.SetDeflectorCommand;
import frc.robot.commands.ZoneCheck;
import frc.robot.commands.Reverse;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
@ -73,21 +71,20 @@ public class RobotContainer {
*
*/
private void configureBindings() {
m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
/*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.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(Commands.parallel(intakeSubsystem.intake()));
m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
//m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
m_driverController.b().toggleOnTrue(new ZoneCheck(driveSubsystem));
m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem));
m_driverController.y().onTrue(intakeSubsystem.deploy());
}
/**

132
src/main/java/frc/robot/Utils.java
View File

@ -1,5 +1,11 @@
package frc.robot;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.Constants;
import frc.robot.Constants.ShootingRegion;
import frc.robot.Constants.ShootingRegionDimensions;
import edu.wpi.first.math.geometry.Pose3d;
public class Utils {
/**
@ -22,4 +28,130 @@ public class Utils {
return robotTurretRotations;
}
/**
* Determines the region for the robot based on its location and alliance.
* Assumes the robot is a 27" square. Thresholds based on observations from simulator.
* @param robotPose: the robot's pose
* @param alliance: the red/blue alliance of the robot, as a sanity check
* @return the robot's shooting region
*/
public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION; // the default return value if robot isn't in a valid region
double robotX = robotPose.getX();
double robotY = robotPose.getY();
// Receommended: Add a sanity check for invalid robot poses.
if (alliance == Alliance.Blue) {
if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
}
}
else if (alliance == Alliance.Red) {
if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
}
else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
}
}
return region;
}
/**
* Determines the appropriate shooting target for the robot based on its alliance
* and its current region on the field. There are 10 possible targets:
* Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
* and these exist for both red and blue alliances.
* Right/left are from driver's perspective.
* @param region: the identifier of the robot's current field region
* @param alliance: the red/blue alliance of the robot, as a sanity check
* @return the shooting target for a robot with the provided properties. Returns all-zero
* Pose3d object if the region/alliance combination is invalid.
*/
public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
Pose3d shootingTarget = new Pose3d();
if (alliance == Alliance.Blue) {
switch (region) {
case BLUE_OWN_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
break;
case BLUE_LEFT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case BLUE_RIGHT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
default:
break;
}
}
else if (alliance == Alliance.Red) {
switch (region) {
case RED_OWN_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
break;
case RED_LEFT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case RED_RIGHT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
default:
break;
}
}
return shootingTarget;
}
}

54
src/main/java/frc/robot/commands/Reverse.java Normal file
View File

@ -0,0 +1,54 @@
// 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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
/**
* An example command that uses an example subsystem.
*/
public class Reverse extends Command {
private IntakeSubsystem intakeSubsystem;
private HopperSubsystem hopperSubsystem;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) {
this.intakeSubsystem = intakeSubsystem;
this.hopperSubsystem = hopperSubsystem;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
intakeSubsystem.reverseIntake();
hopperSubsystem.spindexerReverse();
hopperSubsystem.reverseKicker();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
intakeSubsystem.stopIntake();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

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

@ -1,94 +1,100 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem;
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
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 edu.wpi.first.wpilibj2.command.Command;
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 TalonFXConfiguration()
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
.withSlot0(
new Slot0Configs()
.withKP(0)
.withKI(0)
.withKD(0)
.withKS(0)));
/**
* Creates a new HopperSubsystem.
*/
public HopperSubsystem() {
spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
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 TalonFXConfiguration()
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
)
.withSlot0(new Slot0Configs()
.withKP(0)
.withKI(0)
.withKD(0)
.withKS(0)
));
kickerVelocity = kickerMotor.getVelocity();
}
kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
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.setControl(kickerControl.withVelocity(1000));
kickerMotor.set(0.75);
}
public void feedKicker() {
//kickerMotor.setControl(kickerControl.withVelocity(1000));
kickerMotor.set(0.75);
}
public void reverseKicker() {
//kickerMotor.setControl(kickerControl.withVelocity(-500));
kickerMotor.set(-0.5);
}
public void reverseKicker() {
//kickerMotor.setControl(kickerControl.withVelocity(-500));
kickerMotor.set(-0.5);
}
public void stopKicker() {kickerMotor.stopMotor();}
public void stopKicker() {
kickerMotor.stopMotor();
}
public void spindexerOn() { spindexerMotor.set(0.75);}
public void spindexerOn() {
spindexerMotor.set(0.75);
}
public void spindexerOff() { spindexerMotor.stopMotor();}
public void spindexerReverse() {
spindexerMotor.set(-0.75);
}
@Override
public void periodic() {
Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
}
public void spindexerOff() {
spindexerMotor.stopMotor();
}
@PostTest(enabled = true)
public void hopperSystem_CANDevicesConnected(){
assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
}
@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();
assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
}
}

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

@ -1,76 +1,88 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem;
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
import org.fairportrobotics.frc.posty.test.PostTest;
import static org.fairportrobotics.frc.robolib.motors.Utils.SetMotorDirection;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix6.hardware.TalonFX;
import frc.robot.Constants;
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;
import frc.robot.Constants;
public class IntakeSubsystem extends TestableSubsystem {
/** Creates a new ExampleSubsystem. */
private TalonFX intakeMotor;
private WPI_TalonSRX deployMotor;
/**
* Creates a new ExampleSubsystem.
*/
public IntakeSubsystem()
{
intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
private TalonFX intakeMotor;
private WPI_TalonSRX deployMotor;
deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
}
public IntakeSubsystem() {
intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
public Command deploy()
{
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())
);
}
deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
}
public Command resetDeploy(){
return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
}
public Command deploy() {
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 void setSpeed(double speed)
{
intakeMotor.set(speed);
}
public Command resetDeploy() {
return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
}
public Command intake()
{
return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
}
public void setSpeed(double speed) {
intakeMotor.set(speed);
}
public Command reverseIntake()
{
return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
}
public Command stopIntake() {
return this.runOnce(() -> {
intakeMotor.stopMotor();
});
}
@PostTest(enabled = true)
public void IntakeSubsystem_CANDevicesConnected(){
assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
}
public Command intake() {
return this.runOnce(() -> {
if (intakeMotor.get() == 0) {
setSpeed(-.5);
} else {
intakeMotor.stopMotor();
}
});
}
public Command reverseIntake() {
return this.runEnd(() -> setSpeed(.5), () -> {
intakeMotor.stopMotor();
});
}
@PostTest(enabled = true)
public void IntakeSubsystem_CANDevicesConnected() {
assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
}
}