Revision 83:223186cd7531, committed 2013-04-15

Comitter:

rsavitski

Date:

Mon Apr 15 22:28:07 2013 +0000

Parent:

82:d32b58dbeb94

Child:

84:00c799fd10a7

Commit message:

both procedural and hardcoded waypoint array ai movement layer done and tested for BLUE side

Changed in this revision

--- a/Processes/AI/ai.cpp	Mon Apr 15 19:49:10 2013 +0000
+++ b/Processes/AI/ai.cpp	Mon Apr 15 22:28:07 2013 +0000
@@ -9,6 +9,8 @@
 
 //TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
 
+//#define HARDCODED_WAYPOINTS_HACK
+
 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);
 
@@ -42,14 +44,15 @@
 
 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
 
+#ifndef HARDCODED_WAYPOINTS_HACK
 void ailayer(void const *dummy)
 {
     RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
     RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
     
     //NB: reassign ds.wpptr before each invocation
-    delayed_struct ds = {Thread::gettid(),NULL};
-    RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
+    //delayed_struct ds = {Thread::gettid(),NULL};
+    //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
 
     MotorControl::motorsenabled = true; 
     motion::collavoiden = 1;
@@ -149,6 +152,107 @@
     while(1)
         Thread::wait(1000);
 }
+#else
+enum action_t {top_push_colour, bot_push_colour, bot_push_white};
+
+void ailayer(void const *dummy)
+{
+    RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
+    RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
+    
+    ////////////////////////////////////
+    // put waypoints here
+    ////////////////////////////////////
+    const int wp_num = 3;
+    
+    float x_arr[wp_num] = {1.2,1.5,1.7}; //<--------------------------
+    float y_arr[wp_num] = {1,1.2,1.4}; //<--------------------------
+    float theta_arr[wp_num] = {0,PI, PI/2}; //<----------------------
+    action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<----------------------------
+    
+    Waypoint wp_arr[wp_num];
+
+    for(int i=0; i<wp_num; ++i)
+    {
+        wp_arr[i].x =x_arr[i];
+        wp_arr[i].y =y_arr[i];
+        wp_arr[i].theta =theta_arr[i];
+        
+        wp_arr[i].pos_threshold = 0.01;
+        wp_arr[i].angle_threshold = 0.01*PI;
+        wp_arr[i].angle_exponent = 512;  
+    }
+    
+    ////////////////////////////////////
+
+    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
+    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
+    
+    
+    for(int i=0; i<wp_num; ++i)
+    {
+        motion::waypoint_flag_mutex.lock();
+        motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home
+        motion::waypoint_flag_mutex.unlock();
+        Thread::signal_wait(0x1); //wait until wp reached
+        
+        switch(action[i])
+        {
+            case top_push_colour:
+                if (c_upper.getColour()==own_colour)
+                {
+                    arm::upper_arm.go_down();
+                    top_arm_up_timer.start(1200);
+                }
+            break;
+            case bot_push_colour:
+                if (c_lower.getColour()==own_colour)
+                {
+                    arm::lower_arm.go_down();
+                    bottom_arm_up_timer.start(1200);
+                }
+            break;
+            case bot_push_white:
+                arm::lower_arm.go_down();
+                bottom_arm_up_timer.start(1200);
+            break;            
+        }
+        
+        Thread::wait(2200);
+    }
+    
+    while(1)
+    Thread::wait(1000);
+}
+#endif
 
 void raise_top_arm(const void *dummy)
 {