ai.cpp

00001 
00002 #include "ai.h"
00003 #include "rtos.h"
00004 #include "globals.h"
00005 
00006 AI::AI() :
00007     thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
00008     flag_terminate = false;
00009     //printf("aistart\r\n");
00010 }
00011 
00012 void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing){
00013     targetlock.lock();
00014     target.x = targetX;
00015     target.y = targetY;
00016     target.theta = targetTheta;
00017     target.facing = targetfacing;
00018     targetlock.unlock();
00019 }
00020 
00021 void AI::settarget(Target targetin){
00022     targetlock.lock();
00023     target = targetin;
00024     targetlock.unlock();
00025 }
00026 
00027 AI::Target AI::gettarget(){
00028     targetlock.lock();
00029     Target temptarget = target;
00030     targetlock.unlock();
00031     return temptarget;
00032 }
00033 
00034 void AI::ai_thread () {
00035     /*
00036     //printf("aithreadstart\r\n");
00037     Thread::signal_wait(0x01);
00038     settarget(660, 400, PI/2, true);
00039     
00040     Thread::signal_wait(0x01);
00041     settarget(660, 570, PI, true);
00042     
00043     Thread::signal_wait(0x01);
00044     settarget(400, 870, PI, true);
00045 
00046     Thread::signal_wait(0x01);
00047     settarget(660, 870, PI, false);
00048     
00049     flag_terminate = true;
00050     */
00051 
00052     // goes to the mid
00053     Thread::signal_wait(0x01);
00054     settarget(1500, 1000, PI/2, true);
00055 
00056     // left roll
00057     Thread::signal_wait(0x01);
00058     settarget(500, 1700, PI/2, true);
00059 
00060     // mid
00061     Thread::signal_wait(0x01);
00062     settarget(1500, 1000, PI/2, true);
00063 
00064     // map
00065     Thread::signal_wait(0x01);
00066     settarget(1500, 1700, PI/2, true);
00067 
00068     // mid
00069     Thread::signal_wait(0x01);
00070     settarget(1500, 1000, -PI/2, true);
00071 
00072     // home
00073     Thread::signal_wait(0x01);
00074     settarget(500, 500, 0, true);
00075 
00076     Thread::signal_wait(0x01);
00077     flag_terminate = true;
00078     //OLED3 = true;
00079 
00080     while (true) {
00081         Thread::wait(osWaitForever);
00082     }
00083 }