From c37ffd7d16e57dfe90d379fe3c1e13c1a6d9ada3 Mon Sep 17 00:00:00 2001 From: UHS Robotics Date: Wed, 11 Mar 2020 15:48:21 -0400 Subject: [PATCH 1/2] Hopper using sensor --- src/main/java/frc/robot/RobotContainer.java | 221 +++++++++--------- .../frc/robot/commands/HopperCommand.java | 38 ++- 2 files changed, 138 insertions(+), 121 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 78fb43a..d1bce2c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,116 +25,117 @@ */ public class RobotContainer { - // Subsystem setup - private final FlywheelSubsystem m_flywheelSubsystem = new FlywheelSubsystem(); - private final LiftSubsystem m_liftSubsystem = new LiftSubsystem(); - private final DigitalInput m_magSwitch = new DigitalInput(3); - private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); - // private final DropIntakeSubsystem m_DropIntakeSubsystem = new - // DropIntakeSubsystem(); - private final ServoSubsystem m_servoSubsystem = new ServoSubsystem(); - private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); - private final HopperSubsystem m_hopper = new HopperSubsystem(); - private final ColorSubsystem m_colorSubsystem = new ColorSubsystem(); - private final SpinSubsystem m_spinSubsystem = new SpinSubsystem(); - private final LiftPID m_liftPID = new LiftPID(m_liftSubsystem); - private final RotPID m_rotPID = new RotPID(m_driveSubsystem); - - private final ManualShootingCommand m_manualShooting = new ManualShootingCommand(m_flywheelSubsystem, m_hopper, - m_rotPID, m_driveSubsystem, m_visionSubsystem); - - // Autonomous setup - private final Command m_simpleAutoCommand = new AutonomousSequence(m_driveSubsystem, m_flywheelSubsystem, m_hopper); - SendableChooser m_chooser = new SendableChooser<>(); - DualShockController m_driverController = new DualShockController(0); - DualShockController m_subsystemController = new DualShockController(1); - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - m_driverController.initMapping(OIConstants.kDriverControllerCurvature); - configureButtonBindings(); - m_flywheelSubsystem - .setDefaultCommand(new FlywheelCommand(m_flywheelSubsystem, () -> m_subsystemController.getCrossButton())); - m_liftSubsystem.setDefaultCommand( - new LiftCommand(m_liftSubsystem, m_liftPID, () -> m_subsystemController.getBumper(Hand.kLeft), - () -> m_subsystemController.getBumper(Hand.kRight), m_servoSubsystem)); - m_IntakeSubsystem - .setDefaultCommand(new IntakeCommand(m_IntakeSubsystem, () -> m_subsystemController.getYMapped(Hand.kLeft))); - m_driveSubsystem.setDefaultCommand(new ArcadeDrive(m_driveSubsystem, - () -> m_driverController.getYMapped(Hand.kLeft), () -> m_driverController.getXMapped(Hand.kRight))); - m_hopper.setDefaultCommand(new HopperCommand(m_hopper, () -> m_subsystemController.getYMapped(Hand.kRight))); - // m_chooser.setDefaultOption("target", m_autonPlaceholder); - // m_chooser.addOption("simple drive", m_simpleAutoCommand); - // Shuffleboard.getTab("Autonomous").add(m_chooser); - - } - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - - // new JoystickButton(m_driverController, Button.kTrig.value) - // .whileHeld(new IntakeCommand(m_IntakeSubsystem, () -> - // m_driverController.getTrigButton())); - - // For debugging only, delete before competition - // new JoystickButton(m_driverController, Button.kBumperLeft.value) - // .whileHeld(new DistancePIDCommand(m_driveSubsystem, 500)); - new JoystickButton(m_driverController, Button.kBumperRight.value) - .whenPressed(new RotationPIDCommand(m_driveSubsystem, 180)); - - // TODO: enable this after making sure rotation PID works - /* - * new JoystickButton(m_driverController, Button.kDisk.value) .whenPressed(()->{ - * ((ArcadeDrive)m_driveSubsystem.getDefaultCommand()).toggleInvert(); new - * RotationPIDCommand(m_driveSubsystem, 180); }); + // Subsystem setup + private final FlywheelSubsystem m_flywheelSubsystem = new FlywheelSubsystem(); + private final LiftSubsystem m_liftSubsystem = new LiftSubsystem(); + private final DigitalInput m_magSwitch = new DigitalInput(3); + private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); + // private final DropIntakeSubsystem m_DropIntakeSubsystem = new + // DropIntakeSubsystem(); + private final ServoSubsystem m_servoSubsystem = new ServoSubsystem(); + private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + private final HopperSubsystem m_hopper = new HopperSubsystem(); + private final ColorSubsystem m_colorSubsystem = new ColorSubsystem(); + private final SpinSubsystem m_spinSubsystem = new SpinSubsystem(); + private final LiftPID m_liftPID = new LiftPID(m_liftSubsystem); + private final RotPID m_rotPID = new RotPID(m_driveSubsystem); + + private final ManualShootingCommand m_manualShooting = new ManualShootingCommand(m_flywheelSubsystem, m_hopper, + m_rotPID, m_driveSubsystem, m_visionSubsystem); + + // Autonomous setup + private final Command m_simpleAutoCommand = new AutonomousSequence(m_driveSubsystem, m_flywheelSubsystem, m_hopper); + SendableChooser m_chooser = new SendableChooser<>(); + DualShockController m_driverController = new DualShockController(0); + DualShockController m_subsystemController = new DualShockController(1); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. */ - - // new JoystickButton(m_subsystemController, Button.kRect.value) - // .whenPressed(new VisionDistancePIDCommand(m_driveSubsystem, - // m_visionSubsystem)); - - // new JoystickButton(m_subsystemController, Button.kBumperLeft.value) - // .whileHeld(new DropIntakeCommand(m_DropIntakeSubsystem, - // () -> m_subsystemController.getBumperPressed(Hand.kLeft), - // () -> m_subsystemController.getBumperPressed(Hand.kRight) )); - - // @return 0-1-2-3=blue-green-red-yellow; -1: match revolution - - new JoystickButton(m_subsystemController, Button.kDisk.value) - .whenPressed(new InstantCommand(m_servoSubsystem::toggle, m_servoSubsystem)); - - new JoystickButton(m_driverController, Button.kRect.value) - .whileHeld(m_manualShooting).whenReleased(()->{m_manualShooting.stop();}); - - - /* - * new JoystickButton(m_driverController, Button.kBumperRight.value) - * .whenPressed(new SpinCommand(m_colorSubsystem, m_spinSubsystem, -1)); new - * JoystickButton(m_driverController, Button.kRect.value) .whenPressed(new - * SpinCommand(m_colorSubsystem, m_spinSubsystem, 0)); new - * JoystickButton(m_driverController, Button.kCross.value) .whenPressed(new - * SpinCommand(m_colorSubsystem, m_spinSubsystem, 1)); new - * JoystickButton(m_driverController, Button.kDisk.value) .whenPressed(new - * SpinCommand(m_colorSubsystem, m_spinSubsystem, 2)); new - * JoystickButton(m_driverController, Button.kTrig.value) .whenPressed(new - * SpinCommand(m_colorSubsystem, m_spinSubsystem, 3)); + public RobotContainer() { + m_driverController.initMapping(OIConstants.kDriverControllerCurvature); + configureButtonBindings(); + m_flywheelSubsystem.setDefaultCommand( + new FlywheelCommand(m_flywheelSubsystem, () -> m_subsystemController.getCrossButton())); + m_liftSubsystem.setDefaultCommand( + new LiftCommand(m_liftSubsystem, m_liftPID, () -> m_subsystemController.getBumper(Hand.kLeft), + () -> m_subsystemController.getBumper(Hand.kRight), m_servoSubsystem)); + m_IntakeSubsystem.setDefaultCommand( + new IntakeCommand(m_IntakeSubsystem, () -> m_subsystemController.getYMapped(Hand.kLeft))); + m_driveSubsystem.setDefaultCommand(new ArcadeDrive(m_driveSubsystem, + () -> m_driverController.getYMapped(Hand.kLeft), () -> m_driverController.getXMapped(Hand.kRight))); + m_hopper.setDefaultCommand(new HopperCommand(m_hopper, () -> m_subsystemController.getYMapped(Hand.kRight), + () -> m_subsystemController.getTrigButton())); + // m_chooser.setDefaultOption("target", m_autonPlaceholder); + // m_chooser.addOption("simple drive", m_simpleAutoCommand); + // Shuffleboard.getTab("Autonomous").add(m_chooser); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + + // new JoystickButton(m_driverController, Button.kTrig.value) + // .whileHeld(new IntakeCommand(m_IntakeSubsystem, () -> + // m_driverController.getTrigButton())); + + // For debugging only, delete before competition + // new JoystickButton(m_driverController, Button.kBumperLeft.value) + // .whileHeld(new DistancePIDCommand(m_driveSubsystem, 500)); + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(new RotationPIDCommand(m_driveSubsystem, 180)); + + // TODO: enable this after making sure rotation PID works + /* + * new JoystickButton(m_driverController, Button.kDisk.value) .whenPressed(()->{ + * ((ArcadeDrive)m_driveSubsystem.getDefaultCommand()).toggleInvert(); new + * RotationPIDCommand(m_driveSubsystem, 180); }); + */ + + // new JoystickButton(m_subsystemController, Button.kRect.value) + // .whenPressed(new VisionDistancePIDCommand(m_driveSubsystem, + // m_visionSubsystem)); + + // new JoystickButton(m_subsystemController, Button.kBumperLeft.value) + // .whileHeld(new DropIntakeCommand(m_DropIntakeSubsystem, + // () -> m_subsystemController.getBumperPressed(Hand.kLeft), + // () -> m_subsystemController.getBumperPressed(Hand.kRight) )); + + // @return 0-1-2-3=blue-green-red-yellow; -1: match revolution + + new JoystickButton(m_subsystemController, Button.kDisk.value) + .whenPressed(new InstantCommand(m_servoSubsystem::toggle, m_servoSubsystem)); + + new JoystickButton(m_driverController, Button.kRect.value).whileHeld(m_manualShooting).whenReleased(() -> { + m_manualShooting.stop(); + }); + + /* + * new JoystickButton(m_driverController, Button.kBumperRight.value) + * .whenPressed(new SpinCommand(m_colorSubsystem, m_spinSubsystem, -1)); new + * JoystickButton(m_driverController, Button.kRect.value) .whenPressed(new + * SpinCommand(m_colorSubsystem, m_spinSubsystem, 0)); new + * JoystickButton(m_driverController, Button.kCross.value) .whenPressed(new + * SpinCommand(m_colorSubsystem, m_spinSubsystem, 1)); new + * JoystickButton(m_driverController, Button.kDisk.value) .whenPressed(new + * SpinCommand(m_colorSubsystem, m_spinSubsystem, 2)); new + * JoystickButton(m_driverController, Button.kTrig.value) .whenPressed(new + * SpinCommand(m_colorSubsystem, m_spinSubsystem, 3)); + */ + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous */ - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return m_simpleAutoCommand; - } + public Command getAutonomousCommand() { + return m_simpleAutoCommand; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/HopperCommand.java b/src/main/java/frc/robot/commands/HopperCommand.java index d8bd9f9..b96009f 100644 --- a/src/main/java/frc/robot/commands/HopperCommand.java +++ b/src/main/java/frc/robot/commands/HopperCommand.java @@ -7,11 +7,13 @@ package frc.robot.commands; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants.HopperConstants; import frc.robot.subsystems.HopperSubsystem; +import edu.wpi.first.wpilibj.AnalogInput; public class HopperCommand extends CommandBase { /** @@ -19,14 +21,17 @@ public class HopperCommand extends CommandBase { */ private final HopperSubsystem m_hopper; - private final DoubleSupplier m_activate; + private final DoubleSupplier m_activate; + private final BooleanSupplier m_isManual; + private final AnalogInput m_sensor = new AnalogInput(0); // double m_delay = 0.5; boolean finished = false; - + double hopperLag = 15; - public HopperCommand(HopperSubsystem subsystem, DoubleSupplier hopperActivation) { + public HopperCommand(HopperSubsystem subsystem, DoubleSupplier hopperActivation, BooleanSupplier manualOverride) { m_hopper = subsystem; m_activate = hopperActivation; + m_isManual = manualOverride; addRequirements(m_hopper); } @@ -34,16 +39,27 @@ public HopperCommand(HopperSubsystem subsystem, DoubleSupplier hopperActivation) // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(m_activate.getAsDouble() >= 0.5){ - m_hopper.setPIDTarget(-1 * HopperConstants.targetRPM); - }else if(m_activate.getAsDouble() <= -0.5){ - m_hopper.setPIDTarget(HopperConstants.targetRPM); + if (!m_isManual.getAsBoolean()) { + if (m_sensor.getValue() * 0.125 / 2.54 < 10) { + hopperLag = 15; + m_hopper.setPIDTarget(HopperConstants.targetRPM); + } else { + hopperLag--; + if (hopperLag < 1) { + m_hopper.setPIDTarget(0); + } + } + } else { + if (m_activate.getAsDouble() >= 0.5) { + m_hopper.setPIDTarget(-1 * HopperConstants.targetRPM); + } else if (m_activate.getAsDouble() <= -0.5) { + m_hopper.setPIDTarget(HopperConstants.targetRPM); + } else { + m_hopper.setPIDTarget(0); + } } - else{ - m_hopper.setPIDTarget(0); - } - } + } // Called once the command ends or is interrupted. @Override From a161e52c11ee3b7ed5c5b33127d78de3017bf1c3 Mon Sep 17 00:00:00 2001 From: UHS Robotics Date: Wed, 11 Mar 2020 15:51:03 -0400 Subject: [PATCH 2/2] Changed to constants --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/commands/HopperCommand.java | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cbca795..db53ac3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,6 +33,7 @@ public static final class Ports { public static final int hopper = 20; public static final int liftEncoderA = 6; public static final int liftEncoderB = 4; + public static final int ultrasonicSensor = 0; } @@ -126,6 +127,7 @@ public static final class HopperConstants { public static final double Ki = 0.000001; public static final double Kff = 0.0003; public static final double targetRPM = 300; + public static final double detectionRange=10; } diff --git a/src/main/java/frc/robot/commands/HopperCommand.java b/src/main/java/frc/robot/commands/HopperCommand.java index b96009f..1278194 100644 --- a/src/main/java/frc/robot/commands/HopperCommand.java +++ b/src/main/java/frc/robot/commands/HopperCommand.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants.HopperConstants; +import frc.robot.Constants.Ports; import frc.robot.subsystems.HopperSubsystem; import edu.wpi.first.wpilibj.AnalogInput; @@ -23,7 +24,7 @@ public class HopperCommand extends CommandBase { private final HopperSubsystem m_hopper; private final DoubleSupplier m_activate; private final BooleanSupplier m_isManual; - private final AnalogInput m_sensor = new AnalogInput(0); + private final AnalogInput m_sensor = new AnalogInput(Ports.ultrasonicSensor); // double m_delay = 0.5; boolean finished = false; double hopperLag = 15; @@ -40,7 +41,7 @@ public HopperCommand(HopperSubsystem subsystem, DoubleSupplier hopperActivation, @Override public void execute() { if (!m_isManual.getAsBoolean()) { - if (m_sensor.getValue() * 0.125 / 2.54 < 10) { + if (m_sensor.getValue() * 0.125 / 2.54 < HopperConstants.detectionRange) { hopperLag = 15; m_hopper.setPIDTarget(HopperConstants.targetRPM); } else {