diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e05af11..1617d2b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,24 +66,31 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = 9; - public static final int EXTENDER_SPARK_ID = 10; + public static final int INTAKE_SPARK_ID = -9; + public static final int EXTENDER_SPARK_ID = -10; } public static final class ShooterConstants { + /* Motor IDs */ public static final int SHOOTER_FALCON_ID = 8; + public static final int SHOOTER_ANGLE_ADJUST_ID = 9; + public static final int SHOOTER_ROTATE_ID = 10; /* PID Constants Shooter */ public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final double SHOOTER_TURRET_MIN = -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 { - public static final int CLIMBER_SPARK_ID = 10; + public static final int CLIMBER_SPARK_ID = -1; } public static final class LevelerConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a431921..e678e1a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -72,11 +72,11 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); + // m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); // drives the leveler with an axis input from the driver controller - m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); + // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // runs storage motor at 50 percent - m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index f8649ae..16b6045 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -10,24 +10,41 @@ 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; + public static Gains m_shooterGains = ShooterConstants.DRUM_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. */ public Shooter() { + //Testing purposes reseting gyros + resetGyroAngleAdj(); + resetGyroShooterRotate(); + m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); @@ -86,4 +103,50 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } + + /* Angle Adjustment PID Control */ + public void runAngleAdjustPID(double targetAngle) + { + // Set PID Coefficients + m_angleAdjustPIDController.setP(m_shooterGains.m_kP); + m_angleAdjustPIDController.setI(m_shooterGains.m_kI); + m_angleAdjustPIDController.setD(m_shooterGains.m_kD); + m_angleAdjustPIDController.setIZone(m_shooterGains.m_kIzone); + m_angleAdjustPIDController.setFF(m_shooterGains.m_kF); + m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterGains.m_kPeakOutput); + + // 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) + { + // Set PID Coefficients + m_shooterRotatePIDController.setP(m_shooterGains.m_kP); + m_shooterRotatePIDController.setI(m_shooterGains.m_kI); + m_shooterRotatePIDController.setD(m_shooterGains.m_kD); + m_shooterRotatePIDController.setFF(m_shooterGains.m_kF); + m_shooterRotatePIDController.setIZone(m_shooterGains.m_kIzone); + m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterGains.m_kPeakOutput); + + // Convert input angle in degrees to rotations of the motor + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } + + /* For Testing Purposes, reseting gyro for angle adjuster */ + public void resetGyroAngleAdj() + { + m_angleEncoder.setPosition(0); + } + + /* For Testing Purposes, reseting gyro for shooter rotation */ + public void resetGyroShooterRotate() + { + m_shooterRotateEncoder.setPosition(0); + } } \ No newline at end of file