File content as of revision 45:e16e74bbbf8c:

#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;
Logger logger;
Pacer loggerPacer(50000);

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();

    // Real routines for the contest.
    loadCalibration();
    
    setLeds(1, 0, 0, 0);
    waitForSignalToStart();
        
    setLeds(0, 1, 0, 0);   
    followLineFast();
    
    setLeds(1, 1, 1, 1);
    loggerReportLoop();
}

void loggerService()
{
    if (loggerPacer.pace())
    {
        logger.log();   
    }
}

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();
            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;
        }
    }
}

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

float dotProduct()
{
    float s = (float)reckoner.sin / (1 << 30);
    float c = (float)reckoner.cos / (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.sin / (1 << 30);
    float c = (float)reckoner.cos / (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 updateMotorsToFollowLine()
{
    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()
{
    const int16_t drivingSpeed = 1100;
    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()
{   
    Pacer reportPacer(200000);
    
    loadCalibration();
    uint32_t loopCount = 0;
    Timer timer;
    timer.start();
    while(1)
    {
        loopCount += 1;
        updateReckonerFromEncoders();
        loggerService();
        
        if ((loopCount % 256) == 0)
        {
            pc.printf("%d\r\n", lineTracker.getLinePosition());
        }
        
        lineTracker.read();
        updateMotorsToFollowLineFast();
        
        if (button1DefinitelyPressed())
        {
            break;
        }
    }
    motorsSpeedSet(0, 0);
}