Diff: Processes/AI/ai.cpp

Revision:

65:4709ff6c753c

Parent:

57:d434ceab6892

Child:

67:be3ea5450cc7

Child:

68:662164480f60

--- a/Processes/AI/ai.cpp	Sat Apr 13 22:42:01 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 17:57:12 2013 +0000
@@ -4,6 +4,7 @@
 #include "motion.h"
 #include "Colour.h"
 #include "supportfuncs.h"
+#include "Arm.h"
 
 
 namespace AI
@@ -17,20 +18,26 @@
     current_waypoint.y = 1.85;
     current_waypoint.theta = PI;
     current_waypoint.pos_threshold = 0.01;
-    current_waypoint.angle_threshold = 0.02*PI;
+    current_waypoint.angle_threshold = 0.01*PI;
 
     motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
 
     Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
     Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
 
-    float r = 0.61+0.02;
+    float r = 0.61+0.02+0.03;
 
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkMotionStatus() && c_lower.getColour()==RED)
+        if (motion::checkMotionStatus() && c_upper.getColour()==RED)
         {
+            //temphack!!!!!
+            Thread::wait(1000);
+            arm::upper_arm.go_down();
+            Thread::wait(2000);
+            arm::upper_arm.go_up();
+            
             phi -= 22.5/180*PI;
             current_waypoint.x = 1.5-r*cos(phi);
             current_waypoint.y = 2-r*sin(phi);