Merge branch 'master' into additional-motor-config

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

@@ -91,13 +91,30 @@ public final class Constants {
     }
   
     public static final class StorageConstants {
-        public static final int STORAGE_CAN_ID = -1;
+        public static final int STORAGE_CAN_ID = 10;
+
+        /* Ball Indexes */
         public static final int BEAM_SENSOR_DIO_0 = 0;
         public static final int BEAM_SENSOR_DIO_1 = 1;
         public static final int BEAM_SENSOR_DIO_2 = 2;
         public static final int BEAM_SENSOR_DIO_3 = 3;
         public static final int BEAM_SENSOR_DIO_4 = 4;
         public static final int BEAM_SENSOR_DIO_5 = 5;
+
+        /* PID Values */
+        public static final int SLOT_DISTANCE = 0;
+
+        /* PID Indexes */
+        public static final int PID_PRIMARY = 0;
+
+        /* PID Gains */
+        public static final double storP = 0.1;
+        public static final double storI = 1e-4;
+        public static final double storD = 1.0;
+        public static final double storIz = 0.0;
+        public static final double storF = 0.0;
+        public static final double storkmaxOutput = 1.0;
+        public static final double storkminOutput = -1.0;
     }
   
     public static final class LEDConstants {

src/main/java/frc4388/robot/Gains.java

@@ -17,6 +17,8 @@ public class Gains {
     public double m_kF;
     public int m_kIzone;
     public double m_kPeakOutput;
+    public double m_kmaxOutput;
+    public double m_kminOutput;
 
     /**
      * Creates Gains object for PIDs

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

@@ -134,6 +134,10 @@ public class RobotContainer {
         // interrupts any running command
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
             .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
+
+        /* Storage Neo PID Test */
+        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1)));
     }
       
     /**

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

@@ -9,11 +9,15 @@ package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
 import com.revrobotics.SparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc4388.robot.Constants.StorageConstants;
@@ -21,10 +25,17 @@ import frc4388.robot.Constants.StorageConstants;
 public class Storage extends SubsystemBase {
   private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
   private DigitalInput[] m_beamSensors = new DigitalInput[6];
+
+  CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
+
+  CANEncoder m_encoder = m_storageMotor.getEncoder();
+
   /**
    * Creates a new Storage.
    */
   public Storage() {
+    resetEncoder();
+
     m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
     m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
     m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
@@ -40,17 +51,42 @@ public class Storage extends SubsystemBase {
 
   /**
    * Runs storage motor
+   * 
    * @param input the voltage to run motor at
    */
-  public void runStorage(double input) {
+  public void runStorage(final double input) {
     m_storageMotor.set(input);
-    boolean beam_on = m_beamSensors[0].get();
+    final boolean beam_on = m_beamSensors[0].get();
 
     if (beam_on) {
       System.err.println("Beam on");
     } else {
       System.err.println("Beam off");
     }
-    
+
+  }
+
+  public void resetEncoder()
+  {
+    m_encoder.setPosition(0);
+  }
+
+  /* Storage PID Control */
+  public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
+  {
+    // Set PID Coefficients
+    m_storagePIDController.setP(kP);
+    m_storagePIDController.setI(kI);
+    m_storagePIDController.setD(kD);
+    m_storagePIDController.setIZone(kIz);
+    m_storagePIDController.setFF(kF);
+    m_storagePIDController.setOutputRange(kminOutput, kmaxOutput);
+
+    m_storagePIDController.setReference(targetPos, ControlType.kPosition);
+  }
+
+  public void getEncoderPos()
+  {
+    m_encoder.getPosition();
   }
 }