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 commnad. Remember to require the DriveSubsystem.

package commands;

import robot.Robot;
import robotCore.Logger;
import robotWpi.command.Command;

/**
 *
 */
public class DriveToLineCommand extends Command 
{
    public DriveToLineCommand() 
    {
    	Logger.Log("DriveToLineCommand", 3, "DriveToLineCommand()");
    	
        // Use requires() here to declare subsystem dependencies
        requires(Robot.m_driveSubsystem);
    }

    // Called just before this Command runs the first time
    protected void initialize() 
    {
    	Logger.Log("DriveToLineCommand", 2, "initialize()");
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() 
    {
    	Logger.Log("DriveToLineCommand", -1, "execute()");
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() 
    {
    	Logger.Log("DriveToLineCommand", -1, "isFinished()");
        
    	return (false);
    }

    // Called once after isFinished returns true
    protected void end() 
    {
    	Logger.Log("DriveToLineCommand", 2, "end()");
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() 
    {
    	Logger.Log("DriveToLineCommand", 2, "interrupted()");
    }
}

The light sensors are connected to the Arduino via one of it’s 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 pin 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.

	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:

    protected void initialize() 
    {
    	Logger.Log("DriveToLineCommand", 2, "initialize()");
    	
    	Robot.m_driveSubsystem.SetSpeed(k_power, k_power);
    }

while defining k_power as:

	private static final double k_power 		= 0.5;

which will make the robot drive forward at half power.

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:

    protected boolean isFinished() 
    {
    	Logger.Log("DriveToLineCommand", -1, "isFinished()");
        
    	return (m_leftSensor.get());
    }

Finally, we need to turn off the motors in the end() functions:

    protected void end() 
    {
    	Logger.Log("DriveToLineCommand", 2, "end()");
    	
    	Robot.m_driveSubsystem.SetSpeed(0,  0);
    }

Your DriveToLineCommand.java file should now look like:

package commands;

import robot.Robot;
import robotCore.DigitalInput;
import robotCore.Logger;
import robotWpi.command.Command;

/**
 *
 */
public class DriveToLineCommand extends Command 
{
	private static final int k_leftSensorPin	= 10;
	private static final double k_power 		= 0.5;
	
	private DigitalInput	m_leftSensor = new DigitalInput(k_leftSensorPin);
	
    public DriveToLineCommand() 
    {
    	Logger.Log("DriveToLineCommand", 3, "DriveToLineCommand()");
    	
        // Use requires() here to declare subsystem dependencies
        requires(Robot.m_driveSubsystem);
    }

    // Called just before this Command runs the first time
    protected void initialize() 
    {
    	Logger.Log("DriveToLineCommand", 2, "initialize()");
    	
    	Robot.m_driveSubsystem.SetSpeed(k_power, k_power);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() 
    {
    	Logger.Log("DriveToLineCommand", -1, "execute()");
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() 
    {
    	Logger.Log("DriveToLineCommand", -1, "isFinished()");
        
    	return (m_leftSensor.get());
    }

    // Called once after isFinished returns true
    protected void end() 
    {
    	Logger.Log("DriveToLineCommand", 2, "end()");
    	
    	Robot.m_driveSubsystem.SetSpeed(0,  0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() 
    {
    	Logger.Log("DriveToLineCommand", 2, "interrupted()");
    	
    	end();
    }
}

Finally you connect this command to our #2 test button. After you have changed your OI.java file it should look something like:

package robot;

import robotCore.Joystick;
import commands.DriveForTimeCommand;
import commands.DriveToLineCommand;
import robotWpi.buttons.Button;
import robotWpi.buttons.JoystickButton;;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
     Joystick m_stick = new Joystick(0);
     Button m_trigger = new JoystickButton(m_stick, 1);
     Button m_testButton = new JoystickButton(m_stick, 2);
     
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    public OI()
    {
    	m_trigger.whenPressed(new DriveForTimeCommand(0.75, 2));
    	m_testButton.whenPressed(new DriveToLineCommand());
    }
}

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.

Next: Escape!