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 function driveToLine():
|
1 2 3 4 5 |
public Command driveToLine() { return run(coroutine -> { }).named("driveToLine"); } |
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.
Lets create an instance if DigitalInput in our DriveMechanism class called m_irSensor . Looking at its constructor, we see that the constructor for DigitalInput takes the number of the pin to be read. The IR sensor is connected to pin Minibot.IRSensor, so add the following to your DriveMechanism:
|
1 2 3 4 5 |
private PWMMotor m_leftMotor = new PWMMotor(Minibot.LeftMotorPWM, Minibot.LeftMotorDir); private PWMMotor m_rightMotor = new PWMMotor(Minibot.RightMotorPWM, Minibot.RightMotorDir); private Encoder m_leftEncoder = new Encoder(EncoderType.Quadrature, Minibot.LeftEncoderInt, Minibot.LeftEncoderDir); private Encoder m_rightEncoder = new Encoder(EncoderType.Quadrature, Minibot.RightEncoderInt, Minibot.RightEncoderDir); private final DigitalInput m_irSensor = new DigitalInput(Minibot.IRSensor); |
Note that DigitalInput will probably come up as undefined and you will need to use the “Quick Fix” to include the correct import. Be sure you choose the one that comes from RobotCore.
Now what we need to do is:
- Start the robot driving forward by calling setSpeed(…)
- Create a while loop which loops until the sensor “sees” the black line (i.e. until m_irSensor.get() returns true
- Stop the robot when the loop finishes.
Take a crack at writing this code yourself and compare it to the following solution.
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
public Command driveToLine() { return run(coroutine -> { Logger.log("driveToLine", 1, "start"); setSpeed(0.5, 0.5); while (!m_irSensor.get()) { coroutine.yield(); } stop(); Logger.log("driveToLine", 1, "end"); }).whenCanceled(this::stop).named("driveToLine"); } |
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 |
public class TeleopOpMode implements OpMode { public TeleopOpMode(Robot robot) { Logger.log("TeleopOpMode", 2, "TeleopOpMode"); robot.m_driveMechanism.setDefaultCommand(robot.m_driveMechanism.arcadeDrive( () -> robot.m_cmdJoystick.getX(), () -> robot.m_cmdJoystick.getY())); robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime(0.75, 2)); robot.m_cmdJoystick.getHID().button(1).onTrue(robot.m_driveMechanism.driveForDistance(0.75, 15.0)); robot.m_cmdJoystick.getHID().button(2).onTrue(robot.m_driveMechanism.testMotorSpeed()); robot.m_cmdJoystick.getHID().button(3).whileTrue(robot.m_driveMechanism.calibrateSpeed()); robot.m_cmdJoystick.getHID().button(4).onTrue(robot.m_driveMechanism.turn(0.3, -180)); robot.m_cmdJoystick.getHID().button(5).onTrue(robot.m_driveMechanism.driveCourse()); robot.m_cmdJoystick.getHID().button(6).onTrue(robot.m_driveMechanism.driveToLine()); } |
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.