bunp and turn.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();
    }
}

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
    }
}

// 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 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() / 128;
    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() / 128;
        // 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");
            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 < 2; i++) {
        if (motors & (1 << i)) {
            if (directions & (1 << i)) {
                bk(i);
            }
            else {
                fd(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);
        }
    }
}