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@46:adcd57a5e402, 2013-04-12 (annotated)

Committer:
xiaxia686
Date:
Fri Apr 12 20:59:18 2013 +0000
Revision:
46:adcd57a5e402
Parent:
40:44d3dea4adcc 
Colours Sensors fixed

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 void ailayer(void const *dummy)
rsavitski 31:791739422122 7 {
rsavitski 40:44d3dea4adcc 8 Waypoint *current_waypoint = new Waypoint[5];
rsavitski 40:44d3dea4adcc 9 /*
rsavitski 39:c9058a401410 10 current_waypoint[0].x = 1;
rsavitski 39:c9058a401410 11 current_waypoint[0].y = 1;
rsavitski 39:c9058a401410 12 current_waypoint[0].theta = 0.0;
rsavitski 39:c9058a401410 13 current_waypoint[0].pos_threshold = 0.05;
rsavitski 39:c9058a401410 14 current_waypoint[0].angle_threshold = 0.05*PI;
rsavitski 39:c9058a401410 15
rsavitski 39:c9058a401410 16 current_waypoint[1].x = 2.2;
rsavitski 39:c9058a401410 17 current_waypoint[1].y = 1.5;
rsavitski 39:c9058a401410 18 current_waypoint[1].theta = PI/2;
rsavitski 39:c9058a401410 19 current_waypoint[1].pos_threshold = 0.05;
rsavitski 39:c9058a401410 20 current_waypoint[1].angle_threshold = 0.05*PI;
rsavitski 39:c9058a401410 21
rsavitski 39:c9058a401410 22 current_waypoint[2].x = -999;
rsavitski 40:44d3dea4adcc 23 */
rsavitski 39:c9058a401410 24
rsavitski 40:44d3dea4adcc 25 current_waypoint[0].x = 0.5;
xiaxia686 46:adcd57a5e402 26 current_waypoint[0].y = 1.65;
rsavitski 40:44d3dea4adcc 27 current_waypoint[0].theta = 0.0;
rsavitski 40:44d3dea4adcc 28 current_waypoint[0].pos_threshold = 0.05;
rsavitski 40:44d3dea4adcc 29 current_waypoint[0].angle_threshold = 0.05*PI;
xiaxia686 46:adcd57a5e402 30
xiaxia686 46:adcd57a5e402 31 current_waypoint[1].x = 2.5;
xiaxia686 46:adcd57a5e402 32 current_waypoint[1].y = 0.45;
xiaxia686 46:adcd57a5e402 33 current_waypoint[1].theta = 0.0;
xiaxia686 46:adcd57a5e402 34 current_waypoint[1].pos_threshold = 0.05;
xiaxia686 46:adcd57a5e402 35 current_waypoint[1].angle_threshold = 0.05*PI;
rsavitski 40:44d3dea4adcc 36
xiaxia686 46:adcd57a5e402 37 current_waypoint[2].x = 1.2;
xiaxia686 46:adcd57a5e402 38 current_waypoint[2].y = 0.18;
xiaxia686 46:adcd57a5e402 39 current_waypoint[2].theta = 0;
xiaxia686 46:adcd57a5e402 40 current_waypoint[2].pos_threshold = 0.01;
xiaxia686 46:adcd57a5e402 41 current_waypoint[2].angle_threshold = 0.00001;
rsavitski 40:44d3dea4adcc 42
xiaxia686 46:adcd57a5e402 43 current_waypoint[3].x = -999;
rsavitski 39:c9058a401410 44 /*
rsavitski 31:791739422122 45 //TODO: temp current waypoint hack
rsavitski 31:791739422122 46 current_waypoint = new Waypoint;
rsavitski 31:791739422122 47 current_waypoint->x = 0.5;
rsavitski 31:791739422122 48 current_waypoint->y = 0.7;
rsavitski 31:791739422122 49 current_waypoint->theta = 0.0;
rsavitski 31:791739422122 50 current_waypoint->pos_threshold = 0.05;
rsavitski 39:c9058a401410 51 current_waypoint->angle_threshold = 0.05*PI;
rsavitski 31:791739422122 52
rsavitski 31:791739422122 53 Waypoint* secondwp = new Waypoint;
rsavitski 39:c9058a401410 54 secondwp->x = 1.20;
rsavitski 39:c9058a401410 55 secondwp->y = 0.18;
rsavitski 31:791739422122 56 secondwp->theta = PI;
rsavitski 39:c9058a401410 57 secondwp->pos_threshold = 0.01;
rsavitski 39:c9058a401410 58 secondwp->angle_threshold = 0.00001;
rsavitski 39:c9058a401410 59 */
rsavitski 40:44d3dea4adcc 60 motion::setNewWaypoint(current_waypoint);
xiaxia686 46:adcd57a5e402 61 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
xiaxia686 46:adcd57a5e402 62 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
rsavitski 31:791739422122 63 while(1)
rsavitski 31:791739422122 64 {
xiaxia686 46:adcd57a5e402 65
rsavitski 36:f8e7f0a72a3d 66 Thread::wait(50);
rsavitski 31:791739422122 67
xiaxia686 46:adcd57a5e402 68
rsavitski 40:44d3dea4adcc 69 motion::waypoint_flag_mutex.lock();
rsavitski 40:44d3dea4adcc 70 if (motion::checkWaypointStatus())
rsavitski 31:791739422122 71 {
rsavitski 40:44d3dea4adcc 72
xiaxia686 46:adcd57a5e402 73 if (((current_waypoint+1)->x != -999) && ((c_upper.getColour()==BLUE) || (c_lower.getColour()==RED)))
rsavitski 40:44d3dea4adcc 74 {
rsavitski 40:44d3dea4adcc 75 motion::clearWaypointReached();
rsavitski 39:c9058a401410 76 current_waypoint++;
rsavitski 40:44d3dea4adcc 77 motion::setNewWaypoint(current_waypoint);
rsavitski 40:44d3dea4adcc 78 }
rsavitski 31:791739422122 79 }
rsavitski 40:44d3dea4adcc 80 motion::waypoint_flag_mutex.unlock();
rsavitski 31:791739422122 81 }
rsavitski 31:791739422122 82 }
rsavitski 31:791739422122 83
rsavitski 31:791739422122 84 } //namespace