diff --git a/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java b/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java index bc99d91..5935341 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java @@ -3,10 +3,11 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.stuypulse.robot.constants.Motors; +// import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -16,13 +17,24 @@ public class ClimberTest extends SubsystemBase{ private final RelativeEncoder rightEncoder; private final RelativeEncoder leftEncoder; - public ClimberTest(){ + private final DigitalInput rightMinLift; + private final DigitalInput leftMinLift; + private final DigitalInput rightMaxLift; + private final DigitalInput leftMaxLift; + + + public ClimberTest() { leftMotor = new CANSparkMax(Ports.Climber.LEFT_MOTOR, MotorType.kBrushless); rightMotor = new CANSparkMax(Ports.Climber.RIGHT_MOTOR, MotorType.kBrushless); rightEncoder = rightMotor.getEncoder(); leftEncoder = leftMotor.getEncoder(); + rightMinLift = new DigitalInput(Ports.Climber.TOP_RIGHT_LIMIT); + rightMaxLift = new DigitalInput(Ports.Climber.BOTTOM_RIGHT_LIMIT); + leftMaxLift = new DigitalInput(Ports.Climber.TOP_LEFT_LIMIT); + leftMinLift = new DigitalInput(Ports.Climber.BOTTOM_LEFT_LIMIT); + rightEncoder.setPositionConversionFactor(Settings.ClimberTest.Encoder.POSITION_CONVERSION); leftEncoder.setPositionConversionFactor(Settings.ClimberTest.Encoder.POSITION_CONVERSION); @@ -59,7 +71,7 @@ public double getRightVelocity() { public double getRightPosition() { return rightEncoder.getPosition(); } - + @Override public void periodic() { SmartDashboard.putNumber("Climber/Left Voltage", leftMotor.getAppliedOutput() * leftMotor.getBusVoltage()); @@ -68,6 +80,11 @@ public void periodic() { SmartDashboard.putNumber("Climber/Right Velocity", getRightVelocity()); SmartDashboard.putNumber("Climber/Left Position", getLeftPosition()); SmartDashboard.putNumber("Climber/Left Velocity", getLeftVelocity()); + SmartDashboard.putBoolean("Climber/Right Top", rightMaxLift.get()); + SmartDashboard.putBoolean("Climber/Left Top", leftMaxLift.get()); + SmartDashboard.putBoolean("Climber/Right Botoom", rightMinLift.get()); + SmartDashboard.putBoolean("Climber/Left Bottom", leftMinLift.get()); + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java b/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java index 9b4a138..26ad8fc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java @@ -5,6 +5,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Ports; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -14,12 +15,16 @@ public class ConveyerTest extends SubsystemBase{ private final RelativeEncoder gandalfEncoder; private final RelativeEncoder shooterFeederEncoder; + private final DigitalInput IRSensor; + public ConveyerTest(){ gandalfMotor = new CANSparkMax(Ports.Conveyor.GANDALF, MotorType.kBrushless); shooterFeederMotor = new CANSparkMax(Ports.Conveyor.FEEDER, MotorType.kBrushless); gandalfEncoder = gandalfMotor.getEncoder(); shooterFeederEncoder = shooterFeederMotor.getEncoder(); + + IRSensor = new DigitalInput(Ports.Conveyor.IR_SENSOR); } public void setGandalfVoltage(double voltage){ @@ -49,5 +54,6 @@ public void periodic() { SmartDashboard.putNumber("Conveyor/Shooter Feeder Motor Current", shooterFeederMotor.getOutputCurrent()); SmartDashboard.putNumber("Conveyor/Gandalf Motor Speed", gandalfMotor.get()); SmartDashboard.putNumber("Conveyor/Shooter Feeder Motor Spped", shooterFeederMotor.get()); + SmartDashboard.putBoolean("Conveyor/Conveyor Has Note", !IRSensor.get()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java b/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java index 060978e..105a2c4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java @@ -4,13 +4,17 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Ports; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeTest extends SubsystemBase{ private final CANSparkMax motor; + private final DigitalInput IRSensor; public IntakeTest() { motor = new CANSparkMax(Ports.Intake.MOTOR, MotorType.kBrushless); + IRSensor = new DigitalInput(Ports.Intake.IR_SENSOR); } public void setIntakeVoltage(double voltage) { motor.set(voltage); @@ -19,5 +23,10 @@ public void setIntakeVoltage(double voltage) { public void stop() { motor.set(0); } + + @Override + public void periodic(){ + SmartDashboard.putBoolean("Intake/ Intake has Note", !IRSensor.get()); + } }