Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

initial thing with enum #39

Merged
merged 1 commit into from
Oct 5, 2024
Merged

initial thing with enum #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 21 additions & 8 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,28 +49,41 @@ public static class OperatorConstants {
public static final int BIG_BUTTON_PORT = 14;
}

public static final class Peter {
public static final class Pooer {
public static final int INTAKE_MOTOR_PORT = 33;
// public static final double SHOOTER_SPEED = 3; // Shooter gear ratio: 15:12
public static final int NOTE_DETECTOR_PORT = 1;
public static final int PRE_SHOOTER_PORT = 32;
public static final int SHOOTER_PORT_RIGHT = 30;
public static final int SHOOTER_PORT_LEFT = 31;

public static final double INTAKE_WHEEL_SPEED_RPS = 200; // Intake gear ratio: 2:1
public static final double ROTATIONS_TO_SHOOTER = 300; // Preshooter gear ratio: 4:1
public static final double SHOOT_WHEEL_SPEED_RPS =
3500.0 / 60.0; // TODO: Add right and left motor RPS, and change it to not max
public static final double PRESHOOTER_WHEEL_VOLTAGE = 9;
public static final String CANBUS_NAME = "rio";

public static final double INTAKE_GEAR_RATIO = 2;
public static final double PRESHOOTER_GEAR_RATIO = 4;
public static final double SHOOTER_WHEELS_GEAR_RATIOS = 24d / 18d;

public static final double SHOOTER_STATOR_CURRENT_LIMIT_AMPS = 40.0;
public static final double PRESHOOTER_STATOR_CURRENT_LIMIT_AMPS = 25.0;
public static final double INTAKE_STATOR_CURRENT_LIMIT_AMPS = 50.0;

public static final ShooterType SHOOTER = ShooterType.PETER;

public static enum ShooterType {
PETER(30, 31, 4500.0 / 60.0, 12d / 15d, 40.0),
PIPER(30, 31, 3500.0 / 60.0, 24d / 18d, 40.0);
public final int PORT_1, PORT_2;
public final double SPEED_RPS;
public final double GEAR_RATIO;
public final double STATOR_CURRENT_LIMIT_AMPS;

ShooterType(
int port1, int port2, double speedRPS, double gearRatio, double statorCurrentLimitAmp) {
PORT_1 = port1;
PORT_2 = port2;
SPEED_RPS = speedRPS;
GEAR_RATIO = gearRatio;
STATOR_CURRENT_LIMIT_AMPS = statorCurrentLimitAmp;
}
}
}

public static final class Arm {
Expand Down
75 changes: 26 additions & 49 deletions src/main/java/frc/robot/subsystems/PeterSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package frc.robot.subsystems;

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotionMagicConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
Expand All @@ -21,37 +20,30 @@ public class PeterSubsystem extends SubsystemBase {
private DigitalInput noteSensor;
private TalonFX shooterMotorUp, shooterMotorDown;
private TalonFX preShooterMotor, intakeMotor;
private StatusSignal<Double> preShooterPosition;

private MotionMagicConfigs mmcPreShooter;

public PeterSubsystem() {
// Initalize shooter
// Follower f = new Follower(Constants.Intake.SHOOTER_PORT_LEFT, false );
shooterMotorDown = new TalonFX(Constants.Peter.SHOOTER_PORT_LEFT, Constants.Peter.CANBUS_NAME);
shooterMotorUp = new TalonFX(Constants.Peter.SHOOTER_PORT_RIGHT, Constants.Peter.CANBUS_NAME);
shooterMotorDown = new TalonFX(Constants.Pooer.SHOOTER.PORT_1, Constants.Pooer.CANBUS_NAME);
shooterMotorUp = new TalonFX(Constants.Pooer.SHOOTER.PORT_2, Constants.Pooer.CANBUS_NAME);
shooterMotorUp.setInverted(true);
// shooterMotorRight.setControl(f);
Slot0Configs s0c =
new Slot0Configs().withKP(0.001).withKI(0).withKD(0).withKG(0).withKV(0.2).withKA(0);
CurrentLimitsConfigs clc =
new CurrentLimitsConfigs()
.withStatorCurrentLimitEnable(true)
.withStatorCurrentLimit(Constants.Pooer.SHOOTER.STATOR_CURRENT_LIMIT_AMPS);

shooterMotorUp.getConfigurator().apply(s0c);
shooterMotorDown.getConfigurator().apply(s0c);
shooterMotorDown
.getConfigurator()
.apply(
new CurrentLimitsConfigs()
.withStatorCurrentLimitEnable(true)
.withStatorCurrentLimit(Constants.Peter.SHOOTER_STATOR_CURRENT_LIMIT_AMPS));
shooterMotorUp
.getConfigurator()
.apply(
new CurrentLimitsConfigs()
.withStatorCurrentLimitEnable(true)
.withStatorCurrentLimit(Constants.Peter.SHOOTER_STATOR_CURRENT_LIMIT_AMPS));
shooterMotorUp.getConfigurator().apply(clc);
shooterMotorDown.getConfigurator().apply(clc);

// Preshooter
preShooterMotor = new TalonFX(Constants.Peter.PRE_SHOOTER_PORT, Constants.Peter.CANBUS_NAME);
preShooterMotor = new TalonFX(Constants.Pooer.PRE_SHOOTER_PORT, Constants.Pooer.CANBUS_NAME);
mmcPreShooter = new MotionMagicConfigs();
mmcPreShooter.MotionMagicCruiseVelocity = 80;
mmcPreShooter.MotionMagicAcceleration = 160;
Expand All @@ -65,23 +57,21 @@ public PeterSubsystem() {
.apply(
new CurrentLimitsConfigs()
.withStatorCurrentLimitEnable(true)
.withStatorCurrentLimit(Constants.Peter.PRESHOOTER_STATOR_CURRENT_LIMIT_AMPS));
.withStatorCurrentLimit(Constants.Pooer.PRESHOOTER_STATOR_CURRENT_LIMIT_AMPS));
// Intake
Slot0Configs intakePid =
new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKG(0).withKV(0).withKA(0);

intakeMotor = new TalonFX(Constants.Peter.INTAKE_MOTOR_PORT, Constants.Peter.CANBUS_NAME);
intakeMotor = new TalonFX(Constants.Pooer.INTAKE_MOTOR_PORT, Constants.Pooer.CANBUS_NAME);
intakeMotor.getConfigurator().apply(intakePid);
intakeMotor
.getConfigurator()
.apply(
new CurrentLimitsConfigs()
.withStatorCurrentLimitEnable(true)
.withStatorCurrentLimit(Constants.Peter.INTAKE_STATOR_CURRENT_LIMIT_AMPS));
.withStatorCurrentLimit(Constants.Pooer.INTAKE_STATOR_CURRENT_LIMIT_AMPS));
intakeMotor.setInverted(true);
noteSensor = new DigitalInput(Constants.Peter.NOTE_DETECTOR_PORT);

preShooterPosition = preShooterMotor.getPosition();
noteSensor = new DigitalInput(Constants.Pooer.NOTE_DETECTOR_PORT);
}

public static PeterSubsystem getInstance() {
Expand All @@ -93,12 +83,12 @@ public static PeterSubsystem getInstance() {

// INTAKE FUNCTIONS:
public void spinUpIntake() {
runIntakeAtRPS(Constants.Peter.INTAKE_WHEEL_SPEED_RPS);
runIntakeAtRPS(Constants.Pooer.INTAKE_WHEEL_SPEED_RPS);
}

private void runIntakeAtRPS(double speed) {
VelocityVoltage m_velocityControl =
new VelocityVoltage(speed * Constants.Peter.INTAKE_GEAR_RATIO);
new VelocityVoltage(speed * Constants.Pooer.INTAKE_GEAR_RATIO);
m_velocityControl.withFeedForward(0.1);
intakeMotor.setControl(m_velocityControl);
}
Expand All @@ -110,15 +100,15 @@ public void stopIntake() {
// SHOOTER FUNCTIONS:
private void runRightShooterAtRPS(double speed) {
VelocityVoltage m_velocityControl =
new VelocityVoltage(speed * Constants.Peter.SHOOTER_WHEELS_GEAR_RATIOS);
new VelocityVoltage(speed * Constants.Pooer.SHOOTER.GEAR_RATIO);
m_velocityControl.withFeedForward(0.1);
shooterMotorUp.setControl(m_velocityControl);
shooterMotorUp.getVelocity();
}

private void runLeftShooterAtRPS(double speed) {
VelocityVoltage m_velocityControl =
new VelocityVoltage(speed * Constants.Peter.SHOOTER_WHEELS_GEAR_RATIOS);
new VelocityVoltage(speed * Constants.Pooer.SHOOTER.GEAR_RATIO);
m_velocityControl.withFeedForward(0.1);
shooterMotorDown.setControl(m_velocityControl);
}
Expand All @@ -134,11 +124,11 @@ public void stopShooter(boolean forceStop) {
}

public void spinRightShooter() {
runRightShooterAtRPS(Constants.Peter.SHOOT_WHEEL_SPEED_RPS);
runRightShooterAtRPS(Constants.Pooer.SHOOTER.SPEED_RPS);
}

public void spinLeftShooter() {
runLeftShooterAtRPS(Constants.Peter.SHOOT_WHEEL_SPEED_RPS);
runLeftShooterAtRPS(Constants.Pooer.SHOOTER.SPEED_RPS);
}

public void stopRightShooter() {
Expand All @@ -154,13 +144,13 @@ public void resetPreshooterPosition() {
}

public void reversePreshooterRotations(double count) {
preShooterMotor.setControl(new PositionVoltage(-count * Constants.Peter.PRESHOOTER_GEAR_RATIO));
preShooterMotor.setControl(new PositionVoltage(-count * Constants.Pooer.PRESHOOTER_GEAR_RATIO));
}

public boolean isBackedUp(double count) {
return Math.abs(
preShooterMotor.getPosition().getValueAsDouble()
- (-count * Constants.Peter.PRESHOOTER_GEAR_RATIO))
- (-count * Constants.Pooer.PRESHOOTER_GEAR_RATIO))
< 0.1;
}

Expand All @@ -172,18 +162,10 @@ public void reverseMechanism() {
}

public boolean isShooterReady() {
if (Math.abs(shooterMotorDown.getVelocity().getValue() - Constants.Peter.SHOOT_WHEEL_SPEED_RPS)
< 0.001) {
return true;
}
if (Math.abs(
return Math.abs(
shooterMotorDown.getVelocity().getValueAsDouble()
- (Constants.Peter.SHOOT_WHEEL_SPEED_RPS
* Constants.Peter.SHOOTER_WHEELS_GEAR_RATIOS))
< 10) {
return true;
}
return false;
- (Constants.Pooer.SHOOTER.SPEED_RPS * Constants.Pooer.SHOOTER.GEAR_RATIO))
< 10;
}

/* private void runShooterAtRPS(double speed) {
Expand All @@ -204,11 +186,11 @@ public boolean notePresent() {
// PRE-SHOOTER FUNCTIONS:

public void spinUpPreShooter() {
runPreShooterAtRPS(Constants.Peter.ROTATIONS_TO_SHOOTER);
runPreShooterAtRPS(Constants.Pooer.ROTATIONS_TO_SHOOTER);
}

private void runPreShooterAtRPS(double speed) {
VoltageOut velocityControl = new VoltageOut(Constants.Peter.PRESHOOTER_WHEEL_VOLTAGE);
VoltageOut velocityControl = new VoltageOut(Constants.Pooer.PRESHOOTER_WHEEL_VOLTAGE);
preShooterMotor.setControl(velocityControl);

// VelocityVoltage m_velocityControl =
Expand Down Expand Up @@ -278,9 +260,4 @@ public void periodicSignalLogger() {
SignalLogger.writeDouble(
"Shooter up current", shooterMotorUp.getStatorCurrent().getValueAsDouble());
}

public void runShooter(int i) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'runShooter'");
}
}
Loading