File content as of revision 21:c279c6a83671:

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

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