File content as of revision 18:b65fbb795396:

#include <mbed.h>
#include <Pacer.h>

#include "motors.h"
#include "encoders.h"
#include "leds.h"
#include "pc_serial.h"
#include "test.h"
#include "reckoner.h"
#include "buttons.h"

int __attribute__((noreturn)) main()
{
    pc.baud(115200);
    
    // Enable pull-ups on encoder pins and give them a chance to settle.
    encodersInit();
    motorsInit();
    buttonsInit();

    // Test routines
    //testMotors();
    //testEncoders();
    //testLineSensors();
    //testReckoner();
    //testButtons();
    testDriveHome();

    while(1)
    {

    }
}

void updateReckonerFromEncoders()
{
    while(encoderBuffer.hasEvents())
    {
        PololuEncoderEvent event = encoderBuffer.readEvent();
        switch(event)
        {
        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
            reckoner.handleTickLeftForward();
            break;
        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
            reckoner.handleTickLeftBackward();
            break;
        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
            reckoner.handleTickRightForward();
            break;
        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
            reckoner.handleTickRightBackward();
            break;
        }
    }
}

// The closer this is to zero, the closer we are to pointing towards the home position.
// It is basically a cross product of the two vectors (x, y) and (cos, sin).
float det()
{
    // TODO: get rid of the magic numbers here (i.e. 30)
    float s = (float)reckoner.sin / (1 << 30);
    float c = (float)reckoner.cos / (1 << 30);
    return reckoner.x * s - reckoner.y * c;
}

void __attribute__((noreturn)) driveHome()
{
    led1 = 1; led2 = 1; led3 = 0; led4 = 0;
    
    // First, point the robot at the goal.
    bool dir = false;
    uint16_t transitions = 0;
    Timer timer;
    timer.start();
    while(transitions < 100 || timer.read_ms() < 500)
    {
        updateReckonerFromEncoders();
        {
            bool nextDir = det() > 0;
            if (nextDir != dir) { transitions++; }
            dir = nextDir;
        }
        
        if(dir)
        {
            led3 = 1;
            motorsSpeedSet(-300, 300);   
        }
        else
        {
            led3 = 0;
            motorsSpeedSet(300, -300);
        }
    }
    motorsSpeedSet(0, 0);
    
    while(1)
    {
        
    }
}