File content as of revision 48:597738b77f77:

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

void __attribute__((noreturn)) doDeadReckoning();

Reckoner reckoner;
LineTracker lineTracker;
TurnSensor turnSensor;
Logger logger;

const uint32_t timeout = 0;

const uint32_t logSpacing = 200;

const int16_t drivingSpeed = 500;

uint32_t totalEncoderCounts = 0;
uint32_t nextLogEncoderCount = 0;


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);
    
    if (l3gInit())
    {
        // Error initializing the gyro.
        setLeds(0, 0, 1, 1);
        while(1);
    }
    
    // Enable pull-ups on encoder pins and give them a chance to settle.
    encodersInit();
    motorsInit();
    buttonsInit();

    // Test routines
    //testButtons();
    //testEncoders();
    //testMotors();
    //testMotorSpeed();
    //testL3g();
    //testL3gAndShowAverage();
    //testTurnSensor();
    //testReckoner();
    //testCloseness();  // didn't do it in 2019
    //testDriveHome();
    //testFinalSettleIn();  // doesn't really work
    //testLineSensorsAndCalibrate();
    //testLineFollowing();
    //testTurnInPlace();  // didn't do it in 2019
    //testLogger();       // didn't do it in 2019

    doDeadReckoning();
}

void doDeadReckoning()
{
    loadLineCalibration();
    doGyroCalibration();
    turnSensor.start();

    setLeds(1, 0, 0, 0);
    waitForSignalToStart();
    
    setLeds(0, 1, 0, 0);    
    findLine();

    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 (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 doGyroCalibration()
{
    wait_ms(1000);  // Time for the robot to stop moving.
    while (!l3gCalibrateDone())
    {
        l3gCalibrate();
        wait_ms(2);
    }
}

void loadLineCalibration()
{
    // QTR-3RC calibration from contest in 2014.
    //lineTracker.calibratedMinimum[0] =  100;
    //lineTracker.calibratedMaximum[0] =  792;
    //lineTracker.calibratedMinimum[1] =   94;
    //lineTracker.calibratedMaximum[1] =  807;
    //lineTracker.calibratedMinimum[2] =  103;
    //lineTracker.calibratedMaximum[2] = 1000;    

    // QTR-3RC calibration from David's home setup, 2019-07-27.
    // Generated with testLineSensorsAndCalibrate().
    lineTracker.calibratedMinimum[0] = 210;
    lineTracker.calibratedMaximum[0] = 757;
    lineTracker.calibratedMinimum[1] = 197;
    lineTracker.calibratedMaximum[1] = 1000;
    lineTracker.calibratedMinimum[2] = 203;
    lineTracker.calibratedMaximum[2] = 746;
}

void updateReckoner()
{
    turnSensor.update();
    reckoner.setTurnAngle(turnSensor.getAngle());

    if (!encoderBuffer.hasEvents())
    {
        return;
    }
    
    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 << RECKONER_LOG_UNIT);
    float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT);
    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()
{
    float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT);
    float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT);
    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())
    {
        updateReckoner();
    }
    reckoner.reset();
    turnSensor.reset();
    while(button1DefinitelyPressed())
    {
        updateReckoner();
    }
    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.sinv * straightDriveStrength >> RECKONER_LOG_UNIT;
    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();       
        updateReckoner();
        loggerService();
        updateMotorsToDriveStraight();
        lineStatus.update(lineTracker.getLineVisible());       

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

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

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

void driveHomeAlmost()
{
    Timer timer;
    timer.start();
    
    while(1)
    {
        updateReckoner();
        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);
        }
        
        // tmphax
        if (0) {
        if (det != det)
        {
            // NaN
            setLeds(1, 0, 0, 1);
        }
        else if (det > 0.5)
        {
            setLeds(0, 0, 1, 1);
        }
        else if (det > 0.1)
        {
            setLeds(0, 0, 0, 1);
        }
        else if (det < -0.5)
        {
            setLeds(1, 1, 0, 0);
        }
        else if (det < -0.1)
        {
            setLeds(1, 0, 0, 0);
        }
        else
        {
            // Heading basically the right direction.
            setLeds(1, 1, 1, 1);
        }
        speedLeft = speedRight = 0;
        }
        
        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);
        
        updateReckoner();
        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.cosv > (1 << (RECKONER_LOG_UNIT - 1)))
        {
            // 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.sinv / (1 << RECKONER_LOG_UNIT);
                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("%5d %5d %11d %11d | %11f %11f\r\n",
              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
              determinant(), dotProduct());
        }
    }
    
    // Done!  Stop moving.
    motorsSpeedSet(0, 0);
}