Skip to content

Commit

Permalink
practice day changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@CrolineCrois
CrolineCrois committed Apr 18, 2024
1 parent e7b3714 commit 0526a58
Show file tree
Hide file tree
Showing 5 changed files with 105 additions and 33 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ public static class IntakeConstants {
public static final int BACK_MOTOR_ID = 18;
public static final int PIVOT_MOTOR_ID = 16;

public static final double PIVOT_P = 1;
public static final double PIVOT_P = 2.5;
public static final double PIVOT_I = 0;
public static final double PIVOT_D = 0;
public static final double PIVOT_CONVERSION_FACTOR = 0.2142;
Expand Down
84 changes: 60 additions & 24 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -463,7 +463,9 @@ private void configureBindings() {
/* ElEVATOR TEST */

dPadUp.onTrue(new ElevatorToTrapCommand(elevatorSubsystem));
dPadUp.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false)));
dPadDown.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem));
dPadDown.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(true)));

// elevatorSubsystem.setManual();

Expand Down Expand Up @@ -510,31 +512,40 @@ private void configureBindings() {
// if the elevator is down, run the amp sequence
rightBumper.onTrue(
new ConditionalCommand(
// if elevator is up
new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow the pivot

// if elevator is down
new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem)
.until(() -> mechController.getLeftTriggerAxis() > .05
|| mechController.getRightTriggerAxis() > .05),

// check if the elevator is currently targeting one of the upper positions to choose what to do
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP));
// if elevator is up
new ElevatorToZeroCommand(elevatorSubsystem).alongWith(
Commands.runOnce(() -> {
intakePivotSubsystem.enablePowerLimit(true);
intakePivotSubsystem.setPosition(0);
}, intakePivotSubsystem)
), // stow the pivot

// if elevator is down
new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem)
.until(() -> mechController.getLeftTriggerAxis() > .05
|| mechController.getRightTriggerAxis() > .05),

// check if the elevator is currently targeting one of the upper positions to choose what to do
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP
)
);

// leftBumper toggles the trap position for the elevator
leftBumper.onTrue(
new ConditionalCommand(
new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow intake
() -> {
intakePivotSubsystem.enablePowerLimit(true);
intakePivotSubsystem.setPosition(0);
}, intakePivotSubsystem)), // stow intake
new ConditionalCommand(
new ElevatorToTrapCommand(elevatorSubsystem).andThen(
new IntakePivotSetPositionCommand(intakePivotSubsystem, .45).withTimeout(.1)
new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false)),
new IntakePivotSetPositionCommand(intakePivotSubsystem, .45)
),
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(// extend pivot
new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
.until(() -> intakeRollerSubsystem.getFrontSensorReached()),
new ConditionalWaitCommand(intakeRollerSubsystem::getFrontSensorReached),
new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
),
intakeRollerSubsystem::getAmpSensor
Expand All @@ -544,18 +555,36 @@ private void configureBindings() {
);


// roll behaviors
leftBumper.onTrue(
new ConditionalCommand(
null,
new ConditionalCommand(
null,
new WaitCommand(.05).andThen(
new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
.until(intakeRollerSubsystem::getFrontSensorReached)
),
intakeRollerSubsystem::getAmpSensor
), // raise the elevator
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
);



// aButton runs the intake sequence
aButton.onTrue(
new InstantCommand(() -> {intakePivotSubsystem.setPosition(1);}, intakePivotSubsystem).alongWith(// then extend the intake
Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith(
new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen(
new ConditionalCommand(
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen(
new InstantCommand(() -> {intakePivotSubsystem.setPosition(0);}, intakePivotSubsystem)
new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)
),

new InstantCommand(
() -> {intakePivotSubsystem.setPosition(0);}, intakePivotSubsystem
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem
),
() -> aButton.getAsBoolean()
)
Expand All @@ -564,7 +593,7 @@ private void configureBindings() {
);

// bButton stops the rollers
bButton.onTrue(new InstantCommand(() -> {}, intakeRollerSubsystem));
bButton.onTrue(Commands.idle(intakeRollerSubsystem));

// xButton toggles the intake being stowed
xButton.onTrue(new InstantCommand(() -> {
Expand Down Expand Up @@ -592,14 +621,18 @@ private void configureBindings() {
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
).unless(intakeRollerSubsystem::getRockwellSensorValue)
).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen(
new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
)
);

shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> {
if (yButton.getAsBoolean()) {
lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2);
// shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
shooterFlywheelSubsystem.setShooterMotorSpeed();
if (intakePivotSubsystem.atPosition()) {
shooterFlywheelSubsystem.setShooterMotorSpeed();
}
shooterPivotSubsystem.setAutoAimBoolean(true);
if (shooterFlywheelSubsystem.atSpeed()) {
mechController.setRumble(RumbleType.kBothRumble, .4);
Expand Down Expand Up @@ -658,8 +691,11 @@ private void configureBindings() {
new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
).unless(intakeRollerSubsystem::getRockwellSensorValue));

dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .65, mechController).onlyWhile(dPadRight));

dPadRight.whileTrue(
new ConditionalWaitCommand(() -> intakePivotSubsystem.atPosition())
.andThen(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem,
shooterPivotSubsystem, .7, mechController))
);
// intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
// intakePosition = MathUtil.clamp(intakePosition, 0, 1);
// intakePivotSubsystem.setPosition(intakePosition);
Expand Down
11 changes: 7 additions & 4 deletions src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,8 @@
public class IntakeRollerIntakeCommand extends Command {
private final IntakeRollerSubsystem intakeSubsystem;
private final LightBarSubsystem lightBarSubsystem;
private double speed = .7;
private double frontSpeed = .7;
private double integrationSpeed = .4;

/**
* Runs the rollers on the intake until a note is detected.
Expand All @@ -23,11 +24,12 @@ public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBar
addRequirements(intakeSubsystem);
}

public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBarSubsystem lightBarSubsystem, double speed) {
public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBarSubsystem lightBarSubsystem, double speed, double integrationSpeed) {
this.intakeSubsystem = intakeSubsystem;
this.lightBarSubsystem = lightBarSubsystem;
addRequirements(intakeSubsystem);
this.speed = speed;
this.frontSpeed = speed;
this.integrationSpeed = integrationSpeed;
}

@Override
Expand All @@ -37,7 +39,8 @@ public void initialize() {

@Override
public void execute() {
intakeSubsystem.setRollSpeeds(speed, speed);
intakeSubsystem.setRollSpeeds(frontSpeed, integrationSpeed);
System.out.println(intakeSubsystem.getRockwellSensorValue());
}

@Override
Expand Down
39 changes: 36 additions & 3 deletions src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,14 @@
import static frc.robot.Constants.IntakeConstants;

import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.VoltageConfigs;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;


/** The subsystem for the pivot on the intake. */
Expand All @@ -18,7 +19,9 @@ public class IntakePivotSubsystem extends SubsystemBase {
private final TalonFX pivotMotor;

private PositionVoltage request = new PositionVoltage(0).withSlot(0);
private VoltageConfigs voltageConfig = new VoltageConfigs();
private double setPos = 0;
private static final double FORWARD_VOLTAGE = 1.75;

private NetworkTableInstance ntInstance;
private NetworkTable motorsNTTable;
Expand All @@ -32,6 +35,7 @@ public class IntakePivotSubsystem extends SubsystemBase {
public IntakePivotSubsystem() {
pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
pivotMotor.setNeutralMode(NeutralModeValue.Brake);

// intakeencoder = new Encoder(1, 2);
// extendedlimitswitch = new DigitalInput(extendedlimitswitchID);
// retractedlimitswitch = new DigitalInput(retractedlimitswitchID);
Expand All @@ -42,8 +46,16 @@ public IntakePivotSubsystem() {
slot0Configs.kI = IntakeConstants.PIVOT_I;
slot0Configs.kD = IntakeConstants.PIVOT_D;

voltageConfig.PeakForwardVoltage = FORWARD_VOLTAGE;
voltageConfig.PeakReverseVoltage = -16;


pivotMotor.getConfigurator().apply(slot0Configs);
pivotMotor.setPosition(0);
pivotMotor.getConfigurator().apply(voltageConfig);


request.withLimitForwardMotion(false);

ntInstance = NetworkTableInstance.getDefault();
motorsNTTable = ntInstance.getTable("Motors");
Expand Down Expand Up @@ -77,6 +89,11 @@ public double getEncoderPosition() {
return pivotMotor.getPosition().getValueAsDouble() * IntakeConstants.PIVOT_CONVERSION_FACTOR;
}

/** Returns whether or not the intake is at (or within tolerance of) its setpoint. */
public boolean atPosition() {
return Math.abs(getEncoderPosition() - setPos) < .05;
}

/**
* Sets the pivot motor to a speed.
*
Expand All @@ -86,10 +103,26 @@ public void movePivot(double speed) {
pivotMotor.set(speed);
}

/**
* Enables/disables the pivot motor's forward voltage limit for deploying the intake.
* When enabled, the linkage will not slam down when deploying.
*
* @param enabled True enables the limit, false disables it.
*/
public void enablePowerLimit(boolean enabled) {
if (enabled) {
voltageConfig.PeakForwardVoltage = FORWARD_VOLTAGE;
} else {
voltageConfig.PeakForwardVoltage = 16;
}

pivotMotor.getConfigurator().apply(voltageConfig);
}

@Override
public void periodic() {
pivotMotor.setControl(request.withPosition(setPos / IntakeConstants.PIVOT_CONVERSION_FACTOR));

intake16CurrentEntry.setDouble(pivotMotor.getSupplyCurrent().getValueAsDouble());
intake16VoltageEntry.setDouble(pivotMotor.getMotorVoltage().getValueAsDouble());
intake16TemperatureEntry.setDouble(pivotMotor.getDeviceTemp().getValueAsDouble());
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier re
5,
6,
ShooterConstants.MAX_SHOOTER_DISTANCE};
double[] angles = {Units.degreesToRadians(60),
double[] angles = {Units.degreesToRadians(59),
Units.degreesToRadians(52),
Units.degreesToRadians(36),
Units.degreesToRadians(31), //4
Expand Down

0 comments on commit 0526a58

Please sign in to comment.