Skip to content

Commit

Permalink
Working intake
Browse files Browse the repository at this point in the history
  • Loading branch information
@DerekChen1
DerekChen1 committed Nov 8, 2023
1 parent 6725220 commit 3f84e82
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 37 deletions.
19 changes: 9 additions & 10 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -197,23 +197,22 @@ public static final class Ports {
public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(14, "elevator left motor");
public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(15, "elevator right motor");
public static final int WRIST_MOTOR_PORT = CAN.at(16, "wrist motor port");
public static final int CANCODER = CAN.at(0, "cancoder");
}

public static final class Setpoints {
public static final ElevatorState STOWED = new ElevatorState(0, 0);
public static final ElevatorState STOWED =
new ElevatorState(MIN_EXTENSION_INCHES, MIN_ANGLE_DEGREES);
public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20);
public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION_INCHES, 0);
public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 0);
public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 20);
public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 40);
public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION_INCHES, 0);
public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION_INCHES, 80);
public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 20);
public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 40);
public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 60);
}

public static final double MAX_EXTENSION_INCHES = 54;
public static final double MIN_EXTENSION_INCHES = 1;

public static final double MIN_ANGLE_DEGREES = 0;
public static final double MIN_ANGLE_DEGREES = 3;
public static final double MAX_ANGLE_DEGREES = 109.25;

public static final int FALCON_CPR = 2048;
Expand All @@ -238,9 +237,9 @@ public static final class Setpoints {

public static final class Intake {

public static final double INTAKE_PERCENT = 1;
public static final double INTAKE_PERCENT = 0.7;

public static final double OUTTAKE_PERCENT = -1;
public static final double OUTTAKE_PERCENT = -0.7;

public static final double HOLD_PERCENT = 0.15;

Expand Down
24 changes: 11 additions & 13 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -288,11 +288,9 @@ private void configureButtonBindings() {
.off(jacob.rightTrigger())
.onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE));

jacobLayer
.off(jacob.x())
.onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF))
.onTrue(
new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED));
jacobLayer.off(jacob.x()).onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF));
// .onTrue(
// new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED));

// intake presets
// jacobLayer
Expand All @@ -319,7 +317,7 @@ private void configureButtonBindings() {

anthony
.povLeft()
.onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.ZERO))
.onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))
.onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF))
.onTrue(new SetZeroModeCommand(elevatorSubsystem));

Expand All @@ -331,13 +329,13 @@ private void configureButtonBindings() {
elevatorSubsystem,
() -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE));

// jacob
// .leftBumper()
// .onTrue(
// new GroundPickupCommand(
// intakeSubsystem,
// elevatorSubsystem,
// () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE));
jacob
.a()
.onTrue(
new GroundPickupCommand(
intakeSubsystem,
elevatorSubsystem,
() -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE));

jacobLayer
.off(jacob.povUp())
Expand Down
41 changes: 27 additions & 14 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,34 +13,43 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.Constants.Intake;
import com.playingwithfusion.TimeOfFlight;

public class IntakeSubsystem extends SubsystemBase {
private TalonFX intakeMotor;
private ShuffleboardTab shuffleboard = Shuffleboard.getTab("Intake Subsystem");
private Modes currentIntakeMode;
private LinearFilter filter;
private double filterOutput;
private double statorCurrentLimit;
private final TimeOfFlight coneToF, cubeToF;
// private final TimeOfFlight coneToF, cubeToF;

/** Creates a new IntakeSubsystem. */
public IntakeSubsystem() {
intakeMotor = new TalonFX(Constants.Intake.Ports.INTAKE_MOTOR_PORT);

currentIntakeMode = Modes.OFF;
intakeMotor.configFactoryDefault();
intakeMotor.clearStickyFaults();

intakeMotor.setNeutralMode(NeutralMode.Brake);

filter = LinearFilter.movingAverage(30);

coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT);
cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT);
currentIntakeMode = Modes.OFF;

filterOutput = 0.0;

statorCurrentLimit = 30;

// coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT);
// cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT);

shuffleboard.addDouble(
"Intake motor sensor position", () -> intakeMotor.getSelectedSensorPosition());
shuffleboard.addString("Current mode", () -> currentIntakeMode.toString());
shuffleboard.addDouble("coneToFInches", this::getConeToFInches);
shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches);
shuffleboard.addDouble("filter output", () -> filterOutput);
shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent);
// shuffleboard.addDouble("coneToFInches", this::getConeToFInches);
// shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches);
}

public enum Modes {
Expand All @@ -55,23 +64,26 @@ public void intakePeriodic(Modes mode) {
switch (mode) {
case INTAKE:
intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_PERCENT);
break;
case OUTTAKE:
intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_PERCENT);
break;
case HOLD:
intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_PERCENT);
break;
case OFF:
default:
intakeMotor.set(TalonFXControlMode.PercentOutput, 0);
}
}

private double getCubeToFInches() {
return cubeToF.getRange() / 25.4;
}
// private double getCubeToFInches() {
// return cubeToF.getRange() / 25.4;
// }

private double getConeToFInches() {
return coneToF.getRange() / 25.4;
}
// private double getConeToFInches() {
// return coneToF.getRange() / 25.4;
// }

public Modes getMode() {
return currentIntakeMode;
Expand All @@ -83,7 +95,8 @@ public void setMode(Modes mode) {

@Override
public void periodic() {
if (filter.calculate(intakeMotor.getStatorCurrent()) >= statorCurrentLimit) {
filterOutput = filter.calculate(intakeMotor.getStatorCurrent());
if (filterOutput >= statorCurrentLimit) {
currentIntakeMode = Modes.HOLD;
}

Expand Down

0 comments on commit 3f84e82

Please sign in to comment.