Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

reckon/reckon.cpp@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

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