File content as of revision 55:05c8f439497d:

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

#include "main.h"
#include "motors.h"
#include "encoders.h"
#include "leds.h"
#include "pc_serial.h"
#include "test.h"
#include "reckoner.h"
#include "buttons.h"
#include "line_tracker.h"
#include "l3g.h"
#include "turn_sensor.h"

Reckoner reckoner;
LineTracker lineTracker;
TurnSensor turnSensor;
Logger logger;
Pacer loggerPacer(50000);

uint8_t lapsCompleted = 0;
uint32_t totalEncoderCounts = 0;
uint32_t nextLogEncoderCount = 0;
const uint32_t logSpacing = 100;

void setLeds(bool v1, bool v2, bool v3, bool v4)
{
   led1 = v1;
   led2 = v2;
   led3 = v3;
   led4 = v4;
}

int __attribute__((noreturn)) main()
{
    pc.baud(115200);
    
    // Enable pull-ups on encoder pins and give them a chance to settle.
    if (l3gInit())
    {
        // Error initializing the gyro.
        setLeds(0, 0, 1, 1);
        while(1);
    }

    encodersInit();
    motorsInit();
    buttonsInit();

    // Test routines
    //testMotors();
    //testEncoders();
    //testLineSensors();
    //testReckoner();
    //testButtons();
    //testDriveHome();
    //testFinalSettleIn();
    //testCalibrate();
    //testLineFollowing();
    //testAnalog();
    //testSensorGlitches();
    //testTurnInPlace();
    //testCloseness();
    //testLogger();
    //testL3g();
    //testTurnSensor();
    //testReckoningWithGyro();

    // Real routines for the contest.
    loadCalibration();
    
    setLeds(1, 0, 0, 0);
    waitForSignalToStart();
  
    setLeds(0, 1, 0, 0);    // led3 and led4 get set by followLineSmart for debugging
    //followLineFast();
    followLineSmart();
    
    setLeds(1, 1, 1, 1);
    loggerReportLoop();
}

void loggerService()
{
    // loggerPacer.pace()
    if (totalEncoderCounts > nextLogEncoderCount)
    {
        nextLogEncoderCount += logSpacing;
        
        struct LogEntry entry;
        entry.turnAngle = turnSensor.getAngle() >> 16;
        entry.x = reckoner.x >> 16;
        entry.y = reckoner.y >> 16;
        logger.log(&entry);
    }
}

void loggerReportLoop()
{
    while(1)
    {
        if(button1DefinitelyPressed())
        {
            logger.dump();
        }
    }   
}


void loadCalibration()
{
    /** QTR-3RC **/
    lineTracker.calibratedMinimum[0] =  137;
    lineTracker.calibratedMinimum[1] =  132;
    lineTracker.calibratedMinimum[2] =  154;
    lineTracker.calibratedMaximum[0] =  644;
    lineTracker.calibratedMaximum[1] =  779;
    lineTracker.calibratedMaximum[2] = 1000;    
    
    /** QTR-3A
    lineTracker.calibratedMinimum[0] = 34872;
    lineTracker.calibratedMinimum[1] = 29335;
    lineTracker.calibratedMinimum[2] = 23845;
    lineTracker.calibratedMaximum[0] = 59726;
    lineTracker.calibratedMaximum[1] = 60110;
    lineTracker.calibratedMaximum[2] = 58446;
    **/
}

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

void updateReckoner(TurnSensor & turnSensor)
{
    if (!encoderBuffer.hasEvents())
    {
        return;
    }
    
    reckoner.setTurnAngle(turnSensor.getAngle());
    
    while(encoderBuffer.hasEvents())
    {
        PololuEncoderEvent event = encoderBuffer.readEvent();
        switch(event)
        {
        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
            totalEncoderCounts++;
            reckoner.handleForward();
            break;
        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
            reckoner.handleBackward();
            totalEncoderCounts--;
            break;
        }
    }
}

float magnitude()
{
    return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);   
}

float dotProduct()
{
    float s = (float)reckoner.sinv / (1 << 30);
    float c = (float)reckoner.cosv / (1 << 30);
    float magn = magnitude();
    if (magn == 0){ return 0; }    
    return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
}

// 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 determinant()
{
    // TODO: get rid of the magic numbers here (i.e. 30)
    float s = (float)reckoner.sinv / (1 << 30);
    float c = (float)reckoner.cosv / (1 << 30);
    return (reckoner.x * s - reckoner.y * c) / magnitude();
}

int16_t reduceSpeed(int16_t speed, int32_t reduction)
{
    if (reduction > speed)
    {
        return 0;   
    }
    else
    {
        return speed - reduction;
    }
}

void waitForSignalToStart()
{
    while(!button1DefinitelyPressed())
    {
        updateReckonerFromEncoders();
    }
    reckoner.reset();
    while(button1DefinitelyPressed())
    {
        updateReckonerFromEncoders();
    }
    wait(0.2);
}

void updateMotorsToFollowLineSlow()
{
    const int16_t drivingSpeed = 400;
    const int32_t followLineStrength = drivingSpeed * 5 / 4;

    int16_t speedLeft = drivingSpeed;
    int16_t speedRight = drivingSpeed;
    int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
    if(reduction < 0)
    {
        speedLeft = reduceSpeed(speedLeft, -reduction);   
    }
    else
    {
        speedRight = reduceSpeed(speedRight, reduction);
    }
    
    motorsSpeedSet(speedLeft, speedRight);
}

void updateMotorsToFollowLineFast(int16_t drivingSpeed)
{
    const int32_t followLineStrength = drivingSpeed * 5 / 4;
    static int16_t lastPosition = 1000;

    int16_t position = lineTracker.getLinePosition();

    int16_t speedLeft = drivingSpeed;
    int16_t speedRight = drivingSpeed;
    int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200;
    if(veer > 0)
    {
        speedRight = reduceSpeed(speedRight, veer);
    }
    else
    {
        speedLeft = reduceSpeed(speedLeft, -veer);   
    }
    
    motorsSpeedSet(speedLeft, speedRight);
    
    lastPosition = position;
}

void followLineFast()
{
    totalEncoderCounts = 0;
    Pacer reportPacer(200000);
    
    loadCalibration();
    Timer timer;
    timer.start();
    turnSensor.start();
    while(1)
    {
        turnSensor.update();
        updateReckoner(turnSensor);
        loggerService();
        
        lineTracker.read();
        updateMotorsToFollowLineFast(1000);
        
        if (button1DefinitelyPressed())
        {
            break;
        }
    }
    motorsSpeedSet(0, 0);
}

bool foundStart()
{
    static int32_t lastX = 0;
    bool result = lastX < 0 && reckoner.x >= 0 && abs(reckoner.y) < (85 << 16) &&
      totalEncoderCounts > 10000 && abs(turnSensor.getAngle()) < turnAngle1 * 30;
    lastX = reckoner.x;
    return result;
}

bool onLongStraightPart()
{
    if (lapsCompleted == 0) { return false; }
    
    // Figure out what part of the log corresponds to our current situation.
    uint32_t logIndex = totalEncoderCounts / logSpacing;
    
    if (logIndex >= logger.getSize())
    {
        // Should not happen.
        return false;
    }
    
    // To improve this, we could check that turnSensor.getAngle() matches what is in the log.

    uint32_t angle1 = turnSensor.getAngleUnsigned();

    // 2000 encoder ticks
    const uint32_t lookAheadAmount = 3000 / logSpacing;
    
    // Figure out how far away the next turn is.
    uint32_t i = logIndex;
    while(1)
    {
        i++;
        
        if (i >= logger.getSize())
        {
            // reached the end the log
            return false;
        }

        if (i > logIndex + lookAheadAmount)
        {
            // looked far enough ahead that we don't think there is a turn coming up soon
            return true;
        }
        
        
        uint32_t angle2 = (uint16_t)logger.entries[i].turnAngle << 16;
        if (abs((int32_t)(angle2 - angle1)) > turnAngle45)
        {
            // detected a turn
            return false;
        }
    }
}

void followLineSmart()
{
    lapsCompleted = 0;   
    totalEncoderCounts = 0;

    Pacer reportPacer(200000);
    
    loadCalibration();
    turnSensor.start();
    while(1)
    {
        turnSensor.update();
        updateReckoner(turnSensor);
        loggerService();
        
        lineTracker.read();
        
        // By default, choose a cautious speed of 1000 (out of 1200).
        int16_t speed = 1000;
        
        // Go fast if we know we are on a long straight part.
        if (onLongStraightPart())
        {
            speed = 1200;
            led4 = 1;
        }
        else
        {
            led4 = 0;
        }
        
        
        updateMotorsToFollowLineFast(speed);

        if (foundStart())
        {
            // Another lap has been completed!
            lapsCompleted += 1;
            led3 = lapsCompleted & 1;

            reckoner.reset();
            turnSensor.reset();
            totalEncoderCounts = 0;
            nextLogEncoderCount = 0;
        }
        
        if (lapsCompleted == 3 && totalEncoderCounts > 2000)
        {
            // Classy move: know when you are done with the competition and stop automatically.
            // (Of course, there is a risk that this will backfire!)
            break;
        }
        
        if (button1DefinitelyPressed())
        {
            break;
        }
    }
    motorsSpeedSet(0, 0);
}