File content as of revision 0:434fd09723be:

#include "ai.h"
#include "rtos.h"
#include "globals.h"



AI::AI() :
        thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
    flag_terminate = false;
    flag_motorStop = true;
    flag_manOverride = false;
    //printf("aistart\r\n");
}


void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing, bool colour, int maxSpeed) {
    targetlock.lock();
    MOVE_SPEED = maxSpeed;
    target.x = targetX;
    target.y = targetY;
    target.theta = targetTheta;
    target.facing = targetfacing;
    target.reached = false;
    if (!colour) {
        target.x = 3000 - target.x;
        target.theta = PI - target.theta;

        target.theta -= (floor(target.theta/(2*PI)))*2*PI;
        if (target.theta < -PI) {
            target.theta += 2*PI;
        }
        if (target.theta > PI) {
            target.theta -= 2*PI;
        }


    }
    targetlock.unlock();
}

void AI::settarget(Target targetin) {
    targetlock.lock();
    target = targetin;
    targetlock.unlock();
}

AI::Target AI::gettarget() {
    targetlock.lock();
    Target temptarget = target;
    targetlock.unlock();
    return temptarget;
}