Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@56:ed585a82092b, 2013-04-13 (annotated)

Committer:
rsavitski
Date:
Sat Apr 13 19:01:45 2013 +0000
Revision:
56:ed585a82092b
Parent:
55:0c8897da6b3a
Child:
57:d434ceab6892
R syncpoint. Working before modifying motion layer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsavitski 30:791739422122 1 #include "ai.h"
rsavitski 55:0c8897da6b3a 2 #include "rtos.h"
rsavitski 55:0c8897da6b3a 3 #include "globals.h"
rsavitski 55:0c8897da6b3a 4 #include "motion.h"
rsavitski 55:0c8897da6b3a 5 #include "Colour.h"
rsavitski 55:0c8897da6b3a 6 #include "supportfuncs.h"
rsavitski 55:0c8897da6b3a 7
rsavitski 30:791739422122 8
rsavitski 30:791739422122 9 namespace AI
rsavitski 30:791739422122 10 {
rsavitski 30:791739422122 11
rsavitski 30:791739422122 12 void ailayer(void const *dummy)
rsavitski 30:791739422122 13 {
rsavitski 54:99d3158c9207 14 Waypoint current_waypoint;
rsavitski 38:c9058a401410 15
rsavitski 54:99d3158c9207 16 current_waypoint.x = 2.2;
rsavitski 54:99d3158c9207 17 current_waypoint.y = 1.85;
rsavitski 54:99d3158c9207 18 current_waypoint.theta = PI;
rsavitski 54:99d3158c9207 19 current_waypoint.pos_threshold = 0.01;
rsavitski 54:99d3158c9207 20 current_waypoint.angle_threshold = 0.02*PI;
rsavitski 39:44d3dea4adcc 21
rsavitski 54:99d3158c9207 22 motion::setNewWaypoint(&current_waypoint);
rsavitski 39:44d3dea4adcc 23
madcowswe 50:937e860f4621 24 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
madcowswe 50:937e860f4621 25 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
rsavitski 55:0c8897da6b3a 26
rsavitski 55:0c8897da6b3a 27 float r = 0.61+0.02;
rsavitski 53:b013df99b747 28
rsavitski 54:99d3158c9207 29 for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
rsavitski 30:791739422122 30 {
rsavitski 39:44d3dea4adcc 31 motion::waypoint_flag_mutex.lock();
rsavitski 55:0c8897da6b3a 32 if (motion::checkWaypointStatus() && c_upper.getColour()==RED)
rsavitski 30:791739422122 33 {
rsavitski 54:99d3158c9207 34 phi -= 22.5/180*PI;
rsavitski 54:99d3158c9207 35 current_waypoint.x = 1.5-r*cos(phi);
rsavitski 54:99d3158c9207 36 current_waypoint.y = 2-r*sin(phi);
rsavitski 54:99d3158c9207 37 current_waypoint.theta = constrainAngle(phi+PI/2);
rsavitski 56:ed585a82092b 38
rsavitski 56:ed585a82092b 39 //arm offset
rsavitski 56:ed585a82092b 40 current_waypoint.x += 0.0425*cos(current_waypoint.theta);
rsavitski 56:ed585a82092b 41 current_waypoint.y += 0.0425*sin(current_waypoint.theta);
rsavitski 56:ed585a82092b 42
rsavitski 54:99d3158c9207 43 motion::clearWaypointReached();
rsavitski 54:99d3158c9207 44 motion::setNewWaypoint(&current_waypoint);
rsavitski 30:791739422122 45 }
rsavitski 54:99d3158c9207 46 motion::waypoint_flag_mutex.unlock();
rsavitski 54:99d3158c9207 47
rsavitski 54:99d3158c9207 48 Thread::wait(50);
rsavitski 30:791739422122 49 }
rsavitski 30:791739422122 50 }
rsavitski 30:791739422122 51
rsavitski 30:791739422122 52 } //namespace