Shooter.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

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.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Robot;

public class Shooter extends SubsystemBase {
  // Just made up these numbers
  FlywheelSim shooterSim = new FlywheelSim(DCMotor.getCIM(1), 0.5, 0.005);
  WPI_VictorSPX shooter = new WPI_VictorSPX(42);
  BangBangController controller = new BangBangController();
  // kv came from https://www.reca.lc/motors
  SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.00, 1.0/453.5);
  double setpoint;

  Encoder encoder = new Encoder(6,7);
  EncoderSim encoderSim;
  public Shooter() {
    setpoint = 0;
    encoder.setDistancePerPulse(1.0/2048); // 2048 ticks per rotation
    if (Robot.isSimulation()) {
      encoderSim = new EncoderSim(encoder);
    }

  }

  @Override
  public void periodic() {
    // Only using 90% of feedforward to make sure we never go above since a BB controller
    // doesn't go negative
    double ffComponent = 0.9 * ff.calculate(setpoint);
    double bbComponent = controller.calculate(encoder.getRate(), setpoint);
    shooter.set(ffComponent + bbComponent);
    SmartDashboard.putNumber("ff", ffComponent);
    SmartDashboard.putNumber("bb", bbComponent);
    SmartDashboard.putNumber("shooterSpeed", encoder.getRate());
  }

  @Override
  public void simulationPeriodic() {
    // Pass the current motor voltage to the simulator
    shooterSim.setInput(shooter.getMotorOutputVoltage());
    shooterSim.update(0.02); // one tick
    // Based on the above, update the encoder to show what the theoretical model shows
    encoderSim.setRate(shooterSim.getAngularVelocityRPM() / 60.0);
  }

  public double getSetpoint() {
    return setpoint;
  }

  public void setSetpoint(double setpoint) {
    this.setpoint = setpoint;
  }

}