File content as of revision 20:dbec34f0e76b:

#include <mbed.h>
#include <Pacer.h>
#include <math.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();
    testFinalSettleIn();

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

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, int16_t reduction)
{
    if (reduction > speed)
    {
        return 0;   
    }
    else
    {
        return speed - reduction;
    }
}

void driveHomeAlmost();
void finalSettleIn();

void driveHome()
{
    driveHomeAlmost();
    finalSettleIn();
}

void driveHomeAlmost()
{
    led1 = 1; led2 = 1; led3 = 0; led4 = 0;
    
    bool dir = false;
    uint16_t transitions = 0;
    Timer timer;
    timer.start();
    
    const int16_t maxSpeed = 300;
    
    while(1)
    {
        updateReckonerFromEncoders();
        
        float magn = magnitude();
        
        if (magn < (1<<17))  
        {
            // We are within 8 encoder ticks, so go to the next step.
            break;
        }
        
        float det = determinant();
        
        {
            bool nextDir = det > 0;
            if (nextDir != dir) { transitions++; }
            dir = nextDir;
        }
        
        int16_t speedLeft = maxSpeed;
        int16_t speedRight = maxSpeed;
        if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
        {
            int16_t reduction = (1 - magn/(1<<20)) * maxSpeed;
            speedLeft = reduceSpeed(speedLeft, reduction);
            speedRight = reduceSpeed(speedRight, reduction);
        }
        
        if (det > 0)
        {
            speedLeft = reduceSpeed(speedLeft, det * 1000);
        }
        else
        {
            speedRight = reduceSpeed(speedRight, -det * 1000);               
        }
        motorsSpeedSet(speedLeft, speedRight);
    }
    
    motorsSpeedSet(0, 0);
}

void finalSettleIn()
{
    led1 = 1; led2 = 1; led3 = 1; led4 = 0;
    
    const int16_t settleSpeed = 200;
    const int16_t settleModificationStrength = 100;
    
    Timer timer;
    timer.start();
    
    // State 0: rotating
    // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
    uint8_t state = 0;
    
    while(1)
    {
        updateReckonerFromEncoders();
        
        float dot = dotProduct();
        int16_t speedModification = -dot * settleModificationStrength;
        if (speedModification > settleModificationStrength)
        {
            speedModification = settleModificationStrength;    
        }
        else if (speedModification < -settleModificationStrength)
        {
            speedModification = -settleModificationStrength;
        }
        
        if (state == 0 && timer.read_ms() >= 6000 && reckoner.cos > (1 << 29))
        {
            led1 = 1; led2 = 0; led3 = 1; led4 = 0;
            state = 1;
        }
        
        int16_t rotationSpeed;
        if (state == 1)
        {
            float s = (float)reckoner.sin / (1 << 30);
            rotationSpeed = -s * 300;
        }
        else
        {
            rotationSpeed = settleSpeed;
        }
        
        int16_t speedLeft = -rotationSpeed + speedModification;
        int16_t speedRight = rotationSpeed + speedModification;
        motorsSpeedSet(speedLeft, speedRight);
    }
    
    led1 = 1; led2 = 1; led3 = 1; led4 = 1;
    motorsSpeedSet(0, 0);
}