Skip to content

Commit

Permalink
bump motor
Browse files Browse the repository at this point in the history
  • Loading branch information
xorbotz committed Jan 24, 2024
1 parent 87ccf2e commit 9b281d6
Show file tree
Hide file tree
Showing 3 changed files with 17 additions and 10 deletions.
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -46,14 +46,15 @@ public static final class Shooter {

public static final int shooterID_1 = 22;
public static final int shooterID_2 = 25;
public static final int bumpID = 26;


/* Swerve Current Limiting */
public static final int angleContinuousCurrentLimit = 10;
public static final int driveContinuousCurrentLimit = 10;
public static final double voltageComp = 12.0;

public static final int speakerStrength = -1000;
public static final int speakerStrength = 5900;
public static final int ampStrength = 2560;
public static final int receive = 100;

Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,9 @@ public RobotContainer() {
ShooterSubsystem));
}
private void configureButtonBindings() {
// new JoystickButton(CONTROLLER, 5)
//.whileTrue(
// new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem));
new JoystickButton(CONTROLLER, 5)
.whileTrue(
new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem));
}

public Command getAutonomousCommand() {
Expand Down
18 changes: 12 additions & 6 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
public class ShooterSubsystem extends SubsystemBase {
private final CANSparkMax shooterMotor_1;
private final CANSparkMax shooterMotor_2;
private final CANSparkMax bumpMotor;

private final RelativeEncoder shooterRelativeEncoder_1;
private final RelativeEncoder shooterRelativeEncoder_2;
Expand All @@ -32,6 +33,8 @@ public ShooterSubsystem() {
shooterMotor_2 = new CANSparkMax(Constants.Shooter.shooterID_2, MotorType.kBrushless);
shooterRelativeEncoder_2 = shooterMotor_2.getEncoder();
shooterPidController_2 = shooterMotor_2.getPIDController();

bumpMotor = new CANSparkMax(Constants.Shooter.shooterID_2, MotorType.kBrushless);
configShooterMotors();
}

Expand Down Expand Up @@ -66,6 +69,13 @@ private void configShooterMotors() {
shooterMotor_2.enableVoltageCompensation(Constants.Shooter.voltageComp);
shooterMotor_2.burnFlash();
shooterRelativeEncoder_2.setPosition(0.0);

bumpMotor.restoreFactoryDefaults();
CANSparkMaxUtil.setCANSparkMaxBusUsage(bumpMotor, Usage.kAll);
bumpMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit);
bumpMotor.setInverted(!Constants.Shooter.driveInvert);
bumpMotor.setIdleMode(Constants.Shooter.driveNeutralMode);
bumpMotor.burnFlash();
}

public void speakerShot(boolean shoot) {
Expand Down Expand Up @@ -108,12 +118,8 @@ public void ampShot(boolean shoot) {
}
public void receive(boolean shoot) {
if (shoot) {
shooterPidController_1.setReference(
Constants.Shooter.receive,
ControlType.kVelocity);
shooterPidController_2.setReference(
Constants.Shooter.receive,
ControlType.kVelocity);
shooterMotor_1.set(-.15);
shooterMotor_2.set(-.15);
}
else {
// shooterPidController_1.setReference(
Expand Down

0 comments on commit 9b281d6

Please sign in to comment.