Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

Processes/AI/ai.cpp@39:c9058a401410, 2013-04-10 (annotated)

Committer:
rsavitski
Date:
Wed Apr 10 22:30:09 2013 +0000
Revision:
39:c9058a401410
Parent:
38:6ecf0d21e492
Child:
40:44d3dea4adcc
moving to waypoints reliably

Who changed what in which revision?

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