|
1 | 1 | package frc.robot.subsystems.Shooter; |
2 | 2 |
|
3 | | -public class ShooterIOSim { |
4 | | - |
| 3 | + |
| 4 | + |
| 5 | +import edu.wpi.first.math.MathUtil; |
| 6 | +import edu.wpi.first.math.controller.ProfiledPIDController; |
| 7 | +import edu.wpi.first.math.controller.SimpleMotorFeedforward; |
| 8 | +import edu.wpi.first.math.system.plant.DCMotor; |
| 9 | +import edu.wpi.first.math.trajectory.TrapezoidProfile; |
| 10 | +import edu.wpi.first.wpilibj.simulation.FlywheelSim; |
| 11 | + |
| 12 | + |
| 13 | +import edu.wpi.first.math.util.Units; |
| 14 | + |
| 15 | +public class ShooterIOSim implements ShooterIO { |
| 16 | + public void updateInputs(ShooterIOInputs inputs) { |
| 17 | + leftSim.update(0.020); |
| 18 | + rightSim.update(0.020); |
| 19 | + //need to figure out simpler way for if statements (verbose site) |
| 20 | + } |
| 21 | + private double leftAppliedVolts = 0.0; |
| 22 | + private double rightAppliedVolts = 0.0; |
| 23 | + |
| 24 | + private Double leftSetpointRPM = null; |
| 25 | + private Double rightSetpointRPM = null; |
| 26 | + |
| 27 | + |
| 28 | + private ProfiledPIDController pidController = new ProfiledPIDController( |
| 29 | + ShooterConstants.kP, |
| 30 | + ShooterConstants.kI, |
| 31 | + ShooterConstants.kD, |
| 32 | + new TrapezoidProfile.Constraints( |
| 33 | + ShooterConstants.MAX_VELOCITY, |
| 34 | + ShooterConstants.MAX_ACCELERATION |
| 35 | + ) |
| 36 | + ); |
| 37 | + |
| 38 | + FlywheelSim rightSim = new FlywheelSim(null, DCMotor.getNeoVortex(1), 6.7); |
| 39 | + FlywheelSim leftSim = new FlywheelSim(null, DCMotor.getNeoVortex(1), 6.7); |
| 40 | + |
| 41 | + private SimpleMotorFeedforward leftFF = new SimpleMotorFeedforward(0.0, 0.0, 0.0); |
| 42 | + private SimpleMotorFeedforward rightFF = new SimpleMotorFeedforward(0.0, 0.0, 0.0); |
| 43 | + |
| 44 | + |
| 45 | + @Override |
| 46 | + public void setFeedForwardGains(double kS, double kV, double kA) { |
| 47 | + leftFF = new SimpleMotorFeedforward(kS, kV, kA); |
| 48 | + rightFF = new SimpleMotorFeedforward(kS, kV, kA); |
| 49 | + } |
| 50 | + |
| 51 | + @Override |
| 52 | + public void setVoltage(double voltage) { |
| 53 | + rightSetpointRPM = null; |
| 54 | + rightAppliedVolts = MathUtil.clamp(voltage, -12.0, 12.0); |
| 55 | + rightSim.setInputVoltage(rightAppliedVolts); |
| 56 | + |
| 57 | + leftSetpointRPM = null; |
| 58 | + leftAppliedVolts = MathUtil.clamp(voltage, -12.0, 12.0); |
| 59 | + leftSim.setInputVoltage(rightAppliedVolts); |
| 60 | + } |
| 61 | + |
| 62 | + @Override |
| 63 | + public void setPIDGains(double kP, double kI, double kD) { |
| 64 | + pidController.setP(kP); |
| 65 | + pidController.setI(kI); |
| 66 | + pidController.setD(kD); |
| 67 | + } |
| 68 | + |
| 69 | + @Override |
| 70 | + public void setRPM(double rpm){ |
| 71 | + leftSetpointRPM = rpm; |
| 72 | + rightSetpointRPM = rpm; |
| 73 | + //is it really this simple?? |
| 74 | + } |
| 75 | + |
| 76 | + |
| 77 | + |
5 | 78 | } |
0 commit comments