Revision 39:c9058a401410, committed 2013-04-10

Comitter:

rsavitski

Date:

Wed Apr 10 22:30:09 2013 +0000

Parent:

38:6ecf0d21e492

Child:

40:44d3dea4adcc

Child:

42:26e5f24b55b3

Commit message:

moving to waypoints reliably

Changed in this revision

--- a/Processes/AI/ai.cpp	Wed Apr 10 20:44:29 2013 +0000
+++ b/Processes/AI/ai.cpp	Wed Apr 10 22:30:09 2013 +0000
@@ -11,21 +11,38 @@
 
 void ailayer(void const *dummy)
 {
+    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;
+
+    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;
+
+/*
     //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.1*PI;
+    current_waypoint->angle_threshold = 0.05*PI;
     
     Waypoint* secondwp = new Waypoint;
-    secondwp->x = 2;
-    secondwp->y = 1.2;
+    secondwp->x = 1.20;
+    secondwp->y = 0.18;
     secondwp->theta = PI;
-    secondwp->pos_threshold = 0.05;
-    secondwp->angle_threshold = 0.1*PI;
-    
+    secondwp->pos_threshold = 0.01;
+    secondwp->angle_threshold = 0.00001;
+*/    
     
     while(1)
     {
@@ -35,8 +52,8 @@
         if (checkWaypointStatus())
         {
             clearWaypointReached();
-            //delete current_waypoint;
-            current_waypoint = secondwp;
+            if ((current_waypoint+1)->x != -999) 
+                current_waypoint++;
         }
         waypoint_flag_mutex.unlock();
     }

--- a/Processes/Kalman/Kalman.cpp	Wed Apr 10 20:44:29 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Wed Apr 10 22:30:09 2013 +0000
@@ -19,10 +19,8 @@
 
 Ticker predictticker;
 
-//DigitalOut OLED4(LED4);
-DigitalOut OLED4(p10);
-//DigitalOut OLED1(LED1);
-DigitalOut OLED1(p11);
+DigitalOut OLED4(LED4);
+DigitalOut OLED1(LED1);
 DigitalOut OLED2(LED2);
 
 //State variables

--- a/Processes/Motion/motion.cpp	Wed Apr 10 20:44:29 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 22:30:09 2013 +0000
@@ -6,8 +6,7 @@
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
-DigitalOut OLED4(LED4);
-DigitalOut OLED1(LED1);
+
 namespace motion
 {
 
@@ -36,8 +35,10 @@
     {
         d_reached = true;
         distance_err = 0;
-        OLED1 = 1;
-        if (abs(constrainAngle(target_waypoint.theta - current_state.theta)) < target_waypoint.angle_threshold)
+        
+        angle_err = 0.2*constrainAngle(target_waypoint.theta - current_state.theta);
+        
+        if (abs(angle_err) < target_waypoint.angle_threshold)
         {
             a_reached = true;
             angle_err = 0;
@@ -47,9 +48,7 @@
     AI::waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
     if (d_reached && a_reached)
     {
-        OLED4 = 1;
         AI::setWaypointReached();
-        return;
     }
     AI::waypoint_flag_mutex.unlock();