Revision 31:791739422122, committed 2013-04-10

Comitter:

rsavitski

Date:

Wed Apr 10 18:03:32 2013 +0000

Parent:

26:b16f1045108f

Child:

33:e3f633620816

Commit message:

ai layer thread

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/AI/ai.cpp	Wed Apr 10 18:03:32 2013 +0000
@@ -0,0 +1,62 @@
+#include "ai.h"
+
+namespace AI
+{
+
+Mutex waypoint_flag_mutex;
+
+Waypoint* current_waypoint = NULL; //global scope
+
+bool waypoint_reached = false; // is current waypoint reached
+
+void ailayer(void const *dummy)
+{
+    //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;
+    
+    Waypoint* secondwp = new Waypoint;
+    secondwp->x = 2;
+    secondwp->y = 1.2;
+    secondwp->theta = PI;
+    secondwp->pos_threshold = 0.05;
+    secondwp->angle_threshold = 0.1*PI;
+    
+    
+    while(1)
+    {
+        Thread::wait(100);
+        
+        waypoint_flag_mutex.lock();
+        if (checkWaypointStatus())
+        {
+            clearWaypointReached();
+            delete current_waypoint;
+            current_waypoint = secondwp;
+        }
+        waypoint_flag_mutex.unlock();
+    }
+    
+    Thread::yield(); //TODO!!!!!!!!!!!!!!!!!!!
+}
+
+void setWaypointReached()
+{
+    waypoint_reached = true;
+}
+
+void clearWaypointReached()
+{
+    waypoint_reached = false;
+}
+
+bool checkWaypointStatus()
+{
+    return waypoint_reached;
+}
+
+} //namespace
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/AI/ai.h	Wed Apr 10 18:03:32 2013 +0000
@@ -0,0 +1,22 @@
+#ifndef EUROBOT_PROCESSES_AI_AI_H_
+#define EUROBOT_PROCESSES_AI_AI_H_
+
+#include "rtos.h"
+#include "globals.h"
+
+namespace AI
+{
+
+void ailayer(void const *dummy);
+
+void setWaypointReached();
+void clearWaypointReached();
+bool checkWaypointStatus();
+
+
+extern Waypoint *current_waypoint; 
+extern Mutex waypoint_flag_mutex;
+    
+}
+
+#endif
\ No newline at end of file

--- a/Processes/Motion/motion.cpp	Wed Apr 10 02:01:51 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 18:03:32 2013 +0000
@@ -6,16 +6,12 @@
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
-#include "supportfuncs.h"
 
 namespace motion
 {
 
 void motionlayer(void const *dummy)
-{
-    //TODO: current_waypoint global in AI layer
-    //TODO: threshold for deciding that the waypoint has been achieved
-    
+{    
     // get target waypoint from AI
     Waypoint target_waypoint = *AI::current_waypoint;
     
@@ -31,6 +27,23 @@
     
     float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
     
+    // is the waypoint reached
+    if (distance_err < target_waypoint.pos_threshold)
+    {
+        distance_err = 0;
+    }
+    if (abs(angle_err) < target_waypoint.angle_threshold)
+    {
+        angle_err = 0;
+    }
+        
+    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 (distance_err == 0 && angle_err == 0)
+    {
+        AI::setWaypointReached();
+        return;
+    }
+    AI::waypoint_flag_mutex.unlock();
     
     // angular velocity controller
     const float p_gain_av = 0.3; //TODO: tune
@@ -67,7 +80,7 @@
         
     //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
     
-    //TODO: put into motor control
+    // pass values to the motor control
     MotorControl::set_fwdcmd(forward_v);
     MotorControl::set_omegacmd(angular_v);
 }

--- a/Processes/Motion/motion.h	Wed Apr 10 02:01:51 2013 +0000
+++ b/Processes/Motion/motion.h	Wed Apr 10 18:03:32 2013 +0000
@@ -6,6 +6,8 @@
 #include "math.h"
 #include "Kalman.h"
 #include "MotorControl.h"
+#include "supportfuncs.h"
+#include "ai.h"
 
 namespace motion
 {

--- a/globals.cpp	Wed Apr 10 02:01:51 2013 +0000
+++ b/globals.cpp	Wed Apr 10 18:03:32 2013 +0000
@@ -3,5 +3,4 @@
 
 //Store global objects here
 pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
-Timer SystemTime;
-Waypoint* AI::current_waypoint;
\ No newline at end of file
+Timer SystemTime;
\ No newline at end of file

--- a/globals.h	Wed Apr 10 02:01:51 2013 +0000
+++ b/globals.h	Wed Apr 10 18:03:32 2013 +0000
@@ -114,11 +114,4 @@
     float angle_threshold;
 } Waypoint;
 
-//TODO: hack, move to AI layer
-namespace AI
-{
-    extern Waypoint* current_waypoint; 
-}
-
-
 #endif //GLOBALS_H
\ No newline at end of file

--- a/main.cpp	Wed Apr 10 02:01:51 2013 +0000
+++ b/main.cpp	Wed Apr 10 18:03:32 2013 +0000
@@ -12,6 +12,7 @@
 #include <algorithm>
 #include "motion.h"
 #include "MotorControl.h"
+#include "ai.h"
 
 void motortest();
 void encodertest();
@@ -62,15 +63,6 @@
     Serial pc(USBTX, USBRX);
     pc.baud(115200);
     
-    using AI::current_waypoint;
-    
-    current_waypoint = new Waypoint;
-    current_waypoint->x = 0.5;
-    current_waypoint->y = 0.7;
-    current_waypoint->theta = 0.0;
-    current_waypoint->pos_threshold = 0.02;
-    current_waypoint->angle_threshold = 0.09;
-    
     InitSerial();
     //while(1)
     //    printbuff();
@@ -83,6 +75,9 @@
     
     Ticker motorcontroltestticker;
     motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
+
+    // ai layer thread
+    Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
     
     // motion layer periodic callback
     RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);