Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,8 @@ public static class Drivetrain {

public static class Shooter {
public static final String shooterMotorName = "shooterMotor";
public static final String rampMotorName = "rampMotor";
public static final String rampMotorOneName = "rampMotorOne";
public static final String rampMotorTwoName = "rampMotorTwo";
public static final double shooterSpeed = 1;
public static final double rampSpeed = 0.5;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ public void initialize() {
drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp));
register(drivetrain);

shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorName);
shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorOneName, Constants.Shooter.rampMotorTwoName);
register(shooter);
coOp.getGamepadButton(GamepadKeys.Button.A).whileHeld(
new RunShooter(shooter, Constants.Shooter.shooterSpeed, Constants.Shooter.rampSpeed));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ public void initialize() {
);
drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp));
register(drivetrain);
shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorName);
shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorOneName, Constants.Shooter.rampMotorTwoName);
register(shooter);
intake = new Intake(hardwareMap, Constants.Intake.intakeMotorName);
register(intake);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ public RunShooter(Shooter _subsystem, double _shooterSpeed, double _rampSpeed) {

@Override
public void initialize() {
subsystem.run(shooterSpeed, rampSpeed);
subsystem.runBase(shooterSpeed, rampSpeed);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,25 +5,29 @@
import com.qualcomm.robotcore.hardware.HardwareMap;

public class Shooter extends SubsystemBase {
private final Motor shooterMotor, rampMotor;
private final Motor shooterMotor, rampMotorOne, rampMotorTwo;

public Shooter(HardwareMap map, String shooterMotorName, String rampMotorName) {
public Shooter(HardwareMap map, String shooterMotorName, String rampMotorOneName, String rampMotorTwoName) {
shooterMotor = new Motor(map, shooterMotorName);
shooterMotor.setRunMode(Motor.RunMode.RawPower);
rampMotor = new Motor(map, rampMotorName);
rampMotor.setRunMode(Motor.RunMode.RawPower);
rampMotorOne = new Motor(map, rampMotorOneName);
rampMotorOne.setRunMode(Motor.RunMode.RawPower);
rampMotorTwo = new Motor(map, rampMotorTwoName);
rampMotorTwo.setRunMode(Motor.RunMode.RawPower);
}

public void run(double shooterSpeed, double rampSpeed) {
public void runBase(double shooterSpeed, double rampSpeedOne) {
// Clamp speed to valid motor range [-1.0, 1.0]
double clampedSpeed = Math.max(-1.0, Math.min(1.0, shooterSpeed));
double _rampSpeed = Math.max(-1.0, Math.min(1.0, rampSpeed));
shooterMotor.set(clampedSpeed);
rampMotor.set(_rampSpeed);
double baseSpeed = Math.max(-1.0, Math.min(1.0, shooterSpeed));
double _rampSpeed = Math.max(-0.5, Math.min(0.5, rampSpeedOne));
shooterMotor.set(baseSpeed);
rampMotorOne.set(_rampSpeed);
rampMotorTwo.set(_rampSpeed * -1.0);
}

public void stop() {
shooterMotor.stopMotor();
rampMotor.stopMotor();
rampMotorOne.stopMotor();
rampMotorTwo.stopMotor();
}
}
Loading