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

Reverse command :3

This commit is contained in:
TheRealGrayden 2026-04-13 20:13:46 -04:00
parent 2fdfa62816
commit 343884052f
4 changed files with 135 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,6 +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.Reverse;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
@ -82,7 +83,7 @@ public class RobotContainer {
//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().onTrue(Commands.parallel(intakeSubsystem.reverseIntake(), null));
m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem));
m_driverController.y().onTrue(intakeSubsystem.deploy());
}

14
src/main/java/frc/robot/commands/Reverse.java
View File

@ -12,17 +12,17 @@ import frc.robot.subsystems.IntakeSubsystem;
*/
public class Reverse extends Command {
private int m_subsystem;
private IntakeSubsystem intakeSubsystem;
private HopperSubsystem hopperSubsystem;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public Reverse() {
m_subsystem = subsystem;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) {
this.intakeSubsystem = intakeSubsystem;
this.hopperSubsystem = hopperSubsystem;
}
// Called when the command is initially scheduled.
@ -33,6 +33,9 @@ public class Reverse extends Command {
// 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.
@ -40,6 +43,7 @@ public class Reverse extends Command {
public void end(boolean interrupted) {
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
intakeSubsystem.stopIntake();
}
// Returns true when the command should end.

118
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();
}
}