File content as of revision 31:739b91331f31:

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

uint16_t readPin18()
{
    // Set PCADC bit in PCONP register.  (Table 46).
    LPC_SC->PCONP |= (1 << 12);
    
    // Select PCLK_ADC in PCLKSEL0 register. (Table 40, Table 531).
    LPC_SC->PCLKSEL0 |= (3 << 24);  // PCLK for ADC = CCLK/8 
    
    // Enable ADC0 through PINSEL registers.  (Section 8.5).
    LPC_PINCON->PINSEL1 = (LPC_PINCON->PINSEL1 & ~(3 << 20)) | (1 << 20);
    
    // Pin 18: P0.26/AD0.3/AOUT/RXD3
    
    // 7:0   Bitmap to select what channel to use
    // 15:8  Select clock.
    // 16    BURST = 0
    // 20:17 Reserved
    // 21    PDN = 1, A/D converter is operational
    // 26:24 START = 001 to start conversion now
    LPC_ADC->ADCR = (1 << 3) | (0xFF << 8) | (1 << 21);
    LPC_ADC->ADCR |=  (1 << 24);  
    
    while(!(LPC_ADC->ADGDR >> 31 & 1))  // while not done
    {
    }
    //return 2;
    return LPC_ADC->ADGDR & 0xFFFF;
}

uint16_t readP10()
{
    DigitalInOut pin(p10);
    pin.mode(PullNone);
    pin.output();
    pin = 1;
    wait_us(20);
    uint16_t value = 1000;
    Timer timer;
    timer.start();
    pin.input();
    while(timer.read_us() < 1000)
    {
        if(pin.read() == 0)
        {
            return timer.read_us();   
        }
    }
    return value;
}

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;   
                }
            }
        }
        **/
        reading = readP10();
        
        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();
        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);
    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();
        }

        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\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)
    {
        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;
       }
    }
   
}

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