Skip to content

Commit

Permalink
yes
Browse files Browse the repository at this point in the history
  • Loading branch information
ProgrammingSR committed Mar 23, 2024
1 parent 3e0aada commit 8dcdd3f
Show file tree
Hide file tree
Showing 2 changed files with 3 additions and 3 deletions.
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/commands/NoteIntakeCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ public NoteIntakeCommand(IntakeSubsystem subsystem) {
addRequirements(m_intakeSubsystem);

// If the distance sensor is not being used, then we want to use a timer to stop this command
m_intakeTimer.reset();
m_intakeTimer.start();
}

Expand All @@ -46,6 +47,6 @@ public void end(boolean interrupted) {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle);
return m_intakeSubsystem.m_colorSensorToggle ? (m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle): m_intakeTimer.get()>3;
}
}
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -29,15 +29,14 @@ public class IntakeSubsystem extends SubsystemBase {
private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);

/** If true, the distance sensor will be used to determine if we have a note */
private boolean m_colorSensorToggle = Robot.isReal();
public boolean m_colorSensorToggle = Robot.isReal();
private ColorSensorV3 m_colorSensor = new ColorSensorV3(Port.kMXP);

private double m_intakeSpeed = 0;
private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;

/** Creates a new IntakeSubsystem */
public IntakeSubsystem() {

m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition());
m_armEncoder.setDistancePerRotation(360);
Expand Down

0 comments on commit 8dcdd3f

Please sign in to comment.