Diff: Processes/AI/ai.cpp

Revision:

77:8d83a0c00e66

Parent:

76:532d9bc1d2aa

Child:

78:3178a1e46146

--- a/Processes/AI/ai.cpp	Mon Apr 15 13:44:49 2013 +0000
+++ b/Processes/AI/ai.cpp	Mon Apr 15 15:30:12 2013 +0000
@@ -5,11 +5,27 @@
 #include "Colour.h"
 #include "supportfuncs.h"
 #include "Arm.h"
+#include "MotorControl.h"
 
-//TODO: after 2013, kill entire AI layer as it is hacked together. Rest is fine-ish
+//TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
+
+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);
 
 namespace AI
 {
+// starting position
+#ifdef TEAM_RED
+ColourEnum own_colour = RED;
+
+Waypoint home_wp = {2.9, 1.75, PI, 0.03, 0.05*PI, 32};
+#endif
+
+#ifdef TEAM_BLUE
+ColourEnum own_colour = BLUE;
+
+Waypoint home_wp = {0.15, 1, 0, 0.03, 0.05*PI, 32};
+#endif
 
 bool delayed_done = true; //TODO: kill
 
@@ -19,16 +35,40 @@
     Waypoint *wpptr;
 };
 
-void arm_upper(const void *dummy); //TODO: kill
+void raise_top_arm(const void *dummy);
+void raise_bottom_arm(const void *dummy);
 
 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
 
 void ailayer(void const *dummy)
 {
+    MotorControl::motorsenabled = true; 
+    motion::collavoiden = 1;
+    
+    motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
+    
+    while(!motion::checkMotionStatus()); // wait until there
+    MotorControl::motorsenabled = false;
+    
+    Thread::signal_wait(0x2);
+    
+    // CORD PULLED
+    
     Waypoint 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);
+    current_waypoint.x = 1.5;
+    current_waypoint.y = 1;
+    current_waypoint.theta = (-3.0f/4.0f)*PI;
+    current_waypoint.pos_threshold = 0.03;
+    current_waypoint.angle_threshold = 0.05*PI;
+    current_waypoint.angle_exponent = 512;
+    
+    MotorControl::motorsenabled = true;
+    motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
+    
+    while(1);
+    
+    ///////////////////////////////////////////////////////
 
     // first waypoint for approach
     current_waypoint.x = 2.2;
@@ -45,18 +85,18 @@
     bool firstavoidstop = 1;
     delayed_struct ds = {Thread::gettid(),&current_waypoint};
     RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
-    RtosTimer delayed_armer(arm_upper, osTimerOnce);
+    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()==RED || firstavoidstop) && delayed_done)
+        if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done)
         {
             //temphack!!!!!
             //Thread::wait(1000);
             arm::upper_arm.go_down();
             delayed_done = false;
-            delayed_armer.start(1200);
+            top_arm_up_timer.start(1200);
             delayed_wp_set.start(2400);
             //Thread::wait(2000);
             //arm::upper_arm.go_up();
@@ -88,15 +128,19 @@
     }
 }
 
-void arm_upper(const void *dummy)
+void raise_top_arm(const void *dummy)
 {
     arm::upper_arm.go_up();
 }
 
+void raise_bottom_arm(const void *dummy)
+{
+    arm::lower_arm.go_up();
+}
+
 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
 {
     delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
-    //arm::upper_arm.go_up();
     motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
     delayed_done = true;
 }