Dave Lu / FYDP_Final2

Only imu output

Dependencies:   Servo mbed

Fork of FYDP_Final2 by Mark Vandermeulen

reckon/reckon.cpp@5:d2e955a94940, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 09:58:50 2015 +0000
Revision:
5:d2e955a94940
Parent:
0:21019d94ad33 
only imu output

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:21019d94ad33 1 #include "reckon.h"
majik 0:21019d94ad33 2 #include "math.h"
majik 0:21019d94ad33 3
majik 0:21019d94ad33 4 #define M_PI 3.14159265359
majik 0:21019d94ad33 5
majik 0:21019d94ad33 6 Reckon::Reckon(ADNS5090& sensorL, ADNS5090& sensorR, float radius) : optical_flow_L(sensorL), optical_flow_R(sensorR), r(radius)
majik 0:21019d94ad33 7 {
majik 0:21019d94ad33 8 reset();
majik 0:21019d94ad33 9 }
majik 0:21019d94ad33 10
majik 0:21019d94ad33 11 void Reckon::reset(){
majik 0:21019d94ad33 12 flowX=0;
majik 0:21019d94ad33 13 flowY=0;
majik 0:21019d94ad33 14 flowYaw = 0;
majik 0:21019d94ad33 15 flow_dx_L = 0;
majik 0:21019d94ad33 16 flow_dx_R = 0;
majik 0:21019d94ad33 17 flow_dy_L = 0;
majik 0:21019d94ad33 18 flow_dy_R = 0;
majik 0:21019d94ad33 19 }
majik 0:21019d94ad33 20
majik 0:21019d94ad33 21 float Reckon::reduce_angle(float &ang){
majik 0:21019d94ad33 22
majik 0:21019d94ad33 23 if(ang > M_PI)
majik 0:21019d94ad33 24 ang -= 2*M_PI;
majik 0:21019d94ad33 25 else if(ang < -M_PI)
majik 0:21019d94ad33 26 ang += 2*M_PI;
majik 0:21019d94ad33 27 return ang;
majik 0:21019d94ad33 28 }
majik 0:21019d94ad33 29
majik 0:21019d94ad33 30 float Reckon::dest_angle(float x_dest, float y_dest){
majik 0:21019d94ad33 31 float dest_angle = atan2(y_dest - y,x_dest - x);
majik 0:21019d94ad33 32 return reduce_angle(dest_angle);
majik 0:21019d94ad33 33 }
majik 0:21019d94ad33 34
majik 0:21019d94ad33 35 float Reckon::dest_distance(float x_dest, float y_dest){
majik 0:21019d94ad33 36 return sqrt(pow(x_dest-x,2)+pow(y_dest-y,2));
majik 0:21019d94ad33 37 }
majik 0:21019d94ad33 38
majik 0:21019d94ad33 39
majik 0:21019d94ad33 40 bool Reckon::updateOpticalFlow(){
majik 0:21019d94ad33 41
majik 0:21019d94ad33 42 bool motionL = optical_flow_L.updateMotion();
majik 0:21019d94ad33 43 bool motionR = optical_flow_R.updateMotion();
majik 0:21019d94ad33 44
majik 0:21019d94ad33 45 if(motionL || motionR){
majik 0:21019d94ad33 46 float dx = (optical_flow_L.dx() + optical_flow_R.dx())/2;
majik 0:21019d94ad33 47 float dy = (optical_flow_L.dy() + optical_flow_R.dy())/2;
majik 0:21019d94ad33 48 float dAngle = (optical_flow_R.dy() - optical_flow_L.dy())/r;
majik 0:21019d94ad33 49
majik 0:21019d94ad33 50 flowYaw += dAngle/2;
majik 0:21019d94ad33 51 float cos_ = cos(flowYaw);
majik 0:21019d94ad33 52 float sin_ = sin(flowYaw);
majik 0:21019d94ad33 53 flowX += dx*cos_ - dy*sin_;
majik 0:21019d94ad33 54 flowY += dx*sin_ + dy*cos_;
majik 0:21019d94ad33 55 flowYaw += dAngle/2;
majik 0:21019d94ad33 56
majik 0:21019d94ad33 57 reduce_angle(flowYaw);
majik 0:21019d94ad33 58 flow_dx_L += optical_flow_L.dx();
majik 0:21019d94ad33 59 flow_dy_L += optical_flow_L.dy();
majik 0:21019d94ad33 60 flow_dx_R += optical_flow_R.dx();
majik 0:21019d94ad33 61 flow_dy_R += optical_flow_R.dy();
majik 0:21019d94ad33 62
majik 0:21019d94ad33 63 return true;
majik 0:21019d94ad33 64 }else
majik 0:21019d94ad33 65 return false;
majik 0:21019d94ad33 66 }