Shuto Naruse / Eurobot_2012_shared

Eurobot_shared pubulished from Eurobot Primary

ai/ai.cpp@0:434fd09723be, 2012-08-07 (annotated)

Committer:
narshu
Date:
Tue Aug 07 10:25:53 2012 +0000
Revision:
0:434fd09723be
[mbed] converted /Eurobot_2012_Primary/Eurobot_shared

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:434fd09723be 1 #include "ai.h"
narshu 0:434fd09723be 2 #include "rtos.h"
narshu 0:434fd09723be 3 #include "globals.h"
narshu 0:434fd09723be 4
narshu 0:434fd09723be 5
narshu 0:434fd09723be 6
narshu 0:434fd09723be 7 AI::AI() :
narshu 0:434fd09723be 8 thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
narshu 0:434fd09723be 9 flag_terminate = false;
narshu 0:434fd09723be 10 flag_motorStop = true;
narshu 0:434fd09723be 11 flag_manOverride = false;
narshu 0:434fd09723be 12 //printf("aistart\r\n");
narshu 0:434fd09723be 13 }
narshu 0:434fd09723be 14
narshu 0:434fd09723be 15
narshu 0:434fd09723be 16 void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing, bool colour, int maxSpeed) {
narshu 0:434fd09723be 17 targetlock.lock();
narshu 0:434fd09723be 18 MOVE_SPEED = maxSpeed;
narshu 0:434fd09723be 19 target.x = targetX;
narshu 0:434fd09723be 20 target.y = targetY;
narshu 0:434fd09723be 21 target.theta = targetTheta;
narshu 0:434fd09723be 22 target.facing = targetfacing;
narshu 0:434fd09723be 23 target.reached = false;
narshu 0:434fd09723be 24 if (!colour) {
narshu 0:434fd09723be 25 target.x = 3000 - target.x;
narshu 0:434fd09723be 26 target.theta = PI - target.theta;
narshu 0:434fd09723be 27
narshu 0:434fd09723be 28 target.theta -= (floor(target.theta/(2*PI)))*2*PI;
narshu 0:434fd09723be 29 if (target.theta < -PI) {
narshu 0:434fd09723be 30 target.theta += 2*PI;
narshu 0:434fd09723be 31 }
narshu 0:434fd09723be 32 if (target.theta > PI) {
narshu 0:434fd09723be 33 target.theta -= 2*PI;
narshu 0:434fd09723be 34 }
narshu 0:434fd09723be 35
narshu 0:434fd09723be 36
narshu 0:434fd09723be 37 }
narshu 0:434fd09723be 38 targetlock.unlock();
narshu 0:434fd09723be 39 }
narshu 0:434fd09723be 40
narshu 0:434fd09723be 41 void AI::settarget(Target targetin) {
narshu 0:434fd09723be 42 targetlock.lock();
narshu 0:434fd09723be 43 target = targetin;
narshu 0:434fd09723be 44 targetlock.unlock();
narshu 0:434fd09723be 45 }
narshu 0:434fd09723be 46
narshu 0:434fd09723be 47 AI::Target AI::gettarget() {
narshu 0:434fd09723be 48 targetlock.lock();
narshu 0:434fd09723be 49 Target temptarget = target;
narshu 0:434fd09723be 50 targetlock.unlock();
narshu 0:434fd09723be 51 return temptarget;
narshu 0:434fd09723be 52 }