Skip to content

Commit

Permalink
Merge pull request #2 from team422/Intake
Browse files Browse the repository at this point in the history 
Intake
  • Loading branch information
@keckothedragon
keckothedragon authored May 29, 2024
2 parents fcc2a9d + a0e2803 commit efc29a1
Show file tree
Hide file tree
Showing 22 changed files with 556 additions and 97 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
*.wpilog
advantagekit/logs
logs
ctre_sim

# BlueJ files
*.ctxt
Expand Down
Binary file removed ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
View file
Binary file not shown.
62 changes: 41 additions & 21 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

package frc.robot;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -36,7 +37,7 @@ public static final class ShooterConstants {
// Shooter pivot
public static final double kPivotGearRatio = 46.722;

public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(55);
public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(70);
public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(15);
public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(33);

Expand Down Expand Up @@ -66,29 +67,17 @@ public static final class ShooterConstants {
new LoggedTunableNumber("Shooter Left Flywheel I", 0.0);
public static final LoggedTunableNumber kLeftFlywheelD =
new LoggedTunableNumber("Shooter Left Flywheel D", 0.0);
public static final double kLeftFlywheelVelocity = 10.0;
public static final double kLeftFlywheelAcceleration = 15.0;
public static final ProfiledPIDController kLeftFlywheelController =
new ProfiledPIDController(
kLeftFlywheelP.get(),
kLeftFlywheelI.get(),
kLeftFlywheelD.get(),
new Constraints(kLeftFlywheelVelocity, kLeftFlywheelAcceleration));
public static final PIDController kLeftFlywheelController =
new PIDController(kLeftFlywheelP.get(), kLeftFlywheelI.get(), kLeftFlywheelD.get());

public static final LoggedTunableNumber kRightFlywheelP =
new LoggedTunableNumber("Shooter Right Flywheel P", 1.0);
public static final LoggedTunableNumber kRightFlywheelI =
new LoggedTunableNumber("Shooter Right Flywheel I", 0.0);
public static final LoggedTunableNumber kRightFlywheelD =
new LoggedTunableNumber("Shooter Right Flywheel D", 0.0);
public static final double kRightFlywheelVelocity = 10.0;
public static final double kRightFlywheelAcceleration = 15.0;
public static final ProfiledPIDController kRightFlywheelController =
new ProfiledPIDController(
kRightFlywheelP.get(),
kRightFlywheelI.get(),
kRightFlywheelD.get(),
new Constraints(kRightFlywheelVelocity, kRightFlywheelAcceleration));
public static final PIDController kRightFlywheelController =
new PIDController(kRightFlywheelP.get(), kRightFlywheelI.get(), kRightFlywheelD.get());

public static final LoggedTunableNumber kRightFlywheelKs =
new LoggedTunableNumber("Shooter Right Flywheel kS", 0.38);
Expand All @@ -103,11 +92,40 @@ public static final class ShooterConstants {
new LoggedTunableNumber("Shooter Left Flywheel kV", 0.27);
public static final SimpleMotorFeedforward kLeftFlywheelFeedforward =
new SimpleMotorFeedforward(kLeftFlywheelKs.get(), kLeftFlywheelKv.get());
}

public static final class IntakeConstants {
public static final double kPivotGearRatio = 36.0 / 16;

public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(124);
public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(5);
public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(120);
public static final Rotation2d kDeployedAngle = Rotation2d.fromDegrees(15);

public static final LoggedTunableNumber kTestLeftFlywheelSpeed =
new LoggedTunableNumber("Left test flywheel speed", 0.0);
public static final LoggedTunableNumber kTestRightFlywheelSpeed =
new LoggedTunableNumber("Right test flywheel speed", 20.0);
// don't ask me why it's negative, just a hardware thing
public static final double kDeployRollerVoltage = -6.0;

public static final Rotation2d kPivotOffset = Rotation2d.fromDegrees(123);

public static final LoggedTunableNumber kPivotP =
new LoggedTunableNumber("Intake Pivot P", 3.0);
public static final LoggedTunableNumber kPivotI =
new LoggedTunableNumber("Intake Pivot I", 0.0);
public static final LoggedTunableNumber kPivotD =
new LoggedTunableNumber("Intake Pivot D", 0.0);
public static final double kPivotVelocity = 25.0;
public static final double kPivotAcceleration = 30.0;
public static final ProfiledPIDController kPivotController =
new ProfiledPIDController(
kPivotP.get(),
kPivotI.get(),
kPivotD.get(),
new Constraints(kPivotVelocity, kPivotAcceleration));
}

public static class IndexerConstants {
public static final double kFeederVoltage = 6.0;
public static final double kKickerVoltage = 6.0;
}

public static final class Ports {
Expand All @@ -130,5 +148,7 @@ public static final class Ports {
public static final int kClimbDown = 25;

public static final int kAmp = 54;

public static final int kLed = 9;
}
}
8 changes: 7 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,9 @@ public void robotPeriodic() {

/** This function is called once when the robot is disabled. */
@Override
public void disabledInit() {}
public void disabledInit() {
RobotState.getInstance().onDisable();
}

/** This function is called periodically when disabled. */
@Override
Expand All @@ -78,6 +80,8 @@ public void autonomousInit() {
if (autonomousCommand != null) {
autonomousCommand.schedule();
}

RobotState.getInstance().onEnableAutonomous();
}

/** This function is called periodically during autonomous. */
Expand All @@ -94,6 +98,8 @@ public void teleopInit() {
if (autonomousCommand != null) {
autonomousCommand.cancel();
}

RobotState.getInstance().onEnableTeleop();
}

/** This function is called periodically during operator control. */
Expand Down
60 changes: 50 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants.IndexerConstants;
import frc.robot.Constants.IntakeConstants;
import frc.robot.Constants.Ports;
import frc.robot.Constants.ShooterConstants;
import frc.robot.commands.DriveCommands;
Expand All @@ -29,6 +31,12 @@
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.indexer.IndexerIOFalcon;
import frc.robot.subsystems.indexer.IndexerIOSim;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.pivot.IntakePivotIONeo;
import frc.robot.subsystems.intake.pivot.IntakePivotIOSim;
import frc.robot.subsystems.intake.roller.RollerIOKraken;
import frc.robot.subsystems.intake.roller.RollerIOSim;
import frc.robot.subsystems.led.Led;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.flywheel.FlywheelIOKraken;
import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
Expand All @@ -48,6 +56,8 @@ public class RobotContainer {
private Drive m_drive;
private Shooter m_shooter;
private Indexer m_indexer;
private Intake m_intake;
private Led m_led;

// Controllers
private DriverControls m_driverControls;
Expand All @@ -68,6 +78,7 @@ private void configureSubsystems() {
new ModuleIOTalonFX(1),
new ModuleIOTalonFX(2),
new ModuleIOTalonFX(3));

m_shooter =
new Shooter(
new ShooterPivotIOFalcon(
Expand All @@ -80,13 +91,23 @@ private void configureSubsystems() {
ShooterConstants.kRightFlywheelController,
ShooterConstants.kLeftFlywheelFeedforward,
ShooterConstants.kRightFlywheelFeedforward);

m_indexer =
new Indexer(
new IndexerIOFalcon(
Ports.kKicker,
Ports.kFeeder,
Ports.kIndexerBeamBreakOne,
Ports.kIndexerBeamBreakTwo));

m_intake =
new Intake(
new RollerIOKraken(Ports.kIntakeRoller),
new IntakePivotIONeo(Ports.kIntakePivot),
IntakeConstants.kPivotController);

m_led = new Led(Ports.kLed, 20);

} else {
m_drive =
new Drive(
Expand All @@ -105,11 +126,17 @@ private void configureSubsystems() {
ShooterConstants.kLeftFlywheelFeedforward,
ShooterConstants.kRightFlywheelFeedforward);
m_indexer = new Indexer(new IndexerIOSim());

m_intake =
new Intake(new RollerIOSim(), new IntakePivotIOSim(), IntakeConstants.kPivotController);

m_led = new Led(Ports.kLed, 20);
}

m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
m_intake.setPivotAngle(IntakeConstants.kHomeAngle);

m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer);
m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer, m_intake, m_led);
}

private void configureControllers() {
Expand All @@ -130,31 +157,44 @@ private void configureButtonBindings() {
m_driverControls::getDriveRotation));

m_driverControls
.testShooter()
.revShooter()
.onTrue(
Commands.runOnce(
() -> {
m_shooter.setFlywheelVelocity(
ShooterConstants.kTestLeftFlywheelSpeed.get(),
ShooterConstants.kTestRightFlywheelSpeed.get());
m_shooter.setFlywheelVelocity(17.0, 20.0);
m_shooter.setPivotAngle(Rotation2d.fromDegrees(50));
// deploy intake so shooter pivot doesn't run into it
m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
}))
.onFalse(
Commands.runOnce(
() -> {
m_shooter.setFlywheelVelocity(0.0, 0.0);
m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
}));

m_driverControls
.testKicker()
.onTrue(m_indexer.runKicker(6.0))
.runKicker()
.onTrue(m_indexer.runKicker(IndexerConstants.kKickerVoltage))
.onFalse(m_indexer.runKicker(0.0));

m_driverControls
.testFeeder()
.onTrue(m_indexer.runFeeder(6.0))
.onFalse(m_indexer.runFeeder(0.0));
.deployIntake()
.onTrue(
Commands.runOnce(
() -> {
m_indexer.setFeederVoltage(IndexerConstants.kFeederVoltage);
m_intake.setRollerVoltage(IntakeConstants.kDeployRollerVoltage);
m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
}))
.onFalse(
Commands.runOnce(
() -> {
m_indexer.setFeederVoltage(0.0);
m_intake.setRollerVoltage(0.0);
m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
}));
}

public void updateRobotState() {
Expand Down
Loading

0 comments on commit efc29a1

Please sign in to comment.