File content as of revision 57:99bec7fab454:

// 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 "reckoner.h"
#include "buttons.h"
#include "l3g.h"
#include "turn_sensor.h"

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

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

void testCloseness()
{
    led1 = 1;
    while(1)
    {
        updateReckonerFromEncoders();
        float magn = magnitude();
        
        led3 = (magn < (1<<(14+7)));
        led4 = (magn < (1<<(14+9)));
    }
}

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

void testTurnInPlace()
{
    led1 = 1;
    while(!button1DefinitelyPressed())
    {
        updateReckonerFromEncoders();
        showOrientationWithLeds34();
    }
    led2 = 1;
    
    Pacer motorUpdatePacer(10000);
    Timer timer;
    timer.start();
    motorsSpeedSet(-300, 300);
    while(timer.read_ms() < 4000)
    {
        updateReckonerFromEncoders();
        showOrientationWithLeds34();
    }
    timer.reset();
    
    float integral = 0;
    while (timer.read_ms() < 4000)
    {
        if (motorUpdatePacer.pace())
        {
            int16_t rotationSpeed;
            float s = (float)reckoner.sinv / (1 << 30);
            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();      
}


void testSensorGlitches()
{
    AnalogIn testInput(p18);
    Pacer reportPacer(1000000);
    uint32_t badCount = 0, goodCount = 0;
    pc.printf("hi\r\n");
    
    //uint16_t riseCount = 0;
    uint16_t reading = 0xFF;
    
    while(1)
    {
        /** This digital filtering did not work
        {
            wait(0.01);
            uint16_t raw = testInput.read_u16();
            if (raw < reading)
            {
                riseCount = 0;
                reading = raw;
            }
            else
            {
                riseCount++;
                if (riseCount == 10)
                {
                    riseCount = 0;
                    reading = raw;   
                }
            }
        }
        **/

        uint16_t values[LINE_SENSOR_COUNT];        
        readSensors(values);
        reading = values[0];
        
        if(reading > 100)
        {
            badCount += 1;
            //pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount);
        }
        else
        {
            goodCount += 1;
        }
        
        if (reportPacer.pace())
        {
            pc.printf("h %5d %11d %11d\r\n", reading, badCount, goodCount);
        }
    }
}

void testAnalog()
{
    AnalogIn testInput(p18);
    
    DigitalOut pin20(p20);
    DigitalOut pin19(p19);
    //DigitalOut pin18(p18);
    DigitalOut pin17(p17);
    DigitalOut pin16(p16);
    DigitalOut pin15(p15);
    
    pin20 = 0;
    pin19 = 0;
    //pin18 = 0;
    pin17 = 0;
    pin16 = 0;
    pin15 = 0;
    
    uint32_t badCount = 0, goodCount = 0;
    
    Pacer reportPacer(1000000);
    while(1)
    {
        uint16_t reading = testInput.read_u16();
        if(reading > 100)
        {
            badCount += 1;
            pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount);   
        }
        else
        {
            goodCount += 1;   
        }
        
        if (reportPacer.pace())
        {
            pc.printf("Hello\r\n");   
        }
    }
}

// This also tests the LineTracker by printing out a lot of data from it.
void testLineFollowing()
{
    led1 = 1;   
    while(!button1DefinitelyPressed())
    {
        updateReckonerFromEncoders();
    }
    led2 = 1;
    
    Pacer reportPacer(200000);
    
    loadCalibration();
    uint16_t loopCount = 0;
    while(1)
    {
        updateReckonerFromEncoders();
        bool lineVisiblePrevious = lineTracker.getLineVisible();
        lineTracker.read();
        updateMotorsToFollowLineSlow();
        loopCount += 1;
        
        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 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()
{
    Pacer reportPacer(100000);
    while(1)
    {
        updateReckonerFromEncoders();
        
        led1 = (reckoner.x > 0);
        led2 = (reckoner.y > 0);
        showOrientationWithLeds34();
        
        if (reportPacer.pace())
        {
            pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
              encoderLeft.getCount(), encoderRight.getCount(), determinant());
        }
    }
}

void testLineSensors()
{
    led1 = 1;
    Pacer reportPacer(100000);
    Pacer clearStatsPacer(2000000);
    
    uint16_t min[LINE_SENSOR_COUNT];
    uint16_t max[LINE_SENSOR_COUNT];
    
    bool const printBarGraph = true;
    while (1)
    {
        if (clearStatsPacer.pace())
        {
            for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
            {
                min[i] = 0xFFFF;
                max[i] = 0;
            }
        }
        
        //values[0] = lineSensorsAnalog[0].read_u16();
        //values[1] = lineSensorsAnalog[1].read_u16();
        //values[2] = lineSensorsAnalog[2].read_u16();

        uint16_t values[3];
        readSensors(values);
        
        for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
        {
            if (values[i] > max[i]){ max[i] = values[i]; }
            if (values[i] < min[i]){ min[i] = values[i]; }
        }
        
        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]);
            }
        }
    }
}

// Values from David's office     Values from dev lab,
// in the day time, 2014-02-27:   2014-02-27:
// #  calmin calmax
// 0  34872 59726                 0  40617 60222
// 1  29335 60110                 1  36937 61198
// 2  23845 58446                 2  33848 58862
void testCalibrate()
{
    Timer timer;
    timer.start();
    
    Pacer reportPacer(200000);
    
    bool doneCalibrating = false;
    
    led1 = 1;
    
    while(1)
    {
        lineTracker.read();
        if(!doneCalibrating)
        {
            lineTracker.updateCalibration();
        }

        led3 = doneCalibrating;
        led4 = lineTracker.getLineVisible();
        
        if (button1DefinitelyPressed())
        {
            doneCalibrating = true;
        }
        
        if (reportPacer.pace())
        {
            pc.printf("\x1B[0;0H");  // VT100 command for "go to 0,0"
            for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
            {
                pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
            }
        }
    }
}

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(1);
        
        motorsSpeedSet(0, 0);
        led3 = 1;
        wait(2);
        
        motorsSpeedSet(1200, 1200);
        wait(1);
    }
}

void infiniteReckonerReportLoop()
{
    Pacer reportPacer(200000);
    while(1)
    {
        showOrientationWithLeds34();
        if(reportPacer.pace())
        {
            pc.printf("%11d %11d %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');
}

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("%d, %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("%d, %d\r\n", timer.read_us(), gz);
            reportedReading = true;
        }
    }
}

void testTurnSensor()
{
    pc.printf("Test turn sensor\r\n");
    Pacer reportPacer(200000);
    TurnSensor turnSensor;
    turnSensor.start();
    while(1)
    {
        turnSensor.update();
        if (reportPacer.pace())
        {
            pc.printf("%d\r\n", turnSensor.getAngleDegrees());
        }
    }
}

void testReckoningWithGyro()
{   
    setLeds(0, 0, 0, 1);
    waitForSignalToStart();
    setLeds(1, 0, 0, 1);

    loadCalibration();
    Timer timer;
    timer.start();
    TurnSensor turnSensor;
    turnSensor.start();
    while(1)
    {
        turnSensor.update();
        updateReckoner(turnSensor);
        loggerService();
        
        if (button1DefinitelyPressed())
        {
            break;
        }
    }
    motorsSpeedSet(0, 0);
    
    setLeds(1, 1, 1, 1);
    loggerReportLoop();
}