From 85764a54b25b816b8da7883b0cf1ce35fcd70681 Mon Sep 17 00:00:00 2001 From: NoMythic <2491nomythic@gmail.com> Date: Sat, 10 Feb 2024 15:06:02 -0600 Subject: [PATCH] tested distance sensor and corrected code for it --- .../frc/robot/subsystems/IndexerSubsystem.java | 7 ++++++- .../java/frc/robot/subsystems/IntakeSubsystem.java | 14 +++++++------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index 0b117b23..642b7283 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -5,6 +5,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.SparkAnalogSensor.Mode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.settings.Constants.IndexerConstants; @@ -18,7 +19,7 @@ public IndexerSubsystem() { } public boolean isNoteIn() { - return m_DistanceSensor.getVoltage()>1; + return m_DistanceSensor.getVoltage()>2; } public void on() { @@ -32,4 +33,8 @@ public void off() { public void reverse() { m_IndexerMotor.set(-IndexerConstants.INDEXER_SPEED); } + @Override + public void periodic() { + SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage()); + } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 9bc84591..7ab90aa2 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -6,12 +6,12 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; - import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; - import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.settings.Constants.IntakeConstants; public class IntakeSubsystem extends SubsystemBase { /** Creates a new Intake. */ @@ -23,11 +23,11 @@ public class IntakeSubsystem extends SubsystemBase { double intakeRunSpeed; public IntakeSubsystem() { - CANSparkMax intake1 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); - CANSparkMax intake2 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); - CANSparkMax brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless); - CANSparkMax brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless); - CANSparkMax brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless); + intake1 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); + intake2 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); + brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless); + brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless); + brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless); intake2.follow(intake1); intake2.setInverted(true);