Revision 52:bffe5f7c39a3, committed 2013-04-12

Comitter:

rsavitski

Date:

Fri Apr 12 17:29:38 2013 +0000

Parent:

40:fefdbb9b5968

Child:

53:b013df99b747

Commit message:

diff check

Changed in this revision

--- a/Processes/AI/ai.cpp	Thu Apr 11 18:49:11 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 17:29:38 2013 +0000
@@ -22,19 +22,19 @@
     current_waypoint[2].x = -999;
 */
 
-    current_waypoint[0].x = 0.5;
-    current_waypoint[0].y = 1.85;
-    current_waypoint[0].theta = 0.0;
-    current_waypoint[0].pos_threshold = 0.05;
-    current_waypoint[0].angle_threshold = 0.05*PI;
+    current_waypoint[0].x = 1.5;
+    current_waypoint[0].y = 1.29;
+    current_waypoint[0].theta = PI;
+    current_waypoint[0].pos_threshold = 0.02;
+    current_waypoint[0].angle_threshold = 0.02*PI;
 
-    current_waypoint[1].x = 1.2;
+    //current_waypoint[1].x = 1.2;
     current_waypoint[1].y = 0.18;
     current_waypoint[1].theta = 0;
     current_waypoint[1].pos_threshold = 0.01;
     current_waypoint[1].angle_threshold = 0.00001;
 
-    current_waypoint[2].x = -999;
+    current_waypoint[1].x = -999;
 /*
     //TODO: temp current waypoint hack
     current_waypoint = new Waypoint;
@@ -51,6 +51,7 @@
     secondwp->pos_threshold = 0.01;
     secondwp->angle_threshold = 0.00001;
 */    
+    current_waypoint++;
     motion::setNewWaypoint(current_waypoint);
     while(1)
     {

--- a/Processes/Motion/motion.cpp	Thu Apr 11 18:49:11 2013 +0000
+++ b/Processes/Motion/motion.cpp	Fri Apr 12 17:29:38 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)
         {
@@ -78,13 +89,18 @@
     const float p_gain_fv = 0.5; //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 = 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)

--- a/Processes/Printing/Printing.h	Thu Apr 11 18:49:11 2013 +0000
+++ b/Processes/Printing/Printing.h	Fri Apr 12 17:29:38 2013 +0000
@@ -1,7 +1,7 @@
 
 // Eurobot13 Printing.h
 
-#define PRINTINGOFF
+//#define PRINTINGOFF
 
 #include "mbed.h"
 #include "rtos.h"