File content as of revision 29:cfcf08d8ac79:

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

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

void testAnalogReadWithFilter()
{
    AnalogIn testInput(p18);
    Pacer reportPacer(1000000);
    uint32_t badCount = 0, goodCount = 0;
    while(1)
    {
        uint16_t reading = analogReadWithFilter(&testInput);
        if(reading > 100)
        {
            badCount += 1;
            pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount);   
        }
        else
        {
            goodCount += 1;   
        }
        
        if (reportPacer.pace())
        {
            pc.printf("Hello\r\n");   
        }
    }
}

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();
        updateMotorsToFollowLine();
        
        loopCount += 1;
        
        if (lineVisiblePrevious != lineTracker.getLineVisible())
        {
            pc.printf("%5d ! %1d %4d | %4d %4d %4d | %5d %5d %5d\r\n",
                loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
                lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2],
                lineTracker.rawValues[0], lineTracker.rawValues[1], lineTracker.rawValues[2]
                );
        }
        
        if (reportPacer.pace())
        {
            pc.printf("%5d   %1d %4d | %4d %4d %4d\r\n", loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
                lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
                );
        }
    }
}

void testDriveHome()
{
    led1 = 1;   
    while(!button1DefinitelyPressed())
    {
        updateReckonerFromEncoders();
    }
    driveHomeAlmost();
    finalSettleIn();
    infiniteReckonerReportLoop();     
}

void testFinalSettleIn()
{
    led1 = 1;
    while(!button1DefinitelyPressed())
    {
        updateReckonerFromEncoders();
    }   
    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()
{
    Pacer reportPacer(100000);
    while(1)
    {
        updateReckonerFromEncoders();
        led1 = (reckoner.cos > 0);
        led2 = (reckoner.sin > 0);
        led3 = (reckoner.x > 0);
        led4 = (reckoner.y > 0);
        
        if (reportPacer.pace())
        {
            pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
              encoderLeft.getCount(), encoderRight.getCount(), determinant());
        }
    }
}

void testLineSensors()
{
    led1 = 1;
    Pacer reportPacer(100000);
    bool const printBarGraph = true;
    while (1)
    {
        if (reportPacer.pace())
        {
            uint16_t left = lineSensorsAnalog[0].read_u16();
            uint16_t middle = lineSensorsAnalog[1].read_u16();
            uint16_t right = lineSensorsAnalog[2].read_u16();
            
            if (printBarGraph)
            {
                pc.printf("\x1B[0;0H");  // VT100 command for "go to 0,0"
                printBar("L", left);
                printBar("M", middle);
                printBar("R", right);
            }
            else
            {
                pc.printf("%8d %8d %8d\n", left, middle, right);
            }
        }
    }
}

// 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();
        }

        led2 = calibrationLooksGood();
        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]);
            }
            if (calibrationLooksGood())
            {
                pc.puts("Good.        \r\n");
            }
            else
            {
                pc.puts("Not good yet.\r\n");   
            }
        }
    }
}

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\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)
    {
        if(reportPacer.pace())
        {
            led4 = 1;
            pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
              determinant(), dotProduct());
            led4 = 0;
       }
    }
   
}

void printBar(const char * name, uint16_t adcResult)
{
    pc.printf("%-2s %5d |", name, adcResult);
    uint8_t width = adcResult >> 10;
    uint8_t i;
    for(i = 0; i < width; i++){ pc.putc('#'); }
    for(; i < 63; i++){ pc.putc(' '); }
    pc.putc('|');
    pc.putc('\n');
}