File content as of revision 5:d2e955a94940:

#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;
}