File content as of revision 46:adcd57a5e402:

#include "ai.h"

namespace AI
{

void ailayer(void const *dummy)
{
    Waypoint *current_waypoint = new Waypoint[5];
/*
    current_waypoint[0].x = 1;
    current_waypoint[0].y = 1;
    current_waypoint[0].theta = 0.0;
    current_waypoint[0].pos_threshold = 0.05;
    current_waypoint[0].angle_threshold = 0.05*PI;

    current_waypoint[1].x = 2.2;
    current_waypoint[1].y = 1.5;
    current_waypoint[1].theta = PI/2;
    current_waypoint[1].pos_threshold = 0.05;
    current_waypoint[1].angle_threshold = 0.05*PI;

    current_waypoint[2].x = -999;
*/

    current_waypoint[0].x = 0.5;
    current_waypoint[0].y = 1.65;
    current_waypoint[0].theta = 0.0;
    current_waypoint[0].pos_threshold = 0.05;
    current_waypoint[0].angle_threshold = 0.05*PI;
    
    current_waypoint[1].x = 2.5;
    current_waypoint[1].y = 0.45;
    current_waypoint[1].theta = 0.0;
    current_waypoint[1].pos_threshold = 0.05;
    current_waypoint[1].angle_threshold = 0.05*PI;    

    current_waypoint[2].x = 1.2;
    current_waypoint[2].y = 0.18;
    current_waypoint[2].theta = 0;
    current_waypoint[2].pos_threshold = 0.01;
    current_waypoint[2].angle_threshold = 0.00001;

    current_waypoint[3].x = -999;
/*
    //TODO: temp current waypoint hack
    current_waypoint = new Waypoint;
    current_waypoint->x = 0.5;
    current_waypoint->y = 0.7;
    current_waypoint->theta = 0.0;
    current_waypoint->pos_threshold = 0.05;
    current_waypoint->angle_threshold = 0.05*PI;
    
    Waypoint* secondwp = new Waypoint;
    secondwp->x = 1.20;
    secondwp->y = 0.18;
    secondwp->theta = PI;
    secondwp->pos_threshold = 0.01;
    secondwp->angle_threshold = 0.00001;
*/    
    motion::setNewWaypoint(current_waypoint);
        Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
    Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
    while(1)
    {

        Thread::wait(50);
        
        
        motion::waypoint_flag_mutex.lock();
        if (motion::checkWaypointStatus())
        {
            
            if (((current_waypoint+1)->x != -999) && ((c_upper.getColour()==BLUE) || (c_lower.getColour()==RED)))
            { 
                motion::clearWaypointReached();
                current_waypoint++;
                motion::setNewWaypoint(current_waypoint);
            }
        }
        motion::waypoint_flag_mutex.unlock();
    }
}

} //namespace