Shuto Naruse
ai
ai.cpp@0:8ad510cbb76c, 2012-04-26 (annotated)
- Committer:
- narshu
- Date:
- Thu Apr 26 19:59:10 2012 +0000
- Revision:
- 0:8ad510cbb76c
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| narshu | 0:8ad510cbb76c | 1 | |
| narshu | 0:8ad510cbb76c | 2 | #include "ai.h" |
| narshu | 0:8ad510cbb76c | 3 | #include "rtos.h" |
| narshu | 0:8ad510cbb76c | 4 | #include "globals.h" |
| narshu | 0:8ad510cbb76c | 5 | |
| narshu | 0:8ad510cbb76c | 6 | AI::AI() : |
| narshu | 0:8ad510cbb76c | 7 | thr_AI(aithreadwrapper,this,osPriorityNormal,1024) { |
| narshu | 0:8ad510cbb76c | 8 | flag_terminate = false; |
| narshu | 0:8ad510cbb76c | 9 | //printf("aistart\r\n"); |
| narshu | 0:8ad510cbb76c | 10 | } |
| narshu | 0:8ad510cbb76c | 11 | |
| narshu | 0:8ad510cbb76c | 12 | void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing){ |
| narshu | 0:8ad510cbb76c | 13 | targetlock.lock(); |
| narshu | 0:8ad510cbb76c | 14 | target.x = targetX; |
| narshu | 0:8ad510cbb76c | 15 | target.y = targetY; |
| narshu | 0:8ad510cbb76c | 16 | target.theta = targetTheta; |
| narshu | 0:8ad510cbb76c | 17 | target.facing = targetfacing; |
| narshu | 0:8ad510cbb76c | 18 | targetlock.unlock(); |
| narshu | 0:8ad510cbb76c | 19 | } |
| narshu | 0:8ad510cbb76c | 20 | |
| narshu | 0:8ad510cbb76c | 21 | void AI::settarget(Target targetin){ |
| narshu | 0:8ad510cbb76c | 22 | targetlock.lock(); |
| narshu | 0:8ad510cbb76c | 23 | target = targetin; |
| narshu | 0:8ad510cbb76c | 24 | targetlock.unlock(); |
| narshu | 0:8ad510cbb76c | 25 | } |
| narshu | 0:8ad510cbb76c | 26 | |
| narshu | 0:8ad510cbb76c | 27 | AI::Target AI::gettarget(){ |
| narshu | 0:8ad510cbb76c | 28 | targetlock.lock(); |
| narshu | 0:8ad510cbb76c | 29 | Target temptarget = target; |
| narshu | 0:8ad510cbb76c | 30 | targetlock.unlock(); |
| narshu | 0:8ad510cbb76c | 31 | return temptarget; |
| narshu | 0:8ad510cbb76c | 32 | } |
| narshu | 0:8ad510cbb76c | 33 | |
| narshu | 0:8ad510cbb76c | 34 | void AI::ai_thread () { |
| narshu | 0:8ad510cbb76c | 35 | /* |
| narshu | 0:8ad510cbb76c | 36 | //printf("aithreadstart\r\n"); |
| narshu | 0:8ad510cbb76c | 37 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 38 | settarget(660, 400, PI/2, true); |
| narshu | 0:8ad510cbb76c | 39 | |
| narshu | 0:8ad510cbb76c | 40 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 41 | settarget(660, 570, PI, true); |
| narshu | 0:8ad510cbb76c | 42 | |
| narshu | 0:8ad510cbb76c | 43 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 44 | settarget(400, 870, PI, true); |
| narshu | 0:8ad510cbb76c | 45 | |
| narshu | 0:8ad510cbb76c | 46 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 47 | settarget(660, 870, PI, false); |
| narshu | 0:8ad510cbb76c | 48 | |
| narshu | 0:8ad510cbb76c | 49 | flag_terminate = true; |
| narshu | 0:8ad510cbb76c | 50 | */ |
| narshu | 0:8ad510cbb76c | 51 | |
| narshu | 0:8ad510cbb76c | 52 | // goes to the mid |
| narshu | 0:8ad510cbb76c | 53 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 54 | settarget(1500, 1000, PI/2, true); |
| narshu | 0:8ad510cbb76c | 55 | |
| narshu | 0:8ad510cbb76c | 56 | // left roll |
| narshu | 0:8ad510cbb76c | 57 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 58 | settarget(500, 1700, PI/2, true); |
| narshu | 0:8ad510cbb76c | 59 | |
| narshu | 0:8ad510cbb76c | 60 | // mid |
| narshu | 0:8ad510cbb76c | 61 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 62 | settarget(1500, 1000, PI/2, true); |
| narshu | 0:8ad510cbb76c | 63 | |
| narshu | 0:8ad510cbb76c | 64 | // map |
| narshu | 0:8ad510cbb76c | 65 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 66 | settarget(1500, 1700, PI/2, true); |
| narshu | 0:8ad510cbb76c | 67 | |
| narshu | 0:8ad510cbb76c | 68 | // mid |
| narshu | 0:8ad510cbb76c | 69 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 70 | settarget(1500, 1000, -PI/2, true); |
| narshu | 0:8ad510cbb76c | 71 | |
| narshu | 0:8ad510cbb76c | 72 | // home |
| narshu | 0:8ad510cbb76c | 73 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 74 | settarget(500, 500, 0, true); |
| narshu | 0:8ad510cbb76c | 75 | |
| narshu | 0:8ad510cbb76c | 76 | Thread::signal_wait(0x01); |
| narshu | 0:8ad510cbb76c | 77 | flag_terminate = true; |
| narshu | 0:8ad510cbb76c | 78 | //OLED3 = true; |
| narshu | 0:8ad510cbb76c | 79 | |
| narshu | 0:8ad510cbb76c | 80 | while (true) { |
| narshu | 0:8ad510cbb76c | 81 | Thread::wait(osWaitForever); |
| narshu | 0:8ad510cbb76c | 82 | } |
| narshu | 0:8ad510cbb76c | 83 | } |