File content as of revision 22:44c032e59ff5:

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

// If the calibration stops working, we could just use these values from David's office in the day time:
// #  calmin calmax
// 0  34872 59726
// 1  29335 60110
// 2  23845 58446
void testCalibrate()
{
    Timer timer;
    timer.start();
    
    Pacer reportPacer(200000);
    
    while(1)
    {
        lineTracker.read();
        lineTracker.updateCalibration();
        led4 = calibrationLooksGood();
        
        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');
}