File content as of revision 50:937e860f4621:

#include "ai.h"

namespace AI
{

void ailayer(void const *dummy)
{
    Waypoint *current_waypoint = new Waypoint[5];

    current_waypoint[0].x = 0.5;
    current_waypoint[0].y = 0.5;
    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.5;
    current_waypoint[1].theta = 0;
    current_waypoint[1].pos_threshold = 0.05;
    current_waypoint[1].angle_threshold = 0.05*PI;

    motion::setNewWaypoint(current_waypoint);
    
    int currwptidx = 0;
    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(c_upper.getColour() == BLUE && c_lower.getColour() == RED){
                motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
                motion::clearWaypointReached();
            }

        }
        motion::waypoint_flag_mutex.unlock();
    }
}

} //namespace