My FTC team was tired of programming autonomous programs using a string of distance drives; if you needed to make an adjustment, you needed to redo all of them. Bill Gates once said, “I choose a lazy person to do a hard job. Because a lazy person will find an easy way to do it.” Thus, we decided to use our laziness as motivation to build a program that made our programming lives easier. Throughout the season our team developed a dead reckoner (see previous post) and a robot driver that drives the robot to any specified location on the field. That way, if we made an adjustment, it only affects the target point that we changed. This post addresses how our “robot driver” works.

After the dead reckoner is initialized, you can enable the robot driver. To begin driving, you simply add waypoints to the queue and specify how long you want to take to get to each waypoint. A waypoint contains a field relative (X, Y) coordinate, a time budget to get to that waypoint, a time type (“FromNow” or “FromLastWaypoint”), and an id number. Once the queue contains waypoints, the motion will begin. The robot driver then calculates the X and Y error that the robot needs to drive from the current position supplied by the dead reckoner. Then, based on the time budget specified by the user, the robotDriver increments an instantaneous waypoint along the desired path until the robot reaches the destination.

The advantage of using a string of instantaneous waypoints over a single waypoint is that the robot will drive back onto the specified path if it drifts off of that path. Had we used a single waypoint, the robot would simply take the shortest path from where it is; if the robot drifts off path, it then will take the shortest distance from there. This presents a problem if you are driving near obstacles. If the robot is close to an obstacle and it drifts, it may try to drive through the obstacle instead of driving on the specified path. Since there are structures in the middle of the field, it was imperative that the robot stayed on the specified path. Thus, driving by a string of instantaneous waypoints was the best option. In addition, speed control is accomplished by controlling the rate at which the instantaneous waypoint moves along the specified path.

Code Sample:

//Grab the time now
drivingCurrentTime = DSPTimeBased.getNowSeconds();

//Figure out the elapsed time
drivingElapsedTime = drivingCurrentTime - movePlannedStartTime;

//Compute the next instantaneous position
tempInstCoord = (Coordinate.CoordinateType.TerabyteF,
xVel * drivingElapsedTime + startOfDriveCoordinate.getX(),
yVel * drivingElapsedTime + startOfDriveCoordinate.getY(),
rotVel * drivingElapsedTime + startOfDriveCoordinate.getThetaRAD());

//See if we're done with this move: 
//we're basing this entirely on elapsed time
if(drivingElapsedTime >= movePlannedTimeDelta)
{
    //We're done.  Go back to idle
    state = States.Idle;

    //Make sure we didn't over drive the end 
    //point by just using the end coord.
    tempInstCoord = targetWaypoint.getCoord();

    //See if we were waiting to notify of reaching this waypoint

    if(targetWaypoint.getId() == this.waypointIDForTrigger)
    {
        this.waypointTriggered = true;
    }
}
instantaneousCoordinate = tempInstCoord;

The instantaneous waypoint calculated by the robot driver is sent to the position controller. The position controller’s job, is to drive the robot to the instantaneous waypoint calculated by the robot driver. This is accomplished by driving the error to zero using velocities generated by two PI controllers; one in the X direction and one in the Y direction. Then, by harnessing the power of triangles, we use those velocities to calculate a velocity vector (magnitude and direction) which is then rotated onto the robot’s reference frame. Robot rotation is controlled similarly using a PI controller to drive the rotational error to zero.

    robotErrorTheta = fieldErrorTheta - robotTheta;

Position Controller Code:

//We need to pre-compute the error value for 
//the rotational axis because it's strange 
//and loops around.
rotFieldError = (fieldTargetPosition.getThetaRAD() -
    FieldFeedbackPosition.getThetaRAD() + 20.0 * Math.PI) % 
    (2.0 * Math.PI);

if(rotFieldError > Math.PI)
{
    rotFieldError -= 2.0*Math.PI;
}

//Compute the controllers for the three 
//degrees of freedom, in field frame reference.
xFieldVel = this.xController.processInput(
                fieldTargetPosition.getX(),
                fieldFeedbackPosition.getX());

yFieldVel = this.yController.processInput(
                fieldTargetPosition.getY(), 
                fieldFeedbackPosition.getY());

rotFieldVel = this.rotController.processInput(
                rotFieldError,
                0.0);  //Use the rotational error in place

//Get the error terms back because we need 
//them for other checking
xFieldError = this.xController.getError();
yFieldError = this.yController.getError();

//Move these velocities onto the robot frame of reference
//-Magnitude is the same in field frame as in robot frame
linearVelMagnitude = Math.sqrt(
                       xFieldVel * xFieldVel + 
                       yFieldVel * yFieldVel);

//-Find the angle of the velocity 
// command vector and subtract the robot angle
linearVelTheta = Math.atan2(
                       yFieldVel, 
                       xFieldVel) -
                 fieldFeedbackPosition.getThetaRAD();

//Rotational is rotational
rotaionalVelMagnitude = rotFieldVel;

Finally the position controller sends the calculated magnitude and direction to the holonomic drive code which moves the robot. The holonomic drive calculates speeds for each wheel by taking the specified velocity vector and calculating its components along the angular direction that each wheel drives at.

Holonomic Drive Code:

controlsToSpeed(Mag, thetaDEG, r)
{

    rotConst = 0.75;

    Theta1 = (thetaDEG - 45);//Front Left
    Theta2 = (thetaDEG + 45);//Front Right
    Theta3 = (thetaDEG + 135);//Back Right
    Theta4 = (thetaDEG - 135);//Back Left

    Theta1 = Math.toRadians(Theta1);
    Theta2 = Math.toRadians(Theta2);
    Theta3 = Math.toRadians(Theta3);
    Theta4 = Math.toRadians(Theta4);

    vector1 = Mag * Math.cos(Theta1) + (r*rotConst);
    vector2 = Mag * Math.cos(Theta2) + (r*rotConst);
    vector3 = Mag * Math.cos(Theta3) + (r*rotConst);
    vector4 = Mag * Math.cos(Theta4) + (r*rotConst);

    mag1 = Range.clip(vector1, -1, 1);
    mag2 = Range.clip(vector2, -1, 1);
    mag3 = Range.clip(vector3, -1, 1);
    mag4 = Range.clip(vector4, -1, 1);

Upon arriving at a waypoint, the robotDriver then moves on to the next waypoint in the queue. If the queue is empty, the robot driver reports that the motion is complete and the position controller holds the robot’s position on the field. When we tried to manually push the robot, it responded with a satisfying stubbornness: it pushed back!!

Below is a control diagram for the dead reckoning system, as it was first documented.

With our dead reckoner and robot driver in place, our autonomous programming lives became significantly easier. We could reliably develop and test entire autonomous programs in as little as 20-30 minutes (compared to 4-6 hours before). Therefore, one of the best skills of a programmer is building code that makes his/her life easier.

Blog

Robot Localization -- Driving to Field Coordinates

Bridge Fusion Systems LLC