Revision 66:f1d75e51398d, committed 2013-04-14

Comitter:

madcowswe

Date:

Sun Apr 14 17:22:20 2013 +0000

Parent:

64:c979fb1cd3b5

Child:

67:be3ea5450cc7

Commit message:

Obstacle avoidance working smoothly, fairly hacked motion-ai communication of override

Changed in this revision

--- a/Processes/AI/ai.cpp	Sun Apr 14 14:59:57 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 17:22:20 2013 +0000
@@ -20,16 +20,18 @@
     current_waypoint.angle_threshold = 0.02*PI;
 
     motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
+    motion::collavoiden = 1;
 
     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);
 
     float r = 0.61+0.02;
 
+    bool firstavoidstop = 1;
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkMotionStatus() && c_lower.getColour()==RED)
+        if (motion::checkMotionStatus() && (c_lower.getColour()==RED || firstavoidstop))
         {
             phi -= 22.5/180*PI;
             current_waypoint.x = 1.5-r*cos(phi);
@@ -41,6 +43,12 @@
             current_waypoint.y += 0.0425*sin(current_waypoint.theta);
 
             motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
+            if (firstavoidstop){
+                motion::collavoiden = 0;
+                firstavoidstop = 0;
+            }
+            else
+                motion::collavoiden = 1;
         }
         motion::waypoint_flag_mutex.unlock();            
         

--- a/Processes/Motion/motion.cpp	Sun Apr 14 14:59:57 2013 +0000
+++ b/Processes/Motion/motion.cpp	Sun Apr 14 17:22:20 2013 +0000
@@ -10,6 +10,7 @@
 #include "Kalman.h"
 #include "MotorControl.h"
 #include "supportfuncs.h"
+#include "AvoidDstSensor.h"
 
 namespace motion
 {
@@ -23,6 +24,9 @@
 namespace motion
 {
 
+volatile int collavoiden = 0;
+AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR);
+
 // motion commands supported
 enum motion_type_t { motion_waypoint };
 
@@ -152,6 +156,13 @@
         
     //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
     
+    if(collavoiden){
+        float d = ADS.Distanceincm();
+        if(d > 10){
+            forward_v *= max(min((d-15)*(1.0f/20.0f),1.0f),-0.1f);
+        }
+    }
+    
     // pass values to the motor control
     MotorControl::set_fwdcmd(forward_v);
     MotorControl::set_omegacmd(angular_v);

--- a/Processes/Motion/motion.h	Sun Apr 14 14:59:57 2013 +0000
+++ b/Processes/Motion/motion.h	Sun Apr 14 17:22:20 2013 +0000
@@ -7,6 +7,8 @@
 namespace motion
 {
 
+extern volatile int collavoiden;
+
 void motionlayer(void const *dummy);
 void waypoint_motion_handler();
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/AvoidDstSensor/AvoidDstSensor.h	Sun Apr 14 17:22:20 2013 +0000
@@ -0,0 +1,17 @@
+#include "mbed.h"
+
+class AvoidDstSensor{
+    private:
+    AnalogIn ain;
+    
+    public:
+    AvoidDstSensor(PinName analoginpin) : ain(analoginpin){}
+    
+    float Raw(){return ain;}
+    
+    float Distanceincm(){
+        float d = ((1.0f/ain)-0.5f)*(1.0f/0.11f);
+        d = (d < 10 || d > 50)? -1:d;
+        return d;
+    }
+};
\ No newline at end of file