Revision 55:0c8897da6b3a, committed 2013-04-12

Comitter:

rsavitski

Date:

Fri Apr 12 22:00:32 2013 +0000

Parent:

51:bc261eae004b

Parent:

54:99d3158c9207

Child:

56:ed585a82092b

Child:

60:5058465904e0

Commit message:

three way merge done. moves around cake and looks for colours

Changed in this revision

--- a/Processes/AI/ai.cpp	Fri Apr 12 21:34:34 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 22:00:32 2013 +0000
@@ -1,44 +1,46 @@
 #include "ai.h"
+#include "rtos.h"
+#include "globals.h"
+#include "motion.h"
+#include "Colour.h"
+#include "supportfuncs.h"
+
 
 namespace AI
 {
 
 void ailayer(void const *dummy)
 {
-    Waypoint *current_waypoint = new Waypoint[5];
+    Waypoint current_waypoint;
 
-    current_waypoint[0].x = 0.5;
-    current_waypoint[0].y = 0.5;
-    current_waypoint[0].theta = 0.0;
-    current_waypoint[0].pos_threshold = 0.05;
-    current_waypoint[0].angle_threshold = 0.05*PI;
-    
-    current_waypoint[1].x = 2.5;
-    current_waypoint[1].y = 0.5;
-    current_waypoint[1].theta = 0;
-    current_waypoint[1].pos_threshold = 0.05;
-    current_waypoint[1].angle_threshold = 0.05*PI;
+    current_waypoint.x = 2.2;
+    current_waypoint.y = 1.85;
+    current_waypoint.theta = PI;
+    current_waypoint.pos_threshold = 0.01;
+    current_waypoint.angle_threshold = 0.02*PI;
 
-    motion::setNewWaypoint(current_waypoint);
-    
-    int currwptidx = 0;
+    motion::setNewWaypoint(&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);
-    
-    while(1)
+
+    float r = 0.61+0.02;
+
+    for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
-        Thread::wait(50);
-        
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkWaypointStatus())
+        if (motion::checkWaypointStatus() && c_upper.getColour()==RED)
         {
-            if(c_upper.getColour() == BLUE && c_lower.getColour() == RED){
-                motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
-                motion::clearWaypointReached();
-            }
-
+            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);
+            motion::clearWaypointReached();
+            motion::setNewWaypoint(&current_waypoint);
         }
-        motion::waypoint_flag_mutex.unlock();
+        motion::waypoint_flag_mutex.unlock();            
+        
+        Thread::wait(50);
     }
 }
 

--- a/Processes/AI/ai.h	Fri Apr 12 21:34:34 2013 +0000
+++ b/Processes/AI/ai.h	Fri Apr 12 22:00:32 2013 +0000
@@ -1,11 +1,6 @@
 #ifndef EUROBOT_PROCESSES_AI_AI_H_
 #define EUROBOT_PROCESSES_AI_AI_H_
 
-#include "rtos.h"
-#include "globals.h"
-#include "motion.h"
-#include "Colour.h"
-
 namespace AI
 {
 

--- a/Processes/Motion/motion.cpp	Fri Apr 12 21:34:34 2013 +0000
+++ b/Processes/Motion/motion.cpp	Fri Apr 12 22:00:32 2013 +0000
@@ -35,14 +35,25 @@
     
     float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
     
+    // reversing
+    bool reversing = false;
+    if ((abs(angle_err) > PI/2) && (distance_err < 0.2))
+    {
+        reversing = true;
+        angle_err = constrainAngle(angle_err + PI);
+        distance_err = -distance_err;
+    }
+    
+    float angle_err_saved = angle_err; // actuated angle error can be overriden by the final turning code, but forward speed envelope should be controlled with the atan angle
+    
     // is the waypoint reached
     waypoint_flag_mutex.lock();
-    if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
+    if (abs(distance_err) < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
     {
         d_reached = true;
-        distance_err = 0;
+        //distance_err = 0;
         
-        angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta);
+        angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
         
         if (abs(angle_err) < target_waypoint.angle_threshold)
         {
@@ -75,16 +86,21 @@
     
     
     // forward velocity controller
-    const float p_gain_fv = 0.5; //TODO: tune
+    const float p_gain_fv = 0.25; //TODO: tune
     
     float max_fv = 0.2; // meters per sec //TODO: tune
-    const float angle_envelope_exponent = 8.0; //TODO: tune
+    float max_fv_reverse = 0.05; //TODO: tune
+    const float angle_envelope_exponent = 512;//32.0; //8.0; //TODO: tune
     
     // control, distance_err in meters
     float forward_v = p_gain_fv * distance_err;
     
+    // if reversing, robot is less stable hence a different cap is used
+    if (reversing)
+        max_fv = max_fv_reverse;
+    
     // control the forward velocity envelope based on angular error
-    max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent);
+    max_fv = max_fv * pow(cos(angle_err_saved/2), angle_envelope_exponent);
     
     // constrain range
     if (forward_v > max_fv)