Skip to content

Commit

Permalink
semi-working intake code
Browse files Browse the repository at this point in the history
  • Loading branch information
ozziexyz committed Oct 22, 2024
1 parent d82bb90 commit 92bf9fe
Show file tree
Hide file tree
Showing 6 changed files with 35 additions and 71 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final Mode currentMode = Mode.SIM;
public static final Mode currentMode = Mode.REAL;

public static enum Mode {
/** Running on a real robot. */
Expand Down
77 changes: 22 additions & 55 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.commands.DriveCommands;
Expand All @@ -29,12 +28,14 @@
import frc.robot.subsystems.drive.ModuleIOSim;
import frc.robot.subsystems.drive.ModuleIOTalonFX;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.IntakeIO;
import frc.robot.subsystems.intake.IntakeIOReal;
import frc.robot.subsystems.intake.IntakeIOSim;
import frc.robot.subsystems.pivot.Pivot;
import frc.robot.subsystems.pivot.PivotIO;
import frc.robot.subsystems.pivot.PivotIOReal;
import frc.robot.subsystems.pivot.PivotIOSim;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -45,15 +46,14 @@
public class RobotContainer {
// Subsystems
private final Drive drive;
private final Intake intake;
private final Pivot pivot;

// Controller
private final CommandXboxController controller = new CommandXboxController(0);

// Dashboard inputs
private final LoggedDashboardChooser<Command> autoChooser;
private final LoggedDashboardNumber flywheelSpeedInput =
new LoggedDashboardNumber("Flywheel Speed", 1500.0);

/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
Expand All @@ -67,13 +67,8 @@ public RobotContainer() {
new ModuleIOTalonFX(1),
new ModuleIOTalonFX(2),
new ModuleIOTalonFX(3));
// drive = new Drive(
// new GyroIOPigeon2(),
// new ModuleIOTalonFX(0),
// new ModuleIOTalonFX(1),
// new ModuleIOTalonFX(2),
// new ModuleIOTalonFX(3));
// flywheel = new Flywheel(new FlywheelIOTalonFX());
intake = Intake.initialize(new IntakeIOReal());
pivot = Pivot.initialize(new PivotIOReal());
break;

case SIM:
Expand All @@ -85,8 +80,8 @@ public RobotContainer() {
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim());
intake = Intake.initialize(new IntakeIOSim());
pivot = Pivot.initialize(new PivotIOSim());
Intake.initialize(new IntakeIOSim());

break;

Expand All @@ -99,19 +94,12 @@ public RobotContainer() {
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {});
intake = Intake.initialize(new IntakeIO() {});
pivot = Pivot.initialize(new PivotIO() {});
// change?
pivot = Pivot.initialize(new PivotIOSim());
break;
}

// This can go away

// Set up auto routines
// NamedCommands.registerCommand(
// "Run Flywheel",
// Commands.startEnd(
// () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)
// .withTimeout(5.0));
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());

// Set up SysId routines
Expand All @@ -126,19 +114,6 @@ public RobotContainer() {
autoChooser.addOption(
"Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));

// autoChooser.addOption(
// "Flywheel SysId (Quasistatic Forward)",
// flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Flywheel SysId (Quasistatic Reverse)",
// flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// autoChooser.addOption(
// "Flywheel SysId (Dynamic Forward)",
// flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Flywheel SysId (Dynamic Reverse)",
// flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));

// Configure the button bindings
configureButtonBindings();
}
Expand All @@ -157,35 +132,27 @@ private void configureButtonBindings() {
() -> -controller.getLeftX(),
() -> -controller.getRightX()));

// controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));

controller.leftBumper().whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
controller.rightBumper()
.onTrue(Commands.run(() -> intake.setIntakeSpeed(0.75)))
.onFalse(Commands.run(() -> intake.setIntakeSpeed(0)));

controller.leftTrigger().whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));

controller.rightBumper().whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kForward));

controller.rightTrigger().whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
controller.leftBumper()
.onTrue(Commands.run(() -> intake.setIntakeSpeed(-0.75)))
.onFalse(Commands.run(() -> intake.setIntakeSpeed(0)));

// controller
// .b()
// .onTrue(
// Commands.runOnce(
// () ->
// drive.setPose(
// new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
// drive)
// .ignoringDisable(true));
// .leftBumper()
// .whileTrue(Commands.startEnd(() -> pivot.runVoltage(3), () -> pivot.runVoltage(0),
// pivot));

// controller
// .a()
// .whileTrue(
// Commands.startEnd(
// () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
// .rightBumper()
// .whileTrue(Commands.startEnd(() -> pivot.runVoltage(-3), () -> pivot.runVoltage(0),
// pivot));

controller.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive));
}


/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/subsystems/intake/Intake.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;

Expand Down Expand Up @@ -50,8 +51,8 @@ public boolean noteNotInIntake() { // ..
}

public Command intake() {
return startEnd(() -> setIntakeSpeed(1), () -> setIntakeSpeed(0))
.onlyWhile(() -> noteNotInIntake());
return Commands.startEnd(() -> io.setMotorSpeed(1), () -> io.setMotorSpeed(0));
// .onlyWhile(() -> noteNotInIntake());
// return (startEnd(() -> setIntakeSpeed(1), () -> setIntakeSpeed(0)).onlyWhile(() ->
// noteNotInIntake())).alongWith(startEnd(() -> setIntakeSpeed(0.1), () ->
// setIntakeSpeed(0)).onlyIf(() -> noteNotInIntake() == true).onlyWhile(() -> noteNotInIntake()
Expand All @@ -63,7 +64,7 @@ public Command spike() {
}
*/
public Command reverse() {
return startEnd(() -> setIntakeSpeed(-1), () -> setIntakeSpeed(0));
return startEnd(() -> io.setMotorSpeed(-1), () -> io.setMotorSpeed(0));
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,6 @@

public class IntakeConstants {
public static final int CanID = 5; // ..
public static final int currentLimit = 40; // vortex
public static final int currentLimit = 60; // vortex
public static final int current = 10; // ..
}
13 changes: 4 additions & 9 deletions src/main/java/frc/robot/subsystems/pivot/Pivot.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.subsystems.intake.Intake;

import org.littletonrobotics.junction.Logger;

public class Pivot extends SubsystemBase {
Expand Down Expand Up @@ -34,9 +33,6 @@ private Pivot(PivotIO io) {
ffModel = new SimpleMotorFeedforward(0.0, 0.0);
break;
}

setDefaultCommand(Pivot.getInstance().currDetPivot());

}

public static Pivot getInstance() {
Expand All @@ -52,11 +48,10 @@ public static Pivot initialize(PivotIO io) {

public Command currDetPivot() {
return Commands.sequence(
Commands.waitUntil(() -> Intake.getInstance().noteNotInIntake()),
Commands.runOnce(() -> Pivot.getInstance().setGoal(Math.PI / 6)),
Commands.waitUntil(() -> !Intake.getInstance().noteNotInIntake()),
Commands.runOnce(() -> Pivot.getInstance().setGoal(0))
);
Commands.waitUntil(() -> Intake.getInstance().noteNotInIntake()),
Commands.runOnce(() -> Pivot.getInstance().setGoal(Math.PI / 6)),
Commands.waitUntil(() -> !Intake.getInstance().noteNotInIntake()),
Commands.runOnce(() -> Pivot.getInstance().setGoal(0)));
}

@Override
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ public class PivotIOReal implements PivotIO {
private CANSparkFlex pivotMotor = new CANSparkFlex(11, MotorType.kBrushless);
private RelativeEncoder pivotEncoderRight = pivotMotor.getEncoder();
private PIDController pid = new PIDController(1.0, 0.0, 0.0);
private double gearing = 36;

private boolean closedLoop = false;
private double ffVolts = 0.0;
Expand All @@ -29,8 +30,8 @@ public PivotIOReal() {

@Override
public void updateInputs(PivotIOInputs inputs) {
inputs.positionRad = pivotEncoderRight.getPosition() * 2 * Math.PI;
inputs.velocityRadPerSec = pivotEncoderRight.getVelocity() * 2 * Math.PI / 60;
inputs.positionRad = (pivotEncoderRight.getPosition() * 2 * Math.PI) / gearing;
inputs.velocityRadPerSec = (pivotEncoderRight.getVelocity() * 2 * Math.PI / 60) / gearing;
inputs.appliedVolts = appliedVolts;
inputs.goalAngle = pid.getSetpoint();

Expand Down

0 comments on commit 92bf9fe

Please sign in to comment.