This is the first part of Assignment A - building a reactive control system to keep a robot alive at high speeds.


We've pasted in selected portions of our code below. First, the key function, computeRotationalVelocity:

int computeRotationalVelocity()
    int currTV = State[38];
    int currRV = State[39];

    // always loop if possible
    if(canTurn(1)) return 450;        // try to full-loop left
    if(canTurn(-1)) return -450;      // try to full-loop right

    // in tunnel or narrow.  try to wall-follow
    if(can90(1, currTV)) return 1;    // predict hard left turn coming up
    if(can90(-1, currTV)) return -1;  // predict hard right turn coming up
    if(currRV == 1) return 450;       // was predicting hard left, do it
    if(currRV == -1) return -450;     // was predicting hard right, do it

    // in a turn, see when to straighten out
    int close = closestPt();
    if(((close == 21) || (close == 29)) && (currRV < 430) && (currRV > -430))
        return 0;                     // closest pt is to side and we've been
                                      // turning at least 20 times in row, so
                                      // go straight

    // can't straighten yet, tick "counter"
    if(currRV >= 430) return currRV - 1;
    return currRV + 1;
The basic idea is to find open spaces and go into an infinite loop to the left or right. We first use the following function, canTurn, to see if we're able to make a full 360-degree turn to one side:

bool canTurn(int curDir)
    int sens1, sens2, sens3, sens4, sens5, sens6, sens7, sens8, sens9;

    sens1 = State[17];      // front
    sens9 = State[25];      // back

    if (curDir < 0)         // curDir < 0 means look right
        sens2 = State[32];
        sens3 = State[31];
        sens4 = State[30];
        sens5 = State[29];
        sens6 = State[28];
        sens7 = State[27];
        sens8 = State[26];
    else                    // curDir > 0 means look left
        sens2 = State[18];
        sens3 = State[19];
        sens4 = State[20];
        sens5 = State[21];
        sens6 = State[22];
        sens7 = State[23];
        sens8 = State[24];

    // Basically, test all sensors to see if their readings are >= the minimum
    // value we think we need.  "ok" returns true if the sensor is bigger than
    // the value we're testing (also supposed to be true if the value is >255,
    // because that's beyond our sensor range, but our code has a bug...)

    if (ok(sens1, one) && ok(sens9, one) &&
        ok(sens2, two) && ok(sens8, two) &&
        ok(sens3, three) && ok(sens7, three) &&
        ok(sens4, four) && ok(sens6, four) &&
        ok(sens5, five)) return true;
    return false;

    // "one" through "five" are precomputed in the main function.
    // If our sensors are >= these readings, then hopefully we have just
    // just barely enough room to make a full loop.  Performance is better the
    // closer these values are to the minimum needed.
    // Here are their values:
    radius = tv * 0.4 / PI;  // radius of tightest circle we turn, in inches
    // robRad == robot radius in inches
    one = 0.5 * radius + robRad;  // this is a guess on our part :)
    two = 2 * radius * cos(0.375 * PI) + robRad;   // actual trig for rest of
    three = 2 * radius * cos(0.25 * PI) + robRad;  // these values :)
    four = 2 * radius * cos(0.125 * PI) + robRad;
    five = 2 * radius + robRad;
If we can't make a loop to the left or right, we're in some narrow spot. Our goal is to make it through the narrow part to the end so we can find an opening and thus loop. Our strategy is to move forward until we're about to hit a wall, then turn hard to just barely miss it; then, straighten out and do it again. How do we know if we're about to hit a wall? Well, first, the following code predicts if we have enough room to make a 90-degree left or right turn, after moving forward a short bit:

bool can90(int currDir, int currTV)
    double sens1, sens2, sens3;

    double predict = currTV * LOOKAHEAD_SECONDS;  // amount of forward
                                                  // movement to predict, in
                                                  // inches

    // look at front sensor and two sensors just to one side of front
    sens1 = State[17];
    if(currDir < 0)         // curDir < 0 means look right
        sens2 = State[32];
        sens3 = State[31];
    else                    // curDir < 0 means look left
        sens2 = State[18];
        sens3 = State[19];

    // try to "adjust" sensor values to match what they'd look like after
    // we moved forward a bit.  This is not totally accurate but close for
    // small predictions.  PROBLEM: we don't check sensor just to OPPOSITE
    // side of forward, so if we're about to scrape a wall on one side we
    // may miss it-- this causes us to get in a lot of collisions
    sens1 -= predict;
    double opp = sens2 * sin(0.125 * PI);  // don't try to figure this out :)
    double adj = sens2 * cos(0.125 * PI) - predict;
    sens2 = sqrt(opp * opp + adj * adj);
    opp = sens3 * sin(0.25 * PI);
    adj = sens2 * cos(0.25 * PI) - predict;
    sens3 = sqrt(opp * opp + adj * adj);

    // true means we think we can make the turn even after moving forward
    if(ok(sens1, one) && ok(sens2, two) && ok(sens3, three)) return true;
    return false;
If we can make a successful prediction, we still have some room before we need to start turning; so we save the robot's prediction "state" by turning 0.1 degree to the left or right to indicate we're predicting an upcoming turn. As soon as our prediction fails, we check which direction we're angling and execute a hard turn that way. Of course, if at any time we suddenly notice we can make a full loop to one side, we start doing that instead.

Eventually we want to pull out of the turn, so we have one last bit of code. We check to see if the closest sensor reading is directly to one side, and if so, assume (naively) we're following a wall and should straighten out. However, we don't want to do this IMMEDIATELY after starting a turn, since until we get away from one wall and close to another, the first wall will still be close to our side. So we implement another "state saving" trick: we subtract 0.1 degrees from the magnitude of our turn each time as a counter. Once we're down 2.0 degrees, we assume enough time has elapsed that we're allowed to straighten out. Note that both full 360-degree loops and new 90-degree turn predictions will reset this counter.


A successful run on Map 1 at speed 160. At this point the robot has been through that loop at the bottom about 4 times.

An unsuccessful run (i.e. we died eventually) on Map 1 at speed 170. This was using a shorter lookahead value for our 90-degree turn code (0.15 seconds instead of 0.20) than the rest of the test runs. The interesting thing is that the robot made the right loop about 4 times, then the left loop once, and then crashed.

An unsuccessful run on Map 1 at speed 200. We think there really is enough room at the beginning to do a full loop, even at 200, but the robot doesn't seem to agree, and doesn't survive very long.

A successful run on Map 2 at speed 200. This map is much more wide open and our robot can find the big open spot in the upper right at any speed.

A successful run on our handmade map at speed 130. This map is designed to annoy our robot and we were pleased that it managed to get through the gap near its starting position and exploit a large open space.

An unsuccessful run on our handmade map at speed 140. This time the robot couldn't figure out what to do; it couldn't pull a full loop and didn't think it could make a 90-degree turn, so it finally just plowed straight into the wall as a default behavior.

As you can see, our code really did fairly well. 160 is a very high speed for Map 1, while on Map 2 we can survive at any speed. The problem comes with maps like our handmade test map, where it's hard to find big open spaces. The robot isn't very capable of jittering its way through successive gaps--it keeps trying to find spots to make big hard turns. There are a couple of obvious problem spots to fill in in our code (especially the blind side in the 90-degree prediction section). But overall, we'd be happy to put our code up against any other group's.