diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 97d4f30..bcd15f8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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 { diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/robot/Gains.java index b2f1de2..7d2b3d7 100644 --- a/src/main/java/frc4388/robot/Gains.java +++ b/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 diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 37a847d..2585fbc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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))); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index a7d1bfb..84f01ec 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/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(); } }