SecondRobotEval/src/main/java/frc/robot/subsystems/Shooter.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);
});
}
}