Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ultrasonic sensor impl #16

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Ultrasonic sensor impl #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c37ffd7
Hopper using sensor
Mar 11, 2020
a161e52
Changed to constants
Mar 11, 2020
e099013
Merge branch 'master' into working
RoboticsUHS Mar 11, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

}

Expand Down Expand Up @@ -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;

}

Expand Down
221 changes: 112 additions & 109 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,114 +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);
private final Turning180Command m_turning180 = new Turning180Command(m_rotPID, m_driveSubsystem);

// Autonomous setup
private final Command m_simpleAutoCommand = new AutonomousSequence(m_driveSubsystem, m_flywheelSubsystem, m_hopper);
SendableChooser<Command> m_chooser = new SendableChooser<>();
DualShockController m_driverController = new DualShockController(0);
DualShockController m_subsystemController = new DualShockController(1);

private final ArcadeDrive m_defaultDrive = new ArcadeDrive(m_driveSubsystem,
() -> m_driverController.getYMapped(Hand.kLeft), () -> m_driverController.getXMapped(Hand.kRight));
/**
* 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(m_defaultDrive);
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));

// TODO: enable this after making sure rotation PID works
new JoystickButton(m_driverController, Button.kDisk.value).whenPressed(() -> {
m_defaultDrive.toggleInvert();
}).whileHeld(m_turning180).whenReleased(()->{m_manualShooting.stop();});

// 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.kBumperRight.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));
// 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<Command> 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.
*/
}

/**
* 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 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
*/
public Command getAutonomousCommand() {
return m_simpleAutoCommand;
}
}
39 changes: 28 additions & 11 deletions src/main/java/frc/robot/commands/HopperCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,43 +7,60 @@

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.Constants.Ports;
import frc.robot.subsystems.HopperSubsystem;
import edu.wpi.first.wpilibj.AnalogInput;

public class HopperCommand extends CommandBase {
/**
* Creates a new HopperCommand.
*/

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(Ports.ultrasonicSensor);
// 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);

}

// 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 < HopperConstants.detectionRange) {
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
Expand Down