reckon.cpp

00001 #include "reckon.h"
00002 #include "math.h"
00003 
00004 #define M_PI 3.14159265359
00005 
00006 Reckon::Reckon(ADNS5090& sensorL, ADNS5090& sensorR, float radius) : optical_flow_L(sensorL), optical_flow_R(sensorR), r(radius)
00007 {
00008     reset();
00009 }
00010 
00011 void Reckon::reset(){
00012     flowX=0;
00013     flowY=0;
00014     flowYaw = 0;
00015     flow_dx_L = 0;
00016     flow_dx_R = 0;
00017     flow_dy_L = 0;
00018     flow_dy_R = 0;
00019 }
00020 
00021 float Reckon::reduce_angle(float &ang){
00022     
00023             if(ang > M_PI)        
00024                 ang -= 2*M_PI;
00025             else if(ang < -M_PI)
00026                 ang += 2*M_PI;
00027             return ang;   
00028     }
00029 
00030 float Reckon::dest_angle(float x_dest, float y_dest){    
00031     float dest_angle = atan2(y_dest - y,x_dest - x);   
00032     return reduce_angle(dest_angle);
00033 }
00034 
00035 float Reckon::dest_distance(float x_dest, float y_dest){    
00036     return sqrt(pow(x_dest-x,2)+pow(y_dest-y,2));
00037 }
00038 
00039 
00040 bool Reckon::updateOpticalFlow(){
00041     
00042     bool motionL = optical_flow_L.updateMotion();
00043     bool motionR = optical_flow_R.updateMotion();
00044     
00045     if(motionL || motionR){
00046         float dx = (optical_flow_L.dx() + optical_flow_R.dx())/2;
00047         float dy = (optical_flow_L.dy() + optical_flow_R.dy())/2;
00048         float dAngle = (optical_flow_R.dy() - optical_flow_L.dy())/r;
00049         
00050         flowYaw += dAngle/2;        
00051         float cos_ = cos(flowYaw);
00052         float sin_ = sin(flowYaw);                
00053         flowX += dx*cos_ - dy*sin_;
00054         flowY += dx*sin_ + dy*cos_;        
00055         flowYaw += dAngle/2;
00056         
00057         reduce_angle(flowYaw);
00058         flow_dx_L += optical_flow_L.dx();
00059         flow_dy_L += optical_flow_L.dy();
00060         flow_dx_R += optical_flow_R.dx();
00061         flow_dy_R += optical_flow_R.dy();
00062         
00063         return true;
00064     }else
00065         return false;
00066 }