Using Sensors
While these little robots don’t have gyros or cameras, they do have two light sensors 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.
/*----------------------------------------------------------------------------*/
/* 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 robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Logger;
/**
* An example command that uses an example subsystem.
*/
public class DriveToLineCommand extends CommandBase {
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 Arduino 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. We see that the constructor for DigitalInput takes the number of the pin to be read. Looking at how the robot is wired, we see that the left sensor is attached to pin 10.
public class DriveToLineCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private static final int k_leftSensorPin = 10;
private DigitalInput m_leftSensor = new DigitalInput(k_leftSensorPin);
Note that, once again, we have defined a constant for the pin number and used that constant in the call to the constructor.
In the initialize() function we need to start our robot driving forward:
public void initialize() {
Logger.Log("DriveToLineCommand", 2, "initialize()");
m_subsystem.setSpeed(k_speed, k_speed);
}
while defining k_speed as:
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:
public boolean isFinished() {
Logger.Log("DriveToLineCommand", -1, "isFinished()");
return (m_leftSensor.get());
}
Finally, we need to turn off the motors in the end() functions:
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:
/*----------------------------------------------------------------------------*/
/* 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 robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.DigitalInput;
import robotCore.Logger;
/**
* An example command that uses an example subsystem.
*/
public class DriveToLineCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private static final int k_leftSensorPin = 10;
private static final double k_speed = 0.5;
private DigitalInput m_leftSensor = new DigitalInput(k_leftSensorPin);
/**
* 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()");
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_leftSensor.get());
}
}
Finally connect this command to button 7 on the joystick:
/*----------------------------------------------------------------------------*/
/* 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 robot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import robot.commands.ArcadeDriveCommand;
import robot.commands.DriveCourseCommand;
import robot.commands.DriveForDistanceCommand;
import robot.commands.DriveForTimeCommand;
import robot.commands.DriveToLineCommand;
import robot.commands.ExampleCommand;
import robot.commands.TestMotorSpeedCommand;
import robot.commands.TestSpeedControlCommand;
import robot.commands.TurnCommand;
import robot.subsystems.DriveSubsystem;
import robot.subsystems.ExampleSubsystem;
import robotCore.Joystick;
/**
* 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 Joystick m_joystick = new Joystick(0);
private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);
private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);
private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);
private final JoystickButton m_button4 = new JoystickButton(m_joystick, 4);
private final JoystickButton m_button5 = new JoystickButton(m_joystick, 5);
private final JoystickButton m_button6 = new JoystickButton(m_joystick, 6);
private final JoystickButton m_button7 = new JoystickButton(m_joystick, 7);
private final ExampleCommand 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_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 20));
m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));
m_button4.whileHeld(new TestSpeedControlCommand(m_driveSubsystem));
m_button5.whenPressed(new TurnCommand(m_driveSubsystem, 0.5, -180));
m_button6.whenPressed(new DriveCourseCommand(m_driveSubsystem));
m_button7.whenPressed(new DriveToLineCommand(m_driveSubsystem));
}
/**
* 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.