Diff: Eurobot_shared/ai/ai.cpp

Revision:

26:0995f61cb7b8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eurobot_shared/ai/ai.cpp	Wed Oct 17 22:22:47 2012 +0000
@@ -0,0 +1,52 @@
+#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;
+}
\ No newline at end of file