Diff: Processes/AI/ai.cpp

Revision:

67:be3ea5450cc7

Parent:

66:f1d75e51398d

Parent:

65:4709ff6c753c

Child:

69:4b7bb92916da

--- a/Processes/AI/ai.cpp	Sun Apr 14 17:22:20 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 18:47:17 2013 +0000
@@ -4,6 +4,7 @@
 #include "motion.h"
 #include "Colour.h"
 #include "supportfuncs.h"
+#include "Arm.h"
 
 
 namespace AI
@@ -15,8 +16,8 @@
 
     current_waypoint.x = 2.2;
     current_waypoint.y = 1.85;
-    current_waypoint.theta = PI;
-    current_waypoint.pos_threshold = 0.01;
+    current_waypoint.theta = (-3.0f/4.0f)*PI;
+    current_waypoint.pos_threshold = 0.03;
     current_waypoint.angle_threshold = 0.02*PI;
 
     motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
@@ -25,14 +26,20 @@
     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.01;
 
     bool firstavoidstop = 1;
     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 || firstavoidstop))
+        if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop))
         {
+            //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);
@@ -41,6 +48,9 @@
             //arm offset
             current_waypoint.x += 0.0425*cos(current_waypoint.theta);
             current_waypoint.y += 0.0425*sin(current_waypoint.theta);
+            
+            current_waypoint.pos_threshold = 0.01;
+            current_waypoint.angle_threshold = 0.01*PI;
 
             motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
             if (firstavoidstop){