Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/ai/ai.cpp@24:7a3906c2f5d5, 2012-05-04 (annotated)

Committer:
narshu
Date:
Fri May 04 05:23:45 2012 +0000
Revision:
24:7a3906c2f5d5
1st working version with accurate target acquisition.

Who changed what in which revision?

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