Diff: Processes/AI/ai.cpp

Revision:

81:ef1ce4f5322b

Parent:

78:3178a1e46146

Child:

83:223186cd7531

--- a/Processes/AI/ai.cpp	Mon Apr 15 17:07:40 2013 +0000
+++ b/Processes/AI/ai.cpp	Mon Apr 15 19:35:29 2013 +0000
@@ -27,6 +27,8 @@
 Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
 #endif
 
+enum layer { top_layer, bot_layer };
+
 bool delayed_done = true; //TODO: kill
 
 struct delayed_struct //TODO: kill
@@ -52,98 +54,100 @@
     MotorControl::motorsenabled = true; 
     motion::collavoiden = 1;
     
+    motion::waypoint_flag_mutex.lock();
     motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
+    motion::waypoint_flag_mutex.unlock();
     Thread::signal_wait(0x1); //wait until wp reached
    
     MotorControl::motorsenabled = false;
+    arm::upper_arm.go_up();
+    arm::lower_arm.go_up();
     
     Thread::signal_wait(0x2); //wait for cord
     
     // CORD PULLED
-    
-    Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
+    MotorControl::motorsenabled = true;
     
-    MotorControl::motorsenabled = true;
+    #ifdef TEAM_BLUE
+    Waypoint mid_wp = {1.8, 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
+    #endif
     
     Waypoint approach_wp = {2.2, 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();
     Thread::signal_wait(0x1); //wait until wp reached
     
-    // TODO: ?(disable collison avoidance), approach first cake wp, enable collision
-    /*
-    ///////////////temp hack
-    Waypoint temp_wp = {1.222,1.440, 149.0/180*PI, 0.01, 0.02*PI, 512};
-    motion::setNewWaypoint(Thread::gettid(),&temp_wp); 
-    Thread::signal_wait(0x1); //wait until wp reached
-    arm::lower_arm.go_down();
-    bottom_arm_up_timer.start(1200);
-    /////////////// temp hack over
-    */
+    Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
+    
+    float r = 0.26+0.35+0.01+0.01; //second 0.01 for being less collisiony with sensors
+    
+    layer layer_to_push = top_layer;
+    
+    float top_target_phi = (180-(11.25+22.5))/180*PI;
+    float bot_target_phi = (180-(7.5+15))/180*PI;
+    
+    float phi = 0; // angle of vector (robot->center of cake)
+    
+    for(int i=0; i<17; ++i)
+    {
+        if (top_target_phi > bot_target_phi)
+        {
+            layer_to_push = top_layer;
+            phi = top_target_phi;
+            top_target_phi -= 22.5/180*PI;
+        }
+        else
+        {
+            layer_to_push = bot_layer;
+            phi = bot_target_phi;
+            bot_target_phi -= 15.0/180*PI;
+        }
+        
+        // set new wp
+        mutable_cake_wp.x = 1.5-r*cos(phi);
+        mutable_cake_wp.y = 2-r*sin(phi);
+        mutable_cake_wp.theta = constrainAngle(phi+PI/2);
+        
+        //arm offset
+        mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
+        mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
+        
+        
+        motion::waypoint_flag_mutex.lock();
+        motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp); 
+        motion::waypoint_flag_mutex.unlock();
+        Thread::signal_wait(0x1); //wait until wp reached
+        
+        if(layer_to_push == top_layer)
+        {
+            ColourEnum colour_read = c_upper.getColour();
+            if (colour_read==own_colour)
+            {
+                arm::upper_arm.go_down();
+                top_arm_up_timer.start(1200);
+            }
+        }
+        else
+        {
+            ColourEnum colour_read = c_lower.getColour();
+            if (colour_read==own_colour || i==5 || i==7 || i==8 || i==10/*|| colour_read==WHITE*/)
+            {
+                arm::lower_arm.go_down();
+                bottom_arm_up_timer.start(1200);
+            }
+        }
+        Thread::wait(2200);
+    }
+    
+    ////////////////////
     
     while(1)
         Thread::wait(1000);
-    /*
-    ///////////////////////////////////////////////////////
-
-    // first waypoint for approach
-    current_waypoint.x = 2.2;
-    current_waypoint.y = 1.85;
-    current_waypoint.theta = (-3.0f/4.0f)*PI;
-    current_waypoint.pos_threshold = 0.03;
-    current_waypoint.angle_threshold = 0.02*PI;
-    
-    motion::collavoiden = 1;
-    MotorControl::motorsenabled = 1;
-    motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
-
-    float r = 0.26+0.35+0.02+0.01;
-
-    bool firstavoidstop = 1;
-    delayed_struct ds = {Thread::gettid(),&current_waypoint};
-    RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
-    RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
-    
-    for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
-    {
-        motion::waypoint_flag_mutex.lock();
-        if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done)
-        {
-            //temphack!!!!!
-            //Thread::wait(1000);
-            arm::upper_arm.go_down();
-            delayed_done = false;
-            top_arm_up_timer.start(1200);
-            delayed_wp_set.start(2400);
-            //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);
-            current_waypoint.theta = constrainAngle(phi+PI/2);
-            
-            //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)
-            {
-                motion::collavoiden = 0;
-                firstavoidstop = 0;
-            }
-            else
-                motion::collavoiden = 1;
-        }
-        motion::waypoint_flag_mutex.unlock();            
-        
-        Thread::wait(50);
-    }*/
 }
 
 void raise_top_arm(const void *dummy)