Merge pull request #15 from Team4388/add-shooter-subsystem-PID-new

Add shooter subsystem pid new

src/main/java/frc4388/robot/Constants.java

@@ -69,6 +69,18 @@ public final class Constants {
         public static final int INTAKE_SPARK_ID = 9;
         public static final int EXTENDER_SPARK_ID = 10;
     }
+  
+    public static final class ShooterConstants {
+        public static final int SHOOTER_FALCON_ID = 8;
+
+        /* 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 double ENCODER_TICKS_PER_REV = 2048;
+    }
     
     public static final class ClimberConstants {
         public static final int CLIMBER_SPARK_ID = 10;

src/main/java/frc4388/robot/RobotContainer.java

@@ -20,6 +20,11 @@ import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
+import frc4388.robot.commands.ShooterVelocityControlPID;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
 import frc4388.robot.subsystems.Drive;
@@ -43,6 +48,7 @@ public class RobotContainer {
     private final Drive m_robotDrive = new Drive();
     private final LED m_robotLED = new LED();
     private final Intake m_robotIntake = new Intake();
+    private final Shooter m_robotShooter = new Shooter();
     private final Climber m_robotClimber = new Climber();
     private final Leveler m_robotLeveler = new Leveler();
     private final Storage m_robotStorage = new Storage();
@@ -66,6 +72,8 @@ public class RobotContainer {
         m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
         // 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));
         // drives the leveler with an axis input from the driver controller
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
         // runs storage motor at 50 percent
@@ -80,6 +88,9 @@ public class RobotContainer {
     */
     private void configureButtonBindings() {
         /* Driver Buttons */
+        // test command to spin the robot while pressing A on the driver controller
+        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive);
         //new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
         //    .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
 
@@ -88,15 +99,18 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
             .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+      
+        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
+            .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000));
         
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new RunExtenderOutIn(m_robotIntake));
-
         /* PID Test Command */
         // runs velocity PID while driving straight
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
             .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500))
             .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
+      
         // resets the yaw of the pigeon
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
@@ -108,9 +122,7 @@ public class RobotContainer {
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
-
+        
-        //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-        //    .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive));
         // interrupts any running command
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
             .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));

src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java

@@ -8,39 +8,31 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Shooter;
-import frc4388.robot.subsystems.Drive;
 
-public class DriveToDistanceMM extends CommandBase {
+public class ShooterVelocityControlPID extends CommandBase {
-  Drive m_drive;
+  Shooter m_shooter;
-  double m_distance;
+  double m_targetVel;
-  double m_leftTarget;
-  double m_rightTarget;
-  
   /**
-   * Creates a new DriveToDistancePID.
+   * Creates a new ShooterVelocityControlPID.
-   * @param subsystem drive subsystem
-   * @param distance distance to travel in inches
    */
-  public DriveToDistanceMM(Drive subsystem, double distance) {
+  public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
     // Use addRequirements() here to declare subsystem dependencies.
-    m_drive = subsystem;
+    m_shooter = subsystem;
-    m_distance = distance * DriveConstants.TICKS_PER_INCH;
+    m_targetVel = targetVel;
-    addRequirements(m_drive);
+    addRequirements(m_shooter);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
-    m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    //m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
+    System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
-    //m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
+    m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
   }
 
   // Called once the command ends or is interrupted.
@@ -51,10 +43,6 @@ public class DriveToDistanceMM extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
+    return false;
-      return true;
-    } else {
-      return false;
-    }
   }
 }

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -83,7 +83,6 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.setInverted(InvertType.FollowMaster);
     m_rightBackMotor.setInverted(InvertType.FollowMaster);
 
-
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -239,6 +238,7 @@ public class Drive extends SubsystemBase {
       SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
       SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
       SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
+
       SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
       SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
 

src/main/java/frc4388/robot/subsystems/Shooter.java

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+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 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 static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
+
+  double velP;
+  /**
+   * Creates a new Shooter subsystem.
+   */
+  public Shooter() {
+    m_shooterFalcon.configFactoryDefault();
+
+    m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
+
+    m_shooterFalcon.setInverted(true);
+    
+    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);
+    m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
+  }
+
+  /**
+   * Runs drum shooter motor.
+   * @param speed Speed to set the motor at
+   */
+  public void runDrumShooter(double speed) {
+    m_shooterFalcon.set(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_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  }
+  /**
+   * Runs drum shooter velocity PID.
+   * @param falcon Motor to use
+   * @param targetVel Target velocity to run motor at
+   */
+  public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
+    velP = actualVel/targetVel; //Percent of target
+    double runSpeed = actualVel + (12000*velP); //Ramp up equation
+    //if (runSpeed > targetVel) {runSpeed = targetVel;}
+    SmartDashboard.putNumber("shoot", actualVel);
+    runSpeed = runSpeed/targetVel; //Convert to percent
+    if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
+      m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
+    }
+    else{ //PID Based on targetVel
+      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
+    }
+  }
+}