Diff: reckon/reckon.cpp

Revision:

0:964eb6a2ef00

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/reckon/reckon.cpp	Wed Mar 18 22:23:48 2015 +0000
@@ -0,0 +1,66 @@
+#include "reckon.h"
+#include "math.h"
+
+#define M_PI 3.14159265359
+
+Reckon::Reckon(ADNS5090& sensorL, ADNS5090& sensorR, float radius) : optical_flow_L(sensorL), optical_flow_R(sensorR), r(radius)
+{
+    reset();
+}
+
+void Reckon::reset(){
+    flowX=0;
+    flowY=0;
+    flowYaw = 0;
+    flow_dx_L = 0;
+    flow_dx_R = 0;
+    flow_dy_L = 0;
+    flow_dy_R = 0;
+}
+
+float Reckon::reduce_angle(float &ang){
+    
+            if(ang > M_PI)        
+                ang -= 2*M_PI;
+            else if(ang < -M_PI)
+                ang += 2*M_PI;
+            return ang;   
+    }
+
+float Reckon::dest_angle(float x_dest, float y_dest){    
+    float dest_angle = atan2(y_dest - y,x_dest - x);   
+    return reduce_angle(dest_angle);
+}
+
+float Reckon::dest_distance(float x_dest, float y_dest){    
+    return sqrt(pow(x_dest-x,2)+pow(y_dest-y,2));
+}
+
+
+bool Reckon::updateOpticalFlow(){
+    
+    bool motionL = optical_flow_L.updateMotion();
+    bool motionR = optical_flow_R.updateMotion();
+    
+    if(motionL || motionR){
+        float dx = (optical_flow_L.dx() + optical_flow_R.dx())/2;
+        float dy = (optical_flow_L.dy() + optical_flow_R.dy())/2;
+        float dAngle = (optical_flow_R.dy() - optical_flow_L.dy())/r;
+        
+        flowYaw += dAngle/2;        
+        float cos_ = cos(flowYaw);
+        float sin_ = sin(flowYaw);                
+        flowX += dx*cos_ - dy*sin_;
+        flowY += dx*sin_ + dy*cos_;        
+        flowYaw += dAngle/2;
+        
+        reduce_angle(flowYaw);
+        flow_dx_L += optical_flow_L.dx();
+        flow_dy_L += optical_flow_L.dy();
+        flow_dx_R += optical_flow_R.dx();
+        flow_dy_R += optical_flow_R.dy();
+        
+        return true;
+    }else
+        return false;
+}
\ No newline at end of file