AdvShooter.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 java.util.function.DoubleSupplier;

import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonSRXFeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import frc.robot.Constants;

public class AdvShooter extends PIDSubsystem {
  WPI_TalonSRX shooter;
  double constant = 1/4096;
  private final SimpleMotorFeedforward m_shooterFeedforward =
      new SimpleMotorFeedforward( Constants.ksShooter,
                                 Constants.kvShooter );
  /** Creates a new AdvShooter. */
  public AdvShooter() {
    super( new PIDController( Constants.kPShooter, 0, Constants.kDShooter ) );
    getController().setTolerance( Constants.kShooterToleranceRPM );
    setSetpoint(5000/60);
    shooter = new WPI_TalonSRX( Constants.kCANShooter );
    shooter.configFactoryDefault();
    shooter.setNeutralMode( NeutralMode.Coast );
    shooter.setInverted( true );
    //shooter.configPeakOutputForward( +1.0 );
    //shooter.configPeakOutputReverse( -1.0 );
    shooter.configSelectedFeedbackSensor( TalonSRXFeedbackDevice.CTRE_MagEncoder_Relative, 0, 0 );
    shooter.configSelectedFeedbackCoefficient( constant );
  }

  @Override
  public void useOutput(double output, double setpoint) {
    // Use the output here
    shooter.setVoltage( output + m_shooterFeedforward.calculate( setpoint ) );
    SmartDashboard.putNumber("Use Out", output + m_shooterFeedforward.calculate( setpoint ) );

  }

  public void setSetpoint( DoubleSupplier setpoint ) {
    // TODO Auto-generated method stub
    setSetpoint(setpoint.getAsDouble()/60);
  }

  @Override
  public double getMeasurement() {
    // Return the process variable measurement here
    return shooter.getSelectedSensorVelocity()*10;
  }

  @Override
  public void periodic() {
    // TODO Auto-generated method stub
    super.periodic();
    SmartDashboard.putNumber("Target", getSetpoint() );
    SmartDashboard.putNumber("Value", getMeasurement() );
    SmartDashboard.putNumber("Motor Out", shooter.getMotorOutputVoltage());
  }
}