diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 97d4f30..12e8dd0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -71,7 +71,10 @@ public final class Constants { } public static final class ShooterConstants { + /* Motor IDs */ public static final int SHOOTER_FALCON_ID = 8; + public static final int SHOOTER_ANGLE_ADJUST_ID = -1; + public static final int SHOOTER_ROTATE_ID = -2; /* PID Constants Shooter */ public static final int SHOOTER_SLOT_IDX = 0; @@ -80,6 +83,8 @@ public final class Constants { public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; + public static final double NEO_UNITS_PER_REV = 42; + public static final double DEGREES_PER_ROT = 360; } public static final class ClimberConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 37a847d..6f6e29f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -134,6 +134,13 @@ public class RobotContainer { // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + + /* TEST shooter rotate PIDs */ + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + /* TEST for both commands above */ } /** diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 169e36f..7d2b87f 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -10,19 +10,32 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; -import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.ShooterConstants; public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + // Configure PID Controllers + CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); + CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); + + CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); + double velP; /** * Creates a new Shooter subsystem. @@ -87,4 +100,38 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } + + /* Angle Adjustment PID Control */ + public void runAngleAdjustPID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + { + // Set PID Coefficients + m_angleAdjustPIDController.setP(kP); + m_angleAdjustPIDController.setI(kI); + m_angleAdjustPIDController.setD(kD); + m_angleAdjustPIDController.setIZone(kIz); + m_angleAdjustPIDController.setFF(kF); + m_angleAdjustPIDController.setOutputRange(kminOutput, kmaxOutput); + + // Convert input angle in degrees to rotations of the motor + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + } + + /* Rotate Shooter PID Control */ + public void runshooterRotatePID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + { + // Set PID Coefficients + m_shooterRotatePIDController.setP(kP); + m_shooterRotatePIDController.setI(kI); + m_shooterRotatePIDController.setD(kD); + m_shooterRotatePIDController.setIZone(kIz); + m_shooterRotatePIDController.setFF(kF); + m_shooterRotatePIDController.setOutputRange(kminOutput, kmaxOutput); + + // Convert input angle in degrees to rotations of the motor + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } } \ No newline at end of file