Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@54:99d3158c9207, 2013-04-12 (annotated)

Committer:
rsavitski
Date:
Fri Apr 12 20:58:59 2013 +0000
Revision:
54:99d3158c9207
Parent:
53:b013df99b747
Child:
55:0c8897da6b3a
crappy movement around cake

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 54:99d3158c9207 8 Waypoint current_waypoint;
rsavitski 38:c9058a401410 9
rsavitski 54:99d3158c9207 10 current_waypoint.x = 2.2;
rsavitski 54:99d3158c9207 11 current_waypoint.y = 1.85;
rsavitski 54:99d3158c9207 12 current_waypoint.theta = PI;
rsavitski 54:99d3158c9207 13 current_waypoint.pos_threshold = 0.01;
rsavitski 54:99d3158c9207 14 current_waypoint.angle_threshold = 0.02*PI;
rsavitski 39:44d3dea4adcc 15
rsavitski 54:99d3158c9207 16 motion::setNewWaypoint(&current_waypoint);
rsavitski 54:99d3158c9207 17
rsavitski 39:44d3dea4adcc 18
rsavitski 30:791739422122 19
rsavitski 54:99d3158c9207 20 float r = 0.61;
rsavitski 53:b013df99b747 21
rsavitski 54:99d3158c9207 22 for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
rsavitski 30:791739422122 23 {
rsavitski 39:44d3dea4adcc 24 motion::waypoint_flag_mutex.lock();
rsavitski 39:44d3dea4adcc 25 if (motion::checkWaypointStatus())
rsavitski 30:791739422122 26 {
rsavitski 54:99d3158c9207 27 phi -= 22.5/180*PI;
rsavitski 54:99d3158c9207 28 current_waypoint.x = 1.5-r*cos(phi);
rsavitski 54:99d3158c9207 29 current_waypoint.y = 2-r*sin(phi);
rsavitski 54:99d3158c9207 30 current_waypoint.theta = constrainAngle(phi+PI/2);
rsavitski 54:99d3158c9207 31 motion::clearWaypointReached();
rsavitski 54:99d3158c9207 32 motion::setNewWaypoint(&current_waypoint);
rsavitski 30:791739422122 33 }
rsavitski 54:99d3158c9207 34 motion::waypoint_flag_mutex.unlock();
rsavitski 54:99d3158c9207 35
rsavitski 54:99d3158c9207 36 Thread::wait(50);
rsavitski 30:791739422122 37 }
rsavitski 30:791739422122 38 }
rsavitski 30:791739422122 39
rsavitski 30:791739422122 40 } //namespace