Skip to content

Commit

Permalink
Intake fully tested, rebound controls
Browse files Browse the repository at this point in the history
  • Loading branch information
@keckothedragon
keckothedragon committed May 28, 2024
1 parent 7416e46 commit 8b532f9
Show file tree
Hide file tree
Showing 11 changed files with 105 additions and 106 deletions.
52 changes: 21 additions & 31 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,38 +92,39 @@ 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 LoggedTunableNumber kTestLeftFlywheelSpeed =
new LoggedTunableNumber("Left test flywheel speed", 0.0);
public static final LoggedTunableNumber kTestRightFlywheelSpeed =
new LoggedTunableNumber("Right test flywheel speed", 20.0);
}

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

// TODO: untuned values, fix later
public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(90);
public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(0);
public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(85);
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 double kDeployRollerVoltage = -6.0;

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

public static final LoggedTunableNumber kPivotP =
new LoggedTunableNumber("Intake Pivot P", 1.0);
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 = 10.0;
public static final double kPivotAcceleration = 15.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 final LoggedTunableNumber kTestPivotAngle =
new LoggedTunableNumber("Test Intake Pivot Angle", 15.0);
public static class IndexerConstants {
public static final double kFeederVoltage = 6.0;
public static final double kKickerVoltage = 6.0;
}

public static final class Ports {
Expand Down
33 changes: 15 additions & 18 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,7 @@
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;
Expand Down Expand Up @@ -132,9 +133,9 @@ private void configureSubsystems() {
private void configureControllers() {
// usually use ps5 on actual robot but xbox is better for keyboard testing
if (RobotBase.isReal()) {
m_driverControls = new DriverControlsPS5(1);
} else {
m_driverControls = new DriverControlsXbox(1);
} else {
m_driverControls = new DriverControlsPS5(1);
}
}

Expand All @@ -147,45 +148,41 @@ 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));
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));


m_driverControls
.testIntake()
.deployIntake()
.onTrue(
Commands.runOnce(
() -> {
m_intake.setRollerVoltage(8);
m_intake.setPivotAngle(Rotation2d.fromDegrees(IntakeConstants.kTestPivotAngle.get()));
m_indexer.setFeederVoltage(IndexerConstants.kFeederVoltage);
m_intake.setRollerVoltage(-4.0);
m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
}))
.onFalse(
Commands.runOnce(
() -> {
m_intake.setRollerVoltage(0);
m_indexer.setFeederVoltage(0.0);
m_intake.setRollerVoltage(0.0);
m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
}));
}
Expand Down
8 changes: 3 additions & 5 deletions src/main/java/frc/robot/oi/DriverControls.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,9 @@ public interface DriverControls {

public double getDriveRotation();

public Trigger testShooter();
public Trigger revShooter();

public Trigger testKicker();
public Trigger runKicker();

public Trigger testFeeder();

public Trigger testIntake();
public Trigger deployIntake();
}
17 changes: 6 additions & 11 deletions src/main/java/frc/robot/oi/DriverControlsPS5.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,36 +12,31 @@ public DriverControlsPS5(int port) {

@Override
public double getDriveForward() {
return m_controller.getLeftY();
return -m_controller.getLeftY();
}

@Override
public double getDriveLeft() {
return m_controller.getLeftX();
return -m_controller.getLeftX();
}

@Override
public double getDriveRotation() {
return m_controller.getRightX();
return -m_controller.getRightX();
}

@Override
public Trigger testShooter() {
public Trigger revShooter() {
return m_controller.L2();
}

@Override
public Trigger testKicker() {
public Trigger runKicker() {
return m_controller.R1();
}

@Override
public Trigger testFeeder() {
return m_controller.L1();
}

@Override
public Trigger testIntake() {
public Trigger deployIntake() {
return m_controller.R2();
}
}
22 changes: 9 additions & 13 deletions src/main/java/frc/robot/oi/DriverControlsXbox.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,35 +12,31 @@ public DriverControlsXbox(int port) {

@Override
public double getDriveForward() {
return m_controller.getLeftY();
return -m_controller.getLeftY();
}

@Override
public double getDriveLeft() {
return m_controller.getLeftX();
return -m_controller.getLeftX();
}

@Override
public double getDriveRotation() {
return m_controller.getRightX();
return -m_controller.getRightX();
}

@Override
public Trigger testShooter() {
return m_controller.button(1);
public Trigger revShooter() {
return m_controller.leftTrigger();
}

@Override
public Trigger testKicker() {
return m_controller.button(2);
public Trigger runKicker() {
return m_controller.rightBumper();
}

@Override
public Trigger testFeeder() {
return m_controller.button(3);
}

public Trigger testIntake() {
return m_controller.button(4);
public Trigger deployIntake() {
return m_controller.rightTrigger();
}
}
28 changes: 14 additions & 14 deletions src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,28 +69,28 @@ public class ModuleIOTalonFX implements ModuleIO {
public ModuleIOTalonFX(int index) {
switch (index) {
case 0:
driveTalon = new TalonFX(1, "Drivetrain");
turnTalon = new TalonFX(2, "Drivetrain");
cancoder = new CANcoder(3, "Drivetrain");
absoluteEncoderOffset = new Rotation2d(1.226); // MUST BE CALIBRATED
driveTalon = new TalonFX(10, "Drivetrain");
turnTalon = new TalonFX(11, "Drivetrain");
cancoder = new CANcoder(12, "Drivetrain");
absoluteEncoderOffset = new Rotation2d(2.827); // MUST BE CALIBRATED
break;
case 1:
driveTalon = new TalonFX(4, "Drivetrain");
turnTalon = new TalonFX(5, "Drivetrain");
cancoder = new CANcoder(6, "Drivetrain");
absoluteEncoderOffset = new Rotation2d(0.175); // MUST BE CALIBRATED
break;
case 2:
driveTalon = new TalonFX(7, "Drivetrain");
turnTalon = new TalonFX(8, "Drivetrain");
cancoder = new CANcoder(9, "Drivetrain");
absoluteEncoderOffset = new Rotation2d(-2.945); // MUST BE CALIBRATED
break;
case 2:
driveTalon = new TalonFX(4, "Drivetrain");
turnTalon = new TalonFX(5, "Drivetrain");
cancoder = new CANcoder(6, "Drivetrain");
absoluteEncoderOffset = new Rotation2d(0.175); // MUST BE CALIBRATED
break;
case 3:
driveTalon = new TalonFX(10, "Drivetrain");
turnTalon = new TalonFX(11, "Drivetrain");
cancoder = new CANcoder(12, "Drivetrain");
absoluteEncoderOffset = new Rotation2d(2.827); // MUST BE CALIBRATED
driveTalon = new TalonFX(1, "Drivetrain");
turnTalon = new TalonFX(2, "Drivetrain");
cancoder = new CANcoder(3, "Drivetrain");
absoluteEncoderOffset = new Rotation2d(1.226); // MUST BE CALIBRATED
break;
default:
throw new RuntimeException("Invalid module index");
Expand Down
8 changes: 8 additions & 0 deletions src/main/java/frc/robot/subsystems/indexer/Indexer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,14 @@ public void periodic() {
Logger.processInputs("Indexer", m_inputs);
}

public void setKickerVoltage(double voltage) {
m_io.setKickerVoltage(voltage);
}

public void setFeederVoltage(double voltage) {
m_io.setFeederVoltage(voltage);
}

public Command runKicker(double voltage) {
return Commands.runOnce(() -> m_io.setKickerVoltage(voltage));
}
Expand Down
13 changes: 13 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.lib.utils.LoggedTunableNumber;
import frc.robot.Constants.IntakeConstants;
import frc.robot.subsystems.intake.pivot.IntakePivotIO;
import frc.robot.subsystems.intake.pivot.IntakePivotInputsAutoLogged;
import frc.robot.subsystems.intake.roller.RollerIO;
Expand All @@ -32,6 +34,17 @@ public Intake(RollerIO io, IntakePivotIO pivotIO, ProfiledPIDController pivotCon

@Override
public void periodic() {
LoggedTunableNumber.ifChanged(
hashCode(),
() -> {
m_pivotController.setP(IntakeConstants.kPivotP.get());
m_pivotController.setI(IntakeConstants.kPivotI.get());
m_pivotController.setD(IntakeConstants.kPivotD.get());
},
IntakeConstants.kPivotP,
IntakeConstants.kPivotI,
IntakeConstants.kPivotD);

m_pivotIO.updateInputs(m_pivotInputs);
m_rollerIO.updateInputs(m_rollerInputs);

Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIONeo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,13 @@ public IntakePivotIONeo(int port) {
m_motor = new CANSparkMax(port, CANSparkMax.MotorType.kBrushless);
m_encoder = m_motor.getAbsoluteEncoder();
m_encoder.setPositionConversionFactor(IntakeConstants.kPivotGearRatio);

m_encoder.setZeroOffset(IntakeConstants.kPivotOffset.getRadians());
}

@Override
public void updateInputs(IntakePivotInputs inputs) {
inputs.curAngle = Units.rotationsToRadians(m_encoder.getPosition());
inputs.curAngle = m_encoder.getPosition();
inputs.curVelocity =
Units.rotationsPerMinuteToRadiansPerSecond(m_motor.getEncoder().getVelocity());
inputs.curVoltage = m_motor.getAppliedOutput();
Expand Down
Loading

0 comments on commit 8b532f9

Please sign in to comment.