File content as of revision 38:5e93a479c244:

#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"

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

const int16_t drivingSpeed = 400;

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.
    encodersInit();
    motorsInit();
    buttonsInit();

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

    // Real routines for the contest.
    loadCalibration();
    
    setLeds(1, 0, 0, 0);
    waitForSignalToStart();
    
    setLeds(0, 1, 0, 0);    
    findLine();
    
    //setLeds(1, 1, 0, 0);
    //turnRightToFindLine();
    
    setLeds(0, 0, 1, 0);
    followLineToEnd();
    
    setLeds(1, 0, 1, 0);
    driveHomeAlmost();
    
    //setLeds(0, 1, 1, 0);
    //finalSettleIn();
    
    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] =  100;
    lineTracker.calibratedMinimum[1] =   94;
    lineTracker.calibratedMinimum[2] =  103;
    lineTracker.calibratedMaximum[0] =  792;
    lineTracker.calibratedMaximum[1] =  807;
    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);
}

// Keep the robot pointing the in the right direction (1, 0).
// This should basically keep it going straight.
void updateMotorsToDriveStraight()
{
    const int32_t straightDriveStrength = 1000;
    int16_t speedLeft = drivingSpeed;
    int16_t speedRight = drivingSpeed;
    int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
    if (reduction > 0)
    {
        speedRight = reduceSpeed(speedRight, reduction);
    }
    else
    {
        speedLeft = reduceSpeed(speedLeft, -reduction);
    }
    motorsSpeedSet(speedLeft, speedRight);   
}

void updateMotorsToFollowLine()
{
    const int 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 findLine()
{   
    GeneralDebouncer lineStatus(10000);
    while(1)
    {
        lineTracker.read();
        lineTracker.updateCalibration();       
        updateReckonerFromEncoders();
        loggerService();
        updateMotorsToDriveStraight();
        lineStatus.update(lineTracker.getLineVisible());       

        if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000)
        {
            break;
        }
    }
}

/**
void turnRightToFindLine()
{   
    while(1)
    {
        lineTracker.read();
        lineTracker.updateCalibration();
        updateReckonerFromEncoders();
        
        if(lineTracker.getLineVisible())
        {
            break;   
        }
        
        motorsSpeedSet(300, 100);        
    }
}**/

void followLineToEnd()
{
    Timer timer;
    timer.start();
    
    GeneralDebouncer lineStatus(10000);
    const uint32_t lineDebounceTime = 1000000;
    
    while(1)
    {
        lineTracker.read();
        updateReckonerFromEncoders();
        loggerService();

        lineStatus.update(lineTracker.getLineVisible());
        
        bool lostLine = lineStatus.getState() == false &&
          lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;        
        if(lostLine && timer.read_us() >= 2000000)
        {
            break;   
        }
        
        updateMotorsToFollowLine();     
    }
}

void driveHomeAlmost()
{
    Timer timer;
    timer.start();
    
    while(1)
    {
        updateReckonerFromEncoders();
        loggerService();

        float magn = magnitude();
        
        if (magn < (1<<(14+7)))  
        {
            // We are within 128 encoder ticks, so go to the next step.
            break;
        }
        
        float det = determinant();
        
        int16_t speedLeft = drivingSpeed;
        int16_t speedRight = drivingSpeed;
        if (magn < (1<<(14+9))) // Within 512 encoder ticks of the origin, so slow down.
        {
            int16_t reduction = (1 - magn/(1<<(14+8))) * (drivingSpeed/2);
            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()
{
    const int16_t settleSpeed = 300;
    const int16_t settleModificationStrength = 150;
    
    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;
    
    Pacer reportPacer(200000);
    Pacer motorUpdatePacer(10000);   

    float integral = 0;
    
    motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling
    
    while(1)
    {
        led1 = (state == 1);
        
        updateReckonerFromEncoders();
        loggerService();

        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() >= 2000 && reckoner.cos > (1 << 29))
        {
            // Stop turning and start trying to maintain the right position.
            state = 1;
        }
        
        if (state == 1 && timer.read_ms() >= 5000)
        {
            // Stop moving.
            break;   
        }
        
        if (motorUpdatePacer.pace())
        {
            int16_t rotationSpeed;
            if (state == 1)
            {
                float s = (float)reckoner.sin / (1 << 30);
                integral += s;
                rotationSpeed = -(s * 2400 + integral * 20);
                
                if (rotationSpeed > 300)
                {
                    rotationSpeed = 300; 
                }
                if (rotationSpeed < -300)
                {
                    rotationSpeed = -300; 
                }
            }
            else
            {
                rotationSpeed = settleSpeed;
            }
            
            int16_t speedLeft = -rotationSpeed + speedModification;
            int16_t speedRight = rotationSpeed + speedModification;
            motorsSpeedSet(speedLeft, speedRight);
        }
        
        if (state == 1 && reportPacer.pace())
        {
            pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
              determinant(), dotProduct());
        }
    }
    
    // Done!  Stop moving.
    motorsSpeedSet(0, 0);
}