Oskar Weigl / Mbed 2 deprecated ICRSEurobot13

This is some awesome robot code

Dependencies:   mbed-rtos mbed QEI

Fork of ICRSEurobot13 by Thomas Branch

Processes/AI/ai.cpp@39:44d3dea4adcc, 2013-04-11 (annotated)

Committer:
rsavitski
Date:
Thu Apr 11 17:23:07 2013 +0000
Revision:
39:44d3dea4adcc
Parent:
38:c9058a401410
Child:
46:adcd57a5e402
Child:
49:665bdca0f2cd
Child:
52:bffe5f7c39a3
moved waypoint functionality from ai to motion layer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsavitski 30:791739422122 1 #include "ai.h"
rsavitski 30:791739422122 2
rsavitski 30:791739422122 3 namespace AI
rsavitski 30:791739422122 4 {
rsavitski 30:791739422122 5
rsavitski 30:791739422122 6 void ailayer(void const *dummy)
rsavitski 30:791739422122 7 {
rsavitski 39:44d3dea4adcc 8 Waypoint *current_waypoint = new Waypoint[5];
rsavitski 39:44d3dea4adcc 9 /*
rsavitski 38:c9058a401410 10 current_waypoint[0].x = 1;
rsavitski 38:c9058a401410 11 current_waypoint[0].y = 1;
rsavitski 38:c9058a401410 12 current_waypoint[0].theta = 0.0;
rsavitski 38:c9058a401410 13 current_waypoint[0].pos_threshold = 0.05;
rsavitski 38:c9058a401410 14 current_waypoint[0].angle_threshold = 0.05*PI;
rsavitski 38:c9058a401410 15
rsavitski 38:c9058a401410 16 current_waypoint[1].x = 2.2;
rsavitski 38:c9058a401410 17 current_waypoint[1].y = 1.5;
rsavitski 38:c9058a401410 18 current_waypoint[1].theta = PI/2;
rsavitski 38:c9058a401410 19 current_waypoint[1].pos_threshold = 0.05;
rsavitski 38:c9058a401410 20 current_waypoint[1].angle_threshold = 0.05*PI;
rsavitski 38:c9058a401410 21
rsavitski 38:c9058a401410 22 current_waypoint[2].x = -999;
rsavitski 39:44d3dea4adcc 23 */
rsavitski 38:c9058a401410 24
rsavitski 39:44d3dea4adcc 25 current_waypoint[0].x = 0.5;
rsavitski 39:44d3dea4adcc 26 current_waypoint[0].y = 1.85;
rsavitski 39:44d3dea4adcc 27 current_waypoint[0].theta = 0.0;
rsavitski 39:44d3dea4adcc 28 current_waypoint[0].pos_threshold = 0.05;
rsavitski 39:44d3dea4adcc 29 current_waypoint[0].angle_threshold = 0.05*PI;
rsavitski 39:44d3dea4adcc 30
rsavitski 39:44d3dea4adcc 31 current_waypoint[1].x = 1.2;
rsavitski 39:44d3dea4adcc 32 current_waypoint[1].y = 0.18;
rsavitski 39:44d3dea4adcc 33 current_waypoint[1].theta = 0;
rsavitski 39:44d3dea4adcc 34 current_waypoint[1].pos_threshold = 0.01;
rsavitski 39:44d3dea4adcc 35 current_waypoint[1].angle_threshold = 0.00001;
rsavitski 39:44d3dea4adcc 36
rsavitski 39:44d3dea4adcc 37 current_waypoint[2].x = -999;
rsavitski 38:c9058a401410 38 /*
rsavitski 30:791739422122 39 //TODO: temp current waypoint hack
rsavitski 30:791739422122 40 current_waypoint = new Waypoint;
rsavitski 30:791739422122 41 current_waypoint->x = 0.5;
rsavitski 30:791739422122 42 current_waypoint->y = 0.7;
rsavitski 30:791739422122 43 current_waypoint->theta = 0.0;
rsavitski 30:791739422122 44 current_waypoint->pos_threshold = 0.05;
rsavitski 38:c9058a401410 45 current_waypoint->angle_threshold = 0.05*PI;
rsavitski 30:791739422122 46
rsavitski 30:791739422122 47 Waypoint* secondwp = new Waypoint;
rsavitski 38:c9058a401410 48 secondwp->x = 1.20;
rsavitski 38:c9058a401410 49 secondwp->y = 0.18;
rsavitski 30:791739422122 50 secondwp->theta = PI;
rsavitski 38:c9058a401410 51 secondwp->pos_threshold = 0.01;
rsavitski 38:c9058a401410 52 secondwp->angle_threshold = 0.00001;
rsavitski 38:c9058a401410 53 */
rsavitski 39:44d3dea4adcc 54 motion::setNewWaypoint(current_waypoint);
rsavitski 30:791739422122 55 while(1)
rsavitski 30:791739422122 56 {
rsavitski 35:f8e7f0a72a3d 57 Thread::wait(50);
rsavitski 30:791739422122 58
rsavitski 39:44d3dea4adcc 59 motion::waypoint_flag_mutex.lock();
rsavitski 39:44d3dea4adcc 60 if (motion::checkWaypointStatus())
rsavitski 30:791739422122 61 {
rsavitski 39:44d3dea4adcc 62
rsavitski 39:44d3dea4adcc 63 if ((current_waypoint+1)->x != -999)
rsavitski 39:44d3dea4adcc 64 {
rsavitski 39:44d3dea4adcc 65 motion::clearWaypointReached();
rsavitski 38:c9058a401410 66 current_waypoint++;
rsavitski 39:44d3dea4adcc 67 motion::setNewWaypoint(current_waypoint);
rsavitski 39:44d3dea4adcc 68 }
rsavitski 30:791739422122 69 }
rsavitski 39:44d3dea4adcc 70 motion::waypoint_flag_mutex.unlock();
rsavitski 30:791739422122 71 }
rsavitski 30:791739422122 72 }
rsavitski 30:791739422122 73
rsavitski 30:791739422122 74 } //namespace