diff --git a/src/main/java/frc/robot/commands/GetNoteStatus.java b/src/main/java/frc/robot/commands/GetNoteStatus.java index 0a37e87..7c171b6 100644 --- a/src/main/java/frc/robot/commands/GetNoteStatus.java +++ b/src/main/java/frc/robot/commands/GetNoteStatus.java @@ -1,8 +1,39 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.NoteSensorSubsystem; /* A simple but often used command to get a loaded (true) or nonloaded (false) status of the time of flight sensor pointed at the 'note' */ public class GetNoteStatus extends Command { - -} + /* */ + + private final NoteSensorSubsystem m_noteSensorSub; + + public GetNoteStatus(NoteSensorSubsystem noteSensorSub) { + this.m_noteSensorSub = noteSensorSub; + addRequirements(noteSensorSub); + } + + @Override + public void initialize() { + // Code to run when the command is first initialized + } + + @Override + public void execute() { + // Code to run repeatedly while the command is scheduled to run + m_noteSensorSub.isNoteLoaded(); + } + + @Override + public void end(boolean interrupted) { + // Code to run when the command ends or is interrupted + } + + @Override + public boolean isFinished() { + // Return true when the command should end + return false; + } + + } // end of class GetNoteStatus diff --git a/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java b/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java index a84cfcb..2511290 100644 --- a/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java @@ -1,5 +1,13 @@ package frc.robot.subsystems; +/* Subsystem class to primarily use a Time of Flight sensor from 'Playing with Fusion'. + * It will read the distance from the sensor to the 'note' and determine if the note is in a load position. + * It should return an actual distance reading to the SmartDashboard and a boolean for a method 'isNoteLoaded' + * This sensor data will change LED status and enable/disable intake and index motors + */ + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + import com.playingwithfusion.TimeOfFlight; import com.playingwithfusion.TimeOfFlight.RangingMode; @@ -7,18 +15,15 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -/* Subsystem class to primarily use a Time of Flight sensor from 'Playing with Fusion'. - * It will read the distance from the sensor to the 'note' and determine if the note is in a load position. - * This sensor data will change LED status and enable/disable intake and index motors - */ -public class NoteSensorSubsystem extends SubsystemBase { - /* Set ID in web interface http://172.22.11.2:5812/ */ +public class NoteSensorSubsystem extends SubsystemBase{ + + /* Set ID in web interface http://172.22.11.2:5812/ */ private TimeOfFlight sensorNoteDistance = new TimeOfFlight(Constants.CANIDs.timeOfFlightID); /* Set the distance to the note to be considered 'in load position.' Measure in (mm) to determine an appropriate value.*/ public int noteDistanceCheck = 0; - + /* Constructor */ public NoteSensorSubsystem() { /* Initialize the sensor, and '.setRangingMode(RandingMode.short)' for this usage. @@ -48,7 +53,6 @@ public void setLEDColor() { * Requires 'isNoteLoadeded' value and two led methods */ } - public void periodic() { // This method will be called once per scheduler run @@ -56,5 +60,5 @@ public void periodic() { SmartDashboard.putNumber("Note Distance", sensorNoteDistance.getRange()); SmartDashboard.putBoolean("Note Loaded", isNoteLoaded()); } - + } // end of class NoteSensorSubsystem