Diff: Processes/AI/ai.cpp

Revision:

90:e4164bb8c60e

Parent:

89:dfe8c2ec5b88

Child:

91:fdadfd883825

--- a/Processes/AI/ai.cpp	Tue Apr 16 13:19:17 2013 +0000
+++ b/Processes/AI/ai.cpp	Wed Apr 17 23:16:25 2013 +0000
@@ -73,13 +73,21 @@
     arm::lower_arm.go_up();
     
     #ifdef TEAM_BLUE
-    Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
+
+    Waypoint mid_wp5 = {1.2, 1, 0, 0.10, 0.15*PI, 32};
+    motion::waypoint_flag_mutex.lock();
+    motion::setNewWaypoint(Thread::gettid(),&mid_wp5);
+    motion::waypoint_flag_mutex.unlock();
+    Thread::signal_wait(0x1); //wait until wp reached
+    Thread::wait(3000);
+
+    Waypoint mid_wp = {1.9, 1.1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
     motion::waypoint_flag_mutex.lock();
     motion::setNewWaypoint(Thread::gettid(),&mid_wp);
     motion::waypoint_flag_mutex.unlock();
     Thread::signal_wait(0x1); //wait until wp reached
     
-    Waypoint approach_wp = {2.24, 1.83/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
+    Waypoint approach_wp = {2.24, 1.84/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
     motion::waypoint_flag_mutex.lock();
     motion::setNewWaypoint(Thread::gettid(),&approach_wp); 
     motion::waypoint_flag_mutex.unlock();