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(); + } }