From 07ab93a06fd4312924ada6b123c925ed39830ffa Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 10 Feb 2020 16:57:41 -0800 Subject: [PATCH 1/3] Added Neo PID stuff in Storage --- src/main/java/frc4388/robot/Constants.java | 17 ++++++++++++ src/main/java/frc4388/robot/Gains.java | 2 ++ .../frc4388/robot/subsystems/Storage.java | 26 ++++++++++++++++--- 3 files changed, 42 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f4f8c2b..bb630ff 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -79,12 +79,29 @@ public final class Constants { public static final class StorageConstants { public static final int STORAGE_CAN_ID = -1; + + /* 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/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index a7d1bfb..5ac0cb9 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -9,7 +9,9 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; import com.revrobotics.SparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -21,6 +23,9 @@ 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(); + /** * Creates a new Storage. */ @@ -40,17 +45,32 @@ 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"); } - + + } + + /* 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); } } From 952649e21bee83dc78548b99d8c0c3ebaae10dbd Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 10 Feb 2020 17:06:17 -0800 Subject: [PATCH 2/3] Added command to run PID method to Robot Container --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4ca2bc8..0e771f5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -102,6 +102,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) + .whenPressed(new InstantCommand(() -> m_robotStorage.runStoragePositionPID(20, 0.1, 1e-4, 1.0, 0.0, 0.0, 1, -1))); } /** From a4ac866d1c207536ff86f5cf976595792e97ec9f Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 10 Feb 2020 19:13:34 -0800 Subject: [PATCH 3/3] Tested and working Neo PID --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/Storage.java | 16 ++++++++++++++++ 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bb630ff..26a3feb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -78,7 +78,7 @@ 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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0e771f5..63cfd7a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,7 +105,7 @@ public class RobotContainer { /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotStorage.runStoragePositionPID(20, 0.1, 1e-4, 1.0, 0.0, 0.0, 1, -1))); + .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 5ac0cb9..84f01ec 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -9,6 +9,7 @@ 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; @@ -16,6 +17,7 @@ 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; @@ -26,10 +28,14 @@ public class Storage extends SubsystemBase { 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); @@ -60,6 +66,11 @@ public class Storage extends SubsystemBase { } + 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) { @@ -73,4 +84,9 @@ public class Storage extends SubsystemBase { m_storagePIDController.setReference(targetPos, ControlType.kPosition); } + + public void getEncoderPos() + { + m_encoder.getPosition(); + } }