Diff: Processes/Motion/motion.cpp

Revision:

66:f1d75e51398d

Parent:

64:c979fb1cd3b5

Child:

67:be3ea5450cc7

--- 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);