everything.c

int runningProgram = 0; // For changing programs
long millis = 0L;       // Keeping track of time between switch hits
//                         ... and any future time use.

void main() {
    // Set up time tracking variable
    millis = mseconds();
    // Turn off those pesky motors
    ao();
    // Decide what program to run
    changeProgram();
    // Run programs forever!
    // Programs are functions that return when they're done
    // ... this will mostly be when the program is changed
    for(;;) {
        if (runningProgram == 0) bunp();
        if (runningProgram == 1) dance();
        if (runningProgram == 2) testPhotocells();
        if (runningProgram == 3) towardsTheLight();
        if (runningProgram == 4) fightFire();
    }
}

void bunp() {
    int alternateBunpCount = 0; // Keeping track of corner bunps
    int sameBunpCount = 0;      // Keeping track of edge grind corner bunps
    int lastDirections = 0b10;  // Which way the dude turned last
    engageMotors(0b11, 0b00);   // Bez forwards
    // Infinite loop with no delay, fast sampling
    for (;;) {
        int d14 = digital(14); // What are the switches doing?
        int d15 = digital(15);
        // Before we worry about the switches, should we stop?
        if (stop_button()) {
            // Stopping gives us a chance to change program, or just pause
            ao();
            // If the program has changed, return, ending the program
            if (changeProgram()) return;
            // If we're not changing program, keep going
            engageMotors(0b11, 0b00);
        }
        // Any switch pressed
        if (d14 || d15) {
            int directions = 0b10;      // Which way to turn (default clockwise)
            long turnTime = 165L;       // How long to turn for (can change)
            //                             ... default around 30 degrees
            if (d15) directions = 0b01; // Hit the right switch, go c/clockwise
            // Less than 1.25s since we last finished a loop
            if (mseconds() - millis < 1250L) {
                // Hit a different switch to last time
                if (directions != lastDirections) {
                    ++alternateBunpCount;
                    sameBunpCount = 0;
                }
                // Hit the same switch as last time
                else {
                    ++sameBunpCount;
                    alternateBunpCount = 0;
                }
            }
            else {
                // Otherwise we're cool, reset the counts
                alternateBunpCount = 0;
                sameBunpCount = 0;
            }
            // Hit different switches in fast succession
            // ... then we're probs in a corner
            if (alternateBunpCount > 1) {
                // Turn slightly longer to get out (around 90 degrees)
                turnTime = 500L;
                alternateBunpCount = 0;
            }
            // Hitting the same switch 3 times in a short time?
            // ... probably grinding a wall into a corner
            if (sameBunpCount > 2) {
                // So go the other way
                // int directions is a bit field of motor directions remember?
                // ... so this funny stuff with 3 reverses the least two bits
                directions = 3 - directions;
                sameBunpCount = 0;
            }
            // So now we've figured what we're gonna do, do it
            // Bez backwards to get clear
            engageMotors(0b11, 0b11);
            // ... for a second
            msleep(1000L);
            // Turn whatever way we're going
            engageMotors(0b11, directions);
            // ... for however long
            msleep(turnTime);
            // And head for the sunset/wall
            engageMotors(0b11,0b00);
            // Don't forget to set variables for the next loop
            lastDirections = directions;
            millis = mseconds();
        }
        // If we haven't hit a switch, who cares, do the loop again
    }
}

void testPhotocells() {
    for (;;) {
        if (stop_button()) {
            ao();
            if (changeProgram()) return;
        }
        msleep(165L);
        print3Numbers(analog(4), analog(3), analog(2));
        engageMotors((start_button() & 1) << 2, 0);
    }
}

void towardsTheLight() {
    int oldDirection = 0b01;
    for(;;) {
        int direction = 0b01;
        if (stop_button()) {
            ao();
            if (changeProgram()) return;
        }
        if (analog(2) > analog(4)) direction = 0b10;
        if (
            analog(2) > analog(4) - 1
            && analog(2) < analog(4) + 1
            && analog(3) > analog(2)
            && analog(3) > analog(4)
            ) direction = 0b00;
        if (direction != oldDirection) {
            engageMotors(0b11, direction);
            oldDirection = direction;
        }
    }
}

void fightFire() {
    // Spin arround looking for readings that're high enough
    // (for x long in total with y samples)
    // Remember time spun for highest reading
    // Spin opposite way to highest reading
    // (this way we don't need to know exactly how long is 360 degrees)
    // Move towards it, stopping to take samples and correcting direction
    // When getting a high enough reading, blow
    // Do it all again
    long firstScanDuration = 4000L;
    long secondScanDuration = 1000L;
    long startTimes[4];
    long brightestTimes[4];
    long startTime = mseconds();
    long brightestTime = startTime;
    int brightest = 255;
    long lastAdjustment;
    int direction = 0b01;
    int i = 0;
    engageMotors(0b11, direction);
    direction = 3 - direction;
    while (mseconds() < startTime + firstScanDuration) {
        int a2 = analog(2);
        int a4 = analog(4);
        int brightness = a2;
        if (a2 > a4) {
            brightness = a4;
        }
        if (brightness < brightest) {
            brightest = brightness;
            brightestTime = mseconds();
        }
    }
    engageMotors(0b11, direction);
    direction = 3 - direction;
    msleep(
           startTime
           + firstScanDuration
           - brightestTime
           + (long) ((int) secondScanDuration / 2));
    for (i = 0; i < 4; ++i) {
        ao();
        msleep(250L);
        brightest = 255;
        engageMotors(0b11, direction);
        startTimes[i] = mseconds();
        while (mseconds() < startTimes[i] + secondScanDuration) {
            int a2 = analog(2);
            int a4 = analog(4);
            int brightness = a2;
            if (a2 > a4) {
                brightness = a4;
            }
            if (brightness < brightest) {
                brightest = brightness;
                brightestTimes[i] = mseconds();
            }
        }
        direction = 3 - direction;
    }
    engageMotors(0b11, direction);
    msleep(
           (long) (
                   (int) (
                          (brightestTimes[0] - startTimes[0])
                          + (secondScanDuration - (brightestTimes[1] - startTimes[1]))
                          + (brightestTimes[2] - startTimes[2])
                          + (secondScanDuration - (brightestTimes[3] - startTimes[3]))
                          )
                   / 4
                   )
           );
    engageMotors(0b11, 0b00);
    lastAdjustment = mseconds();
    while(analog(3) > 10) {
        int a2 = analog(2) + 5;
        int a4 = analog(4);
        print3Numbers(a2, brightest, a4);
        if (stop_button()) {
            ao();
            if (changeProgram()) return;
            engageMotors(0b11, 0b00);
        }
        if (
            mseconds() > lastAdjustment + 125L
            && (
                a2 < brightest + 5
                || a4 < brightest + 5
                )
            ) {
            lastAdjustment = mseconds();
            if (a2 != a4) {
                int direction = 0b10;
                if (a2 < a4) direction = 0b01;
                engageMotors(0b11, direction);
                msleep(50L);
                engageMotors(0b11, 0);
            }
        }   
    }
    for(;;) {
        engageMotors(0b100, 0);
        while(analog(3) < 30) {
            if (stop_button()) {
                ao();
                if (changeProgram()) return;
                engageMotors(0b100, 0);
            }
        }
        msleep(2500L);
        ao();
        msleep(2000L);
        if (analog(3) > 30) break;
    }
    ao();
    beep();
    printf("\nDone!");
    engageMotors(0b11, 0b11);
    msleep(1000L);
}

void print3Numbers(int first, int second, int third) {
    // as long as they're less than 999
    char s[17];
    int firstSpacesEnd = 3 - ((first / 10 > 0) + (first / 100 > 0));
    int secondSpacesEnd = firstSpacesEnd + 5
      - ((second / 10 > 0) + (second / 100 > 0));
    int thirdSpacesEnd = secondSpacesEnd + 5
      - ((third / 10 > 0) + (third / 100 > 0));
    int i;
    s[0] = 10;
    for (i = 1; i < firstSpacesEnd; ++i) {
        s[i] = ' ';
    }
    s[firstSpacesEnd] = '%';
    s[firstSpacesEnd + 1] = 'd';
    s[firstSpacesEnd + 2] = ' ';
    for (i = firstSpacesEnd + 3; i < secondSpacesEnd; ++i) {
        s[i] = ' ';
    }
    s[secondSpacesEnd] = '%';
    s[secondSpacesEnd + 1] = 'd';
    s[secondSpacesEnd + 2] = ' ';
    for (i = secondSpacesEnd + 3; i < thirdSpacesEnd; ++i) {
        s[i] = ' ';
    }
    s[thirdSpacesEnd] = '%';
    s[thirdSpacesEnd + 1] = 'd';
    s[thirdSpacesEnd + 2] = ' ';
    
    s[thirdSpacesEnd + 3] = 0;
    
    printf(s, first, second, third);
}

// A fun program just to test program switching
void dance() {
    // Spin!
    engageMotors(0b11, 0b01);
    for (;;) {
        stop_press();
        ao();
        if (changeProgram()) return;
        engageMotors(0b11, 0b01);
    }
}

int changeProgram() {
    int numberOfPrograms = 5;
    int divideBy = 256 / numberOfPrograms;
    int veryOldProgram = runningProgram;
    // Get a number between 0 and n - 1 programs
    // ... by dividing the knob value by 256/n programs as an int
    // Also remember the knob can be turned without us checking it
    int program = knob() / divideBy;
    int oldProgram = -1; // -1 will always be different, so the first iteration 
    //                      will always print something
    // Wait for the start button
    while(!start_button()) {
        program = knob() / divideBy;
        if (program == numberOfPrograms) program = numberOfPrograms - 1;
        // If a different program from the knob
        if (program != oldProgram) {
            // Print the change to the "laser display board"
            if (program == 0) printf("\nprogram: bunp");
            if (program == 1) printf("\nprogram: dance");
            if (program == 2) printf("\nprogram: test photocells");
            if (program == 3) printf("\nprogram: towards the light");
            if (program == 4) printf("\nprogram: fight fire");
            oldProgram = program;
        }
    }
    runningProgram = program;
    // True if the program has changed, or false if not
    // Program functions should return on true
    if (veryOldProgram != program) return  1;
    return 0;
}

void engageMotors(int motors, int directions) {
    // Work with motors based on two bit fields
    // motors: 0 is off, 1 is on
    // directions: 0 is forwards, 1 is backwards
    int i;
    for (i = 0; i < 3; i++) {
        if (motors & (1 << i)) {
            if (directions & (1 << i)) {
                bk(i);
            }
            else {
                fd(i);
            }
        }
        else {
            off(i);
        }
    }
}

void disengageMotors(int motors) {
    // Turn off some motors based on a bit field
    int i;
    for (i = 0; i < 2; i++) {
        if (motors & (1 << i)) {
            off(i);
        }
    }
}