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@50:937e860f4621, 2013-04-12 (annotated)

Committer:
madcowswe
Date:
Fri Apr 12 21:26:02 2013 +0000
Revision:
50:937e860f4621
Parent:
49:665bdca0f2cd
Parent:
46:adcd57a5e402
Child:
55:0c8897da6b3a
Merged oskar and cooper

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 38:c9058a401410 9
rsavitski 39:44d3dea4adcc 10 current_waypoint[0].x = 0.5;
madcowswe 49:665bdca0f2cd 11 current_waypoint[0].y = 0.5;
rsavitski 39:44d3dea4adcc 12 current_waypoint[0].theta = 0.0;
rsavitski 39:44d3dea4adcc 13 current_waypoint[0].pos_threshold = 0.05;
rsavitski 39:44d3dea4adcc 14 current_waypoint[0].angle_threshold = 0.05*PI;
xiaxia686 46:adcd57a5e402 15
xiaxia686 46:adcd57a5e402 16 current_waypoint[1].x = 2.5;
madcowswe 49:665bdca0f2cd 17 current_waypoint[1].y = 0.5;
rsavitski 39:44d3dea4adcc 18 current_waypoint[1].theta = 0;
xiaxia686 46:adcd57a5e402 19 current_waypoint[1].pos_threshold = 0.05;
madcowswe 49:665bdca0f2cd 20 current_waypoint[1].angle_threshold = 0.05*PI;
rsavitski 39:44d3dea4adcc 21
rsavitski 39:44d3dea4adcc 22 motion::setNewWaypoint(current_waypoint);
rsavitski 30:791739422122 23
madcowswe 49:665bdca0f2cd 24 int currwptidx = 0;
madcowswe 50:937e860f4621 25 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
madcowswe 50:937e860f4621 26 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
madcowswe 50:937e860f4621 27
rsavitski 30:791739422122 28 while(1)
rsavitski 30:791739422122 29 {
rsavitski 35:f8e7f0a72a3d 30 Thread::wait(50);
rsavitski 30:791739422122 31
rsavitski 39:44d3dea4adcc 32 motion::waypoint_flag_mutex.lock();
rsavitski 39:44d3dea4adcc 33 if (motion::checkWaypointStatus())
rsavitski 30:791739422122 34 {
madcowswe 50:937e860f4621 35 if(c_upper.getColour() == BLUE && c_lower.getColour() == RED){
madcowswe 50:937e860f4621 36 motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
rsavitski 39:44d3dea4adcc 37 motion::clearWaypointReached();
rsavitski 39:44d3dea4adcc 38 }
madcowswe 50:937e860f4621 39
rsavitski 30:791739422122 40 }
rsavitski 39:44d3dea4adcc 41 motion::waypoint_flag_mutex.unlock();
rsavitski 30:791739422122 42 }
rsavitski 30:791739422122 43 }
rsavitski 30:791739422122 44
rsavitski 30:791739422122 45 } //namespace