Revision 54:99d3158c9207, committed 2013-04-12

Comitter:

rsavitski

Date:

Fri Apr 12 20:58:59 2013 +0000

Parent:

53:b013df99b747

Child:

55:0c8897da6b3a

Commit message:

crappy movement around cake

Changed in this revision

--- a/Processes/AI/ai.cpp	Fri Apr 12 18:22:53 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 20:58:59 2013 +0000
@@ -5,70 +5,35 @@
 
 void ailayer(void const *dummy)
 {
-    Waypoint *current_waypoint = new Waypoint[5];
-/*
-    current_waypoint[0].x = 1;
-    current_waypoint[0].y = 1;
-    current_waypoint[0].theta = 0.0;
-    current_waypoint[0].pos_threshold = 0.05;
-    current_waypoint[0].angle_threshold = 0.05*PI;
+    Waypoint current_waypoint;
 
-    current_waypoint[1].x = 2.2;
-    current_waypoint[1].y = 1.5;
-    current_waypoint[1].theta = PI/2;
-    current_waypoint[1].pos_threshold = 0.05;
-    current_waypoint[1].angle_threshold = 0.05*PI;
-
-    current_waypoint[2].x = -999;
-*/
-
-    current_waypoint[0].x = 1.8;
-    current_waypoint[0].y = 1.39;
-    current_waypoint[0].theta = PI;
-    current_waypoint[0].pos_threshold = 0.03;
-    current_waypoint[0].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;
 
-    current_waypoint[1].x = 1.5;
-    current_waypoint[1].y = 1.39;
-    current_waypoint[1].theta = PI;
-    current_waypoint[1].pos_threshold = 0.03;
-    current_waypoint[1].angle_threshold = 0.05*PI;
+    motion::setNewWaypoint(&current_waypoint);
+    
 
-    current_waypoint[2].x = -999;
-/*
-    //TODO: temp current waypoint hack
-    current_waypoint = new Waypoint;
-    current_waypoint->x = 0.5;
-    current_waypoint->y = 0.7;
-    current_waypoint->theta = 0.0;
-    current_waypoint->pos_threshold = 0.05;
-    current_waypoint->angle_threshold = 0.05*PI;
     
-    Waypoint* secondwp = new Waypoint;
-    secondwp->x = 1.20;
-    secondwp->y = 0.18;
-    secondwp->theta = PI;
-    secondwp->pos_threshold = 0.01;
-    secondwp->angle_threshold = 0.00001;
-*/    
+    float r = 0.61;
 
-    motion::setNewWaypoint(current_waypoint);
-    while(1)
+    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 ((current_waypoint+1)->x != -999)
-            { 
-                motion::clearWaypointReached();
-                current_waypoint++;
-                motion::setNewWaypoint(current_waypoint);
-            }
+            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 18:22:53 2013 +0000
+++ b/Processes/AI/ai.h	Fri Apr 12 20:58:59 2013 +0000
@@ -4,6 +4,7 @@
 #include "rtos.h"
 #include "globals.h"
 #include "motion.h"
+#include "supportfuncs.h"
 
 namespace AI
 {

--- a/Processes/Motion/motion.cpp	Fri Apr 12 18:22:53 2013 +0000
+++ b/Processes/Motion/motion.cpp	Fri Apr 12 20:58:59 2013 +0000
@@ -86,11 +86,11 @@
     
     
     // 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
     float max_fv_reverse = 0.05; //TODO: tune
-    const float angle_envelope_exponent = 32.0; //8.0; //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;