Various KISS-C++ questions

#include <ctime>
#include <cmath>
#include <deque>
//#include <kovan/motors.hpp>
 
//! \brief Runs this as a standalone program to make sure it works. Comment it out to use it in a class
//! \todo Test this and remove all the crap testing code
#define DEBUG_LINEFOLLOW 1
#ifdef DEBUG_LINEFOLLOW //I'm moving these into the class so these defines are going away in a non-test situation
//! Port the left motor is on
#define LMOTOR 0
//! Port the right motor is on
#define RMOTOR 1
//! Port the tophat sensor is on
#define TOPHAT 6
//! \brief How wide is the robot in ticks
//! \todo implement a ticks to cm conversion routine using a single define
#define WIDTH 250
#else
#include "Robot.h"
#endif
//! Where does black begin
#define GRAYLOW 256
//! Where does white begin
#define GRAYHIGH 767
//! How biased should it be to turn to the left. Can be negative
#define BIAS 5
//! How much should we normalize the acceleration
#define NORM 3
//! How many loops should it go between checks when it is not lost
#define PREC 5
 
/**
 \brief Follows a line at a set speed and for a set distance
 \author Devon Richards
 \date February 6, 2013
 \param distance approximately how far it should follow the line
 \param speed About how fast it should move
 \exception std::string The line was lost
 \return Whether it finished succesfully
 \todo Optimize this using the ternary operator. Should be simple
 
 This functions does its best to follow a solid black line using an infared sensor.
 It tries to go the approximate distance given to it, but this will not be very
 accurate, especially for large distances or low speeds
 */
#ifdef DEBUG_LINEFOLLOW
bool lineFollow(unsigned int distance, const unsigned short &speed)
#else
bool CBC::lineFollow(unsigned int distance, const unsigned short &speed)
#endif
{
#ifdef DEBUG_LINEFOLLOW
    Motor lmotor(LMOTOR);
    Motor rmotor(RMOTOR);
    Analog irsens(TOPHAT);
#endif
    int timeorig=std::time(NULL), counter=0, turn=0, tmptime=seconds() * 1000;
    std::deque<double> alpha(NORM);
    double theta, omega, timetotal=distance/speed, tmpa=0;
    while(seconds()-timeorig <= timetotal)//loop till the approximate distance has been traveled
    {
        if(counter%PREC==0 || turn>1)//only do the time intensive tests when needed
        {
            counter=0;
            if(irsens.value()>GRAYHIGH)//check if we see white
            {
                for(std::deque<double>::iterator i=alpha.begin(); i!=alpha.end() && i<alpha.end(); i++)//find an average
                {
                    tmpa += *i;
                }
                alpha.pop_front();
                alpha.push_back(tmpa/NORM+0.25);
                tmpa=0;
            }
            else if(irsens.value()<GRAYLOW)//check if we see black
            {
                for(std::deque<double>::iterator i=alpha.begin(); i!=alpha.end() && i<alpha.end(); i++)//find an average
                {
                    tmpa += *i;
                }
                alpha.pop_front();
                alpha.push_back(tmpa/NORM-0.25);
                tmpa=0;
            }
            else//we're in gray so we're happy
            {
                alpha.pop_front();
                alpha.push_back(alpha.back()-omega.back()/2);
                theta=0;
                turn=0;
            }
            omega += alpha.back();
            if (omega>0)//use the correct formulas
            {
                lmotor.MoveAtVelocity(speed+BIAS);
                rmotor.MoveAtVelocity(speed-WIDTH*tan(omega));
            }
            else//use the correct formulas
            {
                lmotor.MoveAtVelocity(speed-WIDTH*tan(-1*omega)
                rmotor.MoveAtVelocity(speed-BIAS);
            }
            theta += omega.back()*(seconds() * 1000-tmptime);
            tmptime=seconds() * 1000.0;
        }
        switch(turn)//Makes sure we aren't turning the wrong way.
        {
        case 0:
            if(theta > 15)
            {
                alpha.pop_front();
                alpha.push_back(alpha.back()*-1);
                turn++;
            }
            break;
        case 1:
            if(theta < -15)
            {
                alpha.pop_front();
                alpha.push_back(alpha.back()+2);
                turn++;
            }
            break;
        case 2:
            if (theta < -170)
            {
                alpha.pop_front();
                alpha.push_back(alpha.back()*-1);
                turn++;
            }
            break;
        case 3:
            if (theta > 170)
            {
                return 0;
            }
        }
        counter++;
        msleep(5);
    }
    return true;
}
#ifdef DEBUG_LINEFOLLOW
//! \test This is just here to allow this to be compiled separately for testing purposes
int main()
{
    Motor lmotor(LMOTOR);
    Motor rmotor(RMOTOR);
    lmotor.MoveRelativePosition(100, 200);
    rmotor.MoveRelativePosition(500, 1000);
    lmotor.BlockMotorDone();
    return lineFollow(2000, 500);
}
#endif