While these little robots don’t have gyros or cameras, they do have a infrared sensor that we are going to use to detect black lines. Our first command will be one that will simply drive the robot forward until it sees the black line and then stops. Let’s call this new command DriveToLineCommand. Go ahead and set up the framework for this new command.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 |
/*----------------------------------------------------------------------------*/ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; /** * An example command that uses an example subsystem. */ public class DriveToLineCommand extends Command { private final DriveSubsystem m_subsystem; /** * Creates a new DriveToLineCommand. * * @param subsystem The subsystem used by this command. */ public DriveToLineCommand(DriveSubsystem subsystem) { Logger.log("DriveToLineCommand", 3, "DriveToLineCommand()"); m_subsystem = subsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { Logger.log("DriveToLineCommand", 2, "initialize()"); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { Logger.log("DriveToLineCommand", -1, "execute()"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { Logger.log("DriveToLineCommand", 2, String.format("end(%b)", interrupted)); } // Returns true when the command should end. @Override public boolean isFinished() { Logger.log("DriveToLineCommand", -1, "isFinished()"); return false; } } |
The light sensors are connected to the Microcontroller via one of its digital input pins. Reading the state of that pin will tell us if the sensor is seeing the line or not. The class that reads the digital input pins is called DigitalInput so we will need a variable of this type.
In general, we should not create multiple instances of the same device and since we will be using this sensor in more than one command, we should create the instance in our RobotContainer class. Looking at it’s constructor, we see that the constructor for DigitalInput takes the number of the pin to be read. The IR sensor is connected to pin Device.IO_4, so add the following to your RobotContainer:
|
1 |
private final DigitalInput m_irSensor = new DigitalInput(Device.IO_4); |
Now your DriveToLineCommand is going to need access to this sensor, so we will pass it in as a parameter to DriveToLineCommand’s constructor:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
public class DriveToLineCommand extends Command { private final DriveSubsystem m_subsystem; private final DigitalInput m_irSensor; /** * Creates a new DriveToLineCommand. * * @param subsystem The subsystem used by this command. */ public DriveToLineCommand(DriveSubsystem subsystem, DigitalInput irSensor) { Logger.log("DriveToLineCommand", 3, "DriveToLineCommand()"); m_subsystem = subsystem; m_irSensor = irSensor; // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } |
In the initialize() function we need to start our robot driving forward:
|
1 2 3 4 5 |
public void initialize() { Logger.log("DriveToLineCommand", 2, "initialize()"); m_subsystem.setSpeed(k_speed, k_speed); } |
while defining k_speed as:
|
1 |
private static final double k_speed = 0.5; |
which will make the robot drive forward at half speed.
Next we need to return true from the isFinished() function when the sensor ‘sees’ the line. To check this we call the get() function of our DigitalInput class. If the light sensor ‘sees’ bright (i.e. no black line), then get() will return false. If the sensor ‘sees’ dark (i.e. black line is present), then get() will return true. Hence we need to declare our isFinished() as follows:
|
1 2 3 4 5 |
public boolean isFinished() { Logger.log("DriveToLineCommand", -1, "isFinished()"); return (m_irSensor.get()); } |
Finally, we need to turn off the motors in the end() functions:
|
1 2 3 4 5 |
public void end(boolean interrupted) { Logger.log("DriveToLineCommand", 2, String.format("end(%b)", interrupted)); m_subsystem.setPower(0, 0); } |
Your DriveToLineCommand.java file should now look like:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 |
/*----------------------------------------------------------------------------*/ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import robotCore.DigitalInput; import robotCore.Logger; /** * An example command that uses an example subsystem. */ public class DriveToLineCommand extends Command { private final DriveSubsystem m_subsystem; private final DigitalInput m_irSensor; private static final double k_speed = 0.5; /** * Creates a new DriveToLineCommand. * * @param subsystem The subsystem used by this command. */ public DriveToLineCommand(DriveSubsystem subsystem, DigitalInput irSensor) { Logger.log("DriveToLineCommand", 3, "DriveToLineCommand()"); m_subsystem = subsystem; m_irSensor = irSensor; // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { Logger.log("DriveToLineCommand", 2, "initialize()"); m_subsystem.setSpeed(k_speed, k_speed); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { Logger.log("DriveToLineCommand", -1, "execute()"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { Logger.log("DriveToLineCommand", 2, String.format("end(%b)", interrupted)); m_subsystem.setPower(0, 0); } // Returns true when the command should end. @Override public boolean isFinished() { Logger.log("DriveToLineCommand", -1, "isFinished()"); return (m_irSensor.get()); } } |
Finally connect this command to button 7 on the joystick:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 |
/*----------------------------------------------------------------------------*/ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package frc.robot; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.commands.DriveForTimeCommand; import frc.robot.commands.DriveToLineCommand; import frc.robot.commands.TestMotorSpeedCommand; import frc.robot.commands.TurnCommand; import frc.robot.commands.ArcadeDriveCommand; import frc.robot.commands.CalibrateSpeedCommand; import frc.robot.commands.DriveCourseCommand; import frc.robot.commands.DriveForDistanceCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ExampleSubsystem; import robotCore.Device; import robotCore.DigitalInput; /** * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link Robot} periodic methods (other than the * scheduler calls). Instead, the structure of the robot (including subsystems, * commands, and button mappings) should be declared here. */ public class RobotContainer { // The robot's subsystems and commands are defined here... @SuppressWarnings("unused") private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final CommandJoystick m_joystick = new CommandJoystick(0); private final DigitalInput m_irSensor = new DigitalInput(Device.IO_4); private final ArcadeDriveCommand m_autoCommand = null; // new ExampleCommand(m_exampleSubsystem); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { m_driveSubsystem.setDefaultCommand(new ArcadeDriveCommand(m_driveSubsystem, m_joystick)); // Configure the button bindings configureButtonBindings(); } /** * 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() { m_joystick.button(1).onTrue(new DriveForTimeCommand(m_driveSubsystem, 0.50, 3.0)); m_joystick.button(2).onTrue(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 30)); m_joystick.button(3).onTrue(new TestMotorSpeedCommand(m_driveSubsystem)); m_joystick.button(4).whileTrue(new CalibrateSpeedCommand(m_driveSubsystem)); m_joystick.button(5).onTrue(new TurnCommand(m_driveSubsystem, 0.3, -180)); m_joystick.button(6).onTrue(new DriveCourseCommand(m_driveSubsystem)); m_joystick.button(7).onTrue(new DriveToLineCommand(m_driveSubsystem, m_irSensor)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous return m_autoCommand; } } |
Now deploy your program. Place the robot a couple of inches from the black line and verify that it stops when it reaches the line.