File content as of revision 48:597738b77f77:

// A file for testing routines that will not be used in the final firmware.

#include <mbed.h>
#include "motors.h"
#include <Pacer.h>

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

void __attribute__((noreturn)) infiniteReckonerReportLoop();
void printBar(const char * name, uint16_t adcResult);

void testLogger()
{
    led1 = 1;
    while(!button1DefinitelyPressed())
    {
        led3 = logger.isFull();
    
        updateReckoner();
        loggerService();
    }
    led2 = 1;
    loggerReportLoop();
}

void testCloseness()
{
    doGyroCalibration();
    turnSensor.start();
    led1 = 1;
    while(1)
    {
        updateReckoner();
        float magn = magnitude();
        
        led3 = magn < (1 << 5);
        led4 = magn < (1 << 7);
    }
}

void showOrientationWithLeds34()
{
    led3 = reckoner.cosv > 0;
    led4 = reckoner.sinv > 0;
}

void testTurnInPlace()
{
    doGyroCalibration();
    turnSensor.start();

    led1 = 1;
    while(!button1DefinitelyPressed())
    {
        updateReckoner();
        showOrientationWithLeds34();
    }
    led2 = 1;
    
    Pacer motorUpdatePacer(10000);
    Timer timer;
    timer.start();
    motorsSpeedSet(-300, 300);
    while(timer.read_ms() < 4000)
    {
        updateReckoner();
        showOrientationWithLeds34();
    }
    timer.reset();
    
    float integral = 0;
    while (timer.read_ms() < 4000)
    {
        if (motorUpdatePacer.pace())
        {
            int16_t rotationSpeed;
            float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT);
            integral += s;
            rotationSpeed = -(s * 2400 + integral * 20);
            
            if (rotationSpeed > 450)
            {
                rotationSpeed = 450; 
            }
            if (rotationSpeed < -450)
            {
                rotationSpeed = -450; 
            }
            
            int16_t speedLeft = -rotationSpeed;
            int16_t speedRight = rotationSpeed;
            motorsSpeedSet(speedLeft, speedRight);
        }
    }
    
    infiniteReckonerReportLoop();      
}

// This also tests the LineTracker by printing out a lot of data from it.
void testLineFollowing()
{
    loadLineCalibration();
    doGyroCalibration();
    turnSensor.start();

    led1 = 1;
    while (!button1DefinitelyPressed())
    {
        updateReckoner();
    }
    
    Pacer reportPacer(200000);
    
    uint16_t loopCount = 0;
    while(1)
    {
        updateReckoner();
        bool lineVisiblePrevious = lineTracker.getLineVisible();
        lineTracker.read();
        updateMotorsToFollowLine();
        
        loopCount += 1;
        
        led2 = lineTracker.calibratedValues[0] > 500;
        led3 = lineTracker.calibratedValues[1] > 500;
        led4 = lineTracker.calibratedValues[2] > 500;

        if (lineVisiblePrevious != lineTracker.getLineVisible())
        {
            pc.printf("%5d ! %1d %4d | %5d %5d | %4d %4d %4d\r\n",
                loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
                motorLeftSpeed, motorRightSpeed,
                lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
                );
        }
        
        if (reportPacer.pace())
        {
            pc.printf("%5d   %1d %4d | %5d %5d | %4d %4d %4d\r\n",
                loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
                motorLeftSpeed, motorRightSpeed,
                lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
                );
        }
    }
}

void testDriveHome()
{
    doGyroCalibration();
    turnSensor.start();
    led1 = 1;   
    while(!button1DefinitelyPressed())
    {
        updateReckoner();
    }
    
    setLeds(1, 0, 1, 0);
    driveHomeAlmost();
    
    //setLeds(0, 1, 1, 0);
    //finalSettleIn();
    
    setLeds(1, 1, 1, 1);
    infiniteReckonerReportLoop();     
}

void testFinalSettleIn()
{
    doGyroCalibration();
    turnSensor.start();
    led1 = 1;
    while(!button1DefinitelyPressed())
    {
        updateReckoner();
    }   
    finalSettleIn();
    infiniteReckonerReportLoop();     
}


void testButtons()
{
    led1 = 1;
    
    while(!button1DefinitelyReleased());
    while(!button1DefinitelyPressed());
    led2 = 1;

    while(!button1DefinitelyReleased());
    while(!button1DefinitelyPressed());
    led3 = 1;

    while(!button1DefinitelyReleased());
    while(!button1DefinitelyPressed());
    led4 = 1;
   
    while(1){};
}

void testReckoner()
{
    doGyroCalibration();
    turnSensor.start();
    led1 = 1;
    Pacer reportPacer(100000);
    while(1)
    {
        updateReckoner();
        
        led1 = reckoner.x > 0;
        led2 = reckoner.y > 0;
        showOrientationWithLeds34();
        
        if (reportPacer.pace())
        {
            pc.printf("%6d %6d %11d %11d | %8d %8d %10f\r\n",
              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
              encoderLeft.getCount(), encoderRight.getCount(), determinant());
        }
    }
}

void testTurnSensor()
{
    pc.printf("Test turn sensor\r\n");
    doGyroCalibration();
    led1 = 1;
    //Pacer reportPacer(200000);  // 0.2 s
    Pacer reportPacer(10000000);  // 10 s
    Timer timer;
    timer.start();
    turnSensor.start();
    while(1)
    {
        turnSensor.update();
        if (reportPacer.pace())
        {
            pc.printf("%u, %d, %d\r\n",
              timer.read_ms(),
              turnSensor.getAngleDegrees(),
              turnSensor.getAngleMillidegrees());
        }
    }
}

void testL3gAndShowAverage()
{
    wait_ms(2000);
    Pacer reportPacer(750000);
    Pacer readingPacer(2000);
    int32_t total = 0;
    int32_t readingCount = 0;
    while(1)
    {
        if (readingPacer.pace())
        {
          int32_t result = l3gZAvailable();
          if (result == 1)
          {
              int32_t gz = l3gZRead();
              if (gz < -500000)
              {
                  pc.printf("l3gZRead error: %d\n", gz);
              }
              else
              {
                  total += gz;
                  readingCount += 1;
              }
          }
          else if (result != 0)
          {
              pc.printf("l3gZAvailable error: %d\n", result);
          }
        }

        if (reportPacer.pace())
        {
            float average = (float)total / readingCount;
            pc.printf("%d, %d, %f\r\n", total, readingCount, average);
        }
    }
    
    // Gyro calibration results get hardcoded into TurnSensor::update()
    // for now until we figure out something better.
}

void testL3g()
{
    Pacer reportPacer(750000);
    Timer timer;
    timer.start();
    int32_t gz = 0;
    bool reportedReading = false;
    while(1)
    {
        int32_t result = l3gZAvailable();
        if (result == 1)
        {
            gz = l3gZRead();
            reportedReading = false;
            if (gz > 100 || gz < -100)
            {
                pc.printf("%u, %d\r\n", timer.read_us(), gz); 
                reportedReading = true;
            }
        }
        else if (result != 0)
        {
            pc.printf("l3gZAvailable error: %d\n", result);
        }

        if (reportPacer.pace() && !reportedReading)
        {
            pc.printf("%u, %d\r\n", timer.read_us(), gz);
            reportedReading = true;
        }
    }
}

void testLineSensorsAndCalibrate()
{
    Pacer reportPacer(100000);
    
    const uint16_t * values = lineTracker.rawValues;
    const uint16_t * min = lineTracker.calibratedMinimum;
    const uint16_t * max = lineTracker.calibratedMaximum;
    const uint16_t * calValues = lineTracker.calibratedValues;

    // Comment this out, and hold down button 1 while exposing the line sensor
    // to its typical surfaces to do calibration.
    loadLineCalibration();

    const bool printBarGraph = true;
    while (1)
    {        
        lineTracker.read();
        
        if (button1DefinitelyPressed())
        {
            led1 = 0;
            lineTracker.updateCalibration();
        }
        else
        {
            led1 = 1;
        }
        
        led2 = calValues[0] > 500;
        led3 = calValues[1] > 500;
        led4 = calValues[2] > 500;
        
        if (reportPacer.pace())
        {
            if (printBarGraph)
            {
                pc.printf("\x1B[0;0H");  // VT100 command for "go to 0,0"
                printBar("L", values[0]);
                printBar("M", values[1]);
                printBar("R", values[2]);
                pc.printf("%4d %4d    \r\n", min[0], max[0]);
                pc.printf("%4d %4d    \r\n", min[1], max[1]);
                pc.printf("%4d %4d    \r\n", min[2], max[2]);
            }
            else
            {
                pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]);
            }
        }
    }
}

void testMotorSpeed()
{
    led1 = 1;
    motorsSpeedSet(400, 400);
    wait_ms(4000);
    uint32_t left = encoderLeft.getCount();
    uint32_t right = encoderRight.getCount();
    motorsSpeedSet(0, 0);
    Pacer reportPacer(500000);
    while (1)
    {
        if (reportPacer.pace())
        {
            led2 = 1;
            pc.printf("%8d %8d\r\n", left, right);
            led2 = 0;
        }    
    }
}

void testEncoders()
{
    Pacer reportPacer(500000);
    led1 = 1;
    while(1)
    {
        while(encoderBuffer.hasEvents())
        {
            PololuEncoderEvent event = encoderBuffer.readEvent();
        }
        
        if(reportPacer.pace())
        {
            led2 = 1;
            pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount());
            led2 = 0;
       }
    }
}

void testMotors()
{
    led1 = 1;
    led2 = 0;
    led3 = 0;
    while(1)
    {
        motorsSpeedSet(0, 0);
        led2 = 0;
        led3 = 0;
        wait(2);
        
        motorsSpeedSet(300, 300);
        wait(2);
        
        motorsSpeedSet(-300, 300);
        wait(2);
        
        motorsSpeedSet(0, 0);
        led2 = 1;
        wait(2);
        
        motorsSpeedSet(600, 600);
        wait(2);
        
        motorsSpeedSet(0, 0);
        led3 = 1;
        wait(2);
        
        motorsSpeedSet(1200, 1200);
        wait(2);
    }
}

void infiniteReckonerReportLoop()
{
    Pacer reportPacer(200000);
    while(1)
    {
        showOrientationWithLeds34();
        if(reportPacer.pace())
        {
            pc.printf("%6d %6d %11d %11d | %11f %11f\r\n",
              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
              determinant(), dotProduct());
       }
    }
}

// with should be between 0 and 63
void printBar(const char * name, uint16_t result)
{
    pc.printf("%-2s %5d |", name, result);
    uint16_t width = result >> 4;
    if (width > 63) { width = 63; }
    uint8_t i;
    for(i = 0; i < width; i++){ pc.putc('#'); }
    for(; i < 63; i++){ pc.putc(' '); }
    pc.putc('|');
    pc.putc('\r');
    pc.putc('\n');
}