85 lines
2.7 KiB
Java
85 lines
2.7 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
|
|
|
|
import edu.wpi.first.math.controller.BangBangController;
|
|
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
|
import edu.wpi.first.wpilibj.Encoder;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.constants.ShooterConstants;
|
|
|
|
public class Shooter extends SubsystemBase {
|
|
private WPI_VictorSPX frontMotor;
|
|
private WPI_VictorSPX rearMotor;
|
|
|
|
private BangBangController bangBangController;
|
|
private SimpleMotorFeedforward motorFeedforward;
|
|
|
|
private Encoder frontMotorEncoder;
|
|
private Encoder rearMotorEncoder;
|
|
|
|
public Shooter() {
|
|
frontMotor = new WPI_VictorSPX(ShooterConstants.kFrontMotorCAN);
|
|
rearMotor = new WPI_VictorSPX(ShooterConstants.kRearMotorCAN);
|
|
|
|
rearMotor.follow(frontMotor);
|
|
|
|
bangBangController = new BangBangController(ShooterConstants.kBangBangTolerance);
|
|
|
|
motorFeedforward = new SimpleMotorFeedforward(
|
|
ShooterConstants.kFFS,
|
|
ShooterConstants.kFFG,
|
|
ShooterConstants.kFFV
|
|
);
|
|
|
|
frontMotorEncoder = new Encoder(
|
|
ShooterConstants.kFrontEncoderA,
|
|
ShooterConstants.kFrontEncoderB
|
|
);
|
|
frontMotorEncoder.setReverseDirection(ShooterConstants.kInvertFrontEncoder);
|
|
frontMotorEncoder.setDistancePerPulse(ShooterConstants.kDistancePerPulse);
|
|
|
|
rearMotorEncoder = new Encoder(
|
|
ShooterConstants.kRearEncoderA,
|
|
ShooterConstants.kRearEncoderB
|
|
);
|
|
rearMotorEncoder.setReverseDirection(ShooterConstants.kInvertRearEncoder);
|
|
rearMotorEncoder.setDistancePerPulse(ShooterConstants.kDistancePerPulse);
|
|
}
|
|
|
|
public double getFrontEncoder() {
|
|
return frontMotorEncoder.getRate();
|
|
}
|
|
|
|
public double getRearEncoder() {
|
|
return rearMotorEncoder.getRate();
|
|
}
|
|
|
|
public boolean shooterAtSpeed() {
|
|
return (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2 >= ShooterConstants.kShooterFireSpeed;
|
|
}
|
|
|
|
public Command runAutoShooter() {
|
|
return run(() -> {
|
|
double output = bangBangController.calculate(
|
|
(frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2,
|
|
ShooterConstants.kShooterFireSpeed
|
|
) + motorFeedforward.calculate(ShooterConstants.kShooterFireSpeed);
|
|
|
|
frontMotor.setVoltage(output);
|
|
});
|
|
}
|
|
|
|
public Command runShooter(double speed) {
|
|
return run(() -> {
|
|
frontMotor.set(speed);
|
|
});
|
|
}
|
|
|
|
public Command stopShooter() {
|
|
return run(() -> {
|
|
frontMotor.set(0);
|
|
});
|
|
}
|
|
} |