File content as of revision 67:be3ea5450cc7:

#include "ai.h"
#include "rtos.h"
#include "globals.h"
#include "motion.h"
#include "Colour.h"
#include "supportfuncs.h"
#include "Arm.h"


namespace AI
{

void ailayer(void const *dummy)
{
    Waypoint current_waypoint;

    current_waypoint.x = 2.2;
    current_waypoint.y = 1.85;
    current_waypoint.theta = (-3.0f/4.0f)*PI;
    current_waypoint.pos_threshold = 0.03;
    current_waypoint.angle_threshold = 0.02*PI;

    motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
    motion::collavoiden = 1;

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

    float r = 0.61+0.02+0.01;

    bool firstavoidstop = 1;
    for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
    {
        motion::waypoint_flag_mutex.lock();
        if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop))
        {
            //temphack!!!!!
            Thread::wait(1000);
            arm::upper_arm.go_down();
            Thread::wait(2000);
            arm::upper_arm.go_up();
            
            phi -= 22.5/180*PI;
            current_waypoint.x = 1.5-r*cos(phi);
            current_waypoint.y = 2-r*sin(phi);
            current_waypoint.theta = constrainAngle(phi+PI/2);
            
            //arm offset
            current_waypoint.x += 0.0425*cos(current_waypoint.theta);
            current_waypoint.y += 0.0425*sin(current_waypoint.theta);
            
            current_waypoint.pos_threshold = 0.01;
            current_waypoint.angle_threshold = 0.01*PI;

            motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
            if (firstavoidstop){
                motion::collavoiden = 0;
                firstavoidstop = 0;
            }
            else
                motion::collavoiden = 1;
        }
        motion::waypoint_flag_mutex.unlock();            
        
        Thread::wait(50);
    }
}

} //namespace