Shuto Naruse / Mbed 2 deprecated Eurobot2012_Secondary

Eurobot2012_Secondary

Fork of Eurobot_2012_Secondary by Shuto Naruse

Eurobot_shared/ai/ai.cpp@1:cc2a9eb0bd55, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:25:31 2012 +0000
Revision:
1:cc2a9eb0bd55
Commit before publishing

Who changed what in which revision?

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