Code for our FYDP -only one IMU works right now -RTOS is working
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 10:32:23 by 1.7.2