REV 2M Distance Sensor

Part 0: Reading the Sensor

Add the following objects an existing project:

Add a hardwareMap for your sensor:

For a first test, add some telemetry to your while(opModeIsActive) section…

Click and then run your opMode from the Driver Station. Try pointing the sensor towards a book or something… Move the book towards it, then away from it. If your sensor is outputting values that make sense, you can move on.

Part 1: A Basic While Loop

We want our motors to stop cold when no power is given to them, so let’s add these commands at the top right below your hardwareMaps…

Add this function inside your class, but below the runOpMode() method like this:

Call it in your while loop:

Part 2: Using some math to get more accurate

The big problem with our last attempt is that it typically overshoots the target. The robot is going full speed when it exits the loop and physics says it can’t stop in an instant. This next approach is an attempt to avoid this overshoot.

void driveToDistanceWhile(double targetCM){
    double power = 0; 
    double error = 0; 
    double totalDistanceToTravel = sensorRange.getDistance(DistanceUnit.CM) - targetCM;
    while(sensorRange.getDistance(DistanceUnit.CM) > targetCM){
        error = sensorRange.getDistance(DistanceUnit.CM)-targetCM; 
        power = error/totalDistanceToTravel;
        power = Math.sqrt(power);  
        frontLeft.setPower(power);
        backLeft.setPower(power);
        frontRight.setPower(power);
        backRight.setPower(power);
 } 
         frontLeft.setPower(0);
        backLeft.setPower(0);
        frontRight.setPower(0);
        backRight.setPower(0);
}

Lets try to understand how we calculate power…

1. The farther we are from the target, the faster we want to go. As we get closer, we should slow down.

2. Because the setPower() method of the DcMotor class accepts values between -1.0 and 1.0, we need to calculate a power in that range that is still proportionate to the distance we need to go.

3. totalDistanceToTravel only gets calculated once, so it is our reference point for the total,

4. We repeatedly calculate the error, to see how far we still need to go.

5. power is a percentage of how far we need to go compared to the total

6. at the beginning power will be 1 because error = totalDistanceToTravel

7. in the middle, power will be 0.5 because error will be half of totalDistanceToTravel

8. at the end, power will be 0 because error will be 0.

9. Before we set the power, we actually square root it. This helps because sqrt(1) = 1 while sqrt(.5) = .717 while the sqrt(0) = 0. Effectively, the power stays high a little longer and dies off strongly as we get really close to our target. Look at this graph to see it more clearly.