ai.cpp

00001 #include "ai.h"
00002 #include "rtos.h"
00003 #include "globals.h"
00004 
00005 
00006 
00007 AI::AI() :
00008         thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
00009     flag_terminate = false;
00010     flag_motorStop = true;
00011     flag_manOverride = false;
00012     //printf("aistart\r\n");
00013 }
00014 
00015 
00016 void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing, bool colour, int maxSpeed) {
00017     targetlock.lock();
00018     MOVE_SPEED = maxSpeed;
00019     target.x = targetX;
00020     target.y = targetY;
00021     target.theta = targetTheta;
00022     target.facing = targetfacing;
00023     target.reached = false;
00024     if (!colour) {
00025         target.x = 3000 - target.x;
00026         target.theta = PI - target.theta;
00027 
00028         target.theta -= (floor(target.theta/(2*PI)))*2*PI;
00029         if (target.theta < -PI) {
00030             target.theta += 2*PI;
00031         }
00032         if (target.theta > PI) {
00033             target.theta -= 2*PI;
00034         }
00035 
00036 
00037     }
00038     targetlock.unlock();
00039 }
00040 
00041 void AI::settarget(Target targetin) {
00042     targetlock.lock();
00043     target = targetin;
00044     targetlock.unlock();
00045 }
00046 
00047 AI::Target AI::gettarget() {
00048     targetlock.lock();
00049     Target temptarget = target;
00050     targetlock.unlock();
00051     return temptarget;
00052 }