diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d1dd424..6521b27 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -112,7 +112,8 @@ public final class Constants { public static final class ShooterConstants { /* Motor IDs */ - public static final int SHOOTER_FALCON_ID = 8; + public static final int SHOOTER_FALCON_BALLER_ID = 8; + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15; public static final int SHOOTER_ANGLE_ADJUST_ID = 10; public static final int SHOOTER_ROTATE_ID = 9; @@ -156,7 +157,7 @@ public final class Constants { } public static final class LevelerConstants { - public static final int LEVELER_CAN_ID = 15; + public static final int LEVELER_CAN_ID = 30; } public static final class IntakeConstants {; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 940694c..925e2a4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -163,7 +163,7 @@ public class RobotContainer { // runs the hood with joystick m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); // moves the drum not - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter)); // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index b3d3ac1..11ccd56 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -35,7 +35,7 @@ public class ShooterVelocityControlPID extends CommandBase { @Override public void execute() { //Tells whether the target velocity has been reached - m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition(); + m_actualVel = m_shooter.m_shooterFalconLeft.getSelectedSensorPosition(); m_targetVel = m_shooter.addFireVel(); double error = m_actualVel - m_targetVel; if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){ diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 5860235..6022832 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -312,7 +312,7 @@ public class Drive extends SubsystemBase { public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) { m_pneumaticsSubsystem = subsystem; m_shooter = shooter; - m_orchestra.addInstrument(m_shooter.m_shooterFalcon); + m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft); } public void updateTime() { diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 890e0f7..a45894e 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -21,8 +21,8 @@ import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { - public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); - + public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); + public WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID); public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static Shooter m_shooter; public static IHandController m_controller; @@ -50,24 +50,42 @@ public class Shooter extends SubsystemBase { //SmartDashboard.putNumber("Velocity Target", 10000); //SmartDashboard.putNumber("Angle Target", 3); - m_shooterFalcon.configFactoryDefault(); - m_shooterFalcon.setNeutralMode(NeutralMode.Coast); - m_shooterFalcon.setInverted(true); - m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + //LEFT FALCON + m_shooterFalconLeft.configFactoryDefault(); + m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + m_shooterFalconLeft.setInverted(true); + m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + + //RIGHT FALCON + m_shooterFalconRight.configFactoryDefault(); + m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); + m_shooterFalconRight.setInverted(false); + m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); - m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - - m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - int closedLoopTimeMs = 1; - m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + //LEFT FALCON + m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + + //RIGHT FALCON + //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterTable = new ShooterTables(); - - m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); @@ -83,13 +101,13 @@ public class Shooter extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run try{ - SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature()); + SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); - SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); + SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady); } @@ -118,18 +136,20 @@ public class Shooter extends SubsystemBase { * @param speed Speed to set the motor at */ public void runDrumShooter(double speed) { - m_shooterFalcon.set(speed); + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + m_shooterFalconRight.follow(m_shooterFalconLeft); + //m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed); } /** * Configures gains for shooter PID. */ public void setShooterGains() { - m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } /** @@ -138,6 +158,7 @@ public class Shooter extends SubsystemBase { */ public void runDrumShooterVelocityPID(double targetVel) { //System.out.println("Target Velocity" + targetVel); - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init PID + m_shooterFalconRight.follow(m_shooterFalconLeft); } } \ No newline at end of file