Double integration library
Embed:
(wiki syntax)
Show/hide line numbers
double_integration_classes.h
00001 #include "matrix_math.h" 00002 #include "mbed.h" 00003 00004 #define MINIMUM_VALUE 600/16833.0*981.0 00005 00006 void rotation(double angle[], double* a, bool approx=false, double correction=981); //236 +30 perations 00007 00008 class Position_variables{ 00009 private: 00010 double* _position; 00011 double* _speed; 00012 double _h; 00013 public: 00014 Position_variables(); 00015 double* get_position(); 00016 double* get_speed(); 00017 00018 void set_h(double h); 00019 00020 void update_position(double* acceleration,Serial* pc); //534 + 54 operations 00021 void update_position_fast(double* acceleration,Serial* pc); //96 + 1 operations 00022 }; 00023 00024 class Angle_variables{ 00025 private: 00026 double* _angle; 00027 double _h; 00028 public: 00029 Angle_variables(); 00030 double* get_angle(); 00031 00032 void set_h(double h); 00033 00034 void update_angle(double* angular_velocity,Serial* pc); //48 operations 00035 }; 00036 00037 void rotation(double angle[], double* a, bool approx, double correction){ 00038 double tmp1[3][1], tmp2[3][1]; 00039 //rotation matrix for the various axis ROTATION IS IN COUNTERCLOCKWISE 00040 double R_x[3][3]={{1,0,0},{0,cos(angle[0]),-sin(angle[0])},{0,sin(angle[0]),cos(angle[0])}}; 00041 double R_y[3][3]={{cos(angle[1]),0,sin(angle[1])},{0,1,0},{-sin(angle[1]),0,cos(angle[1])}}; 00042 double R_z[3][3]={{cos(angle[2]),-sin(angle[2]),0},{sin(angle[2]),cos(angle[2]),0},{0,0,1}}; 00043 00044 matrix_mult(&R_x[0][0],a,&tmp1[0][0],3,3,1); //8*9 00045 matrix_mult(&R_y[0][0],&tmp1[0][0],&tmp2[0][0],3,3,1); //8*9 00046 matrix_mult(&R_z[0][0],&tmp2[0][0],&tmp1[0][0],3,3,1); //8*9 00047 tmp1[2][0]=tmp1[2][0]-correction; //2 00048 if((approx)&&(abs(tmp1[2][0])<MINIMUM_VALUE)) tmp1[2][0]=0.0; 00049 matrix_copy(a,&tmp1[0][0],3,1); //6*3 00050 00051 return; 00052 } 00053 00054 Position_variables::Position_variables(){ 00055 _position=new double[3]; 00056 _speed=new double[3]; 00057 _h=1; 00058 00059 for(int i=0;i<3;i++){ 00060 _position[i]=0; 00061 _speed[i]=0; 00062 } 00063 } 00064 00065 double* Position_variables::get_position(){ 00066 double* ptr=new double[3]; 00067 for(int i=0;i<3;i++) ptr[i]=_position[i]; 00068 return ptr; 00069 } 00070 double* Position_variables::get_speed(){ 00071 double* ptr=new double[3]; 00072 for(int i=0;i<3;i++) ptr[i]=_speed[i]; 00073 return ptr; 00074 } 00075 00076 void Position_variables::set_h(double h){ 00077 _h=h; 00078 } 00079 00080 void Position_variables::update_position(double* acceleration,Serial* pc){ //534 + 54 operations 00081 double A[6][6]={{1,0,0,_h,0,0},{0,1,0,0,_h,0},{0,0,1,0,0,_h},{0,0,0,1,0,0},{0,0,0,0,1,0},{0,0,0,0,0,1}}; 00082 double C[6][3]={{0,0,0},{0,0,0},{0,0,0},{_h,0,0},{0,_h,0},{0,0,_h}}; 00083 double y[6][1],tmp1[6][1],tmp2[6][1]; 00084 00085 for(int i=0;i<6;i++){ //6 operations 00086 if(i<3) y[i][0]=_position[i]; 00087 else y[i][0]=_speed[i-3]; 00088 } 00089 00090 matrix_mult(&A[0][0],&y[0][0],&tmp1[0][0],6,6,1); //8*36 operations 00091 matrix_mult(&C[0][0],acceleration,&tmp2[0][0],6,3,1); //8*18 operations 00092 matrix_sum(&tmp1[0][0],&tmp2[0][0],6,1); //9*6 operations 00093 matrix_copy(&y[0][0],&tmp1[0][0],6,1); //6*6 operations 00094 00095 for(int i=0;i<6;i++){ //6 operations 00096 if(i<3) _position[i]=y[i][0]; 00097 else{ 00098 if(acceleration[i-3]==0) _speed[i-3]=0; 00099 else _speed[i-3]=y[i][0]; 00100 } 00101 } 00102 00103 return; 00104 } 00105 00106 void Position_variables::update_position_fast(double* acceleration,Serial* pc){ //96 + 1 operations 00107 double* tmp; 00108 00109 tmp=_speed; 00110 matrix_mult(tmp,_h,3,1); //7*3 operations 00111 matrix_sum(_position,tmp,3,1); //9*3 operations 00112 00113 matrix_mult(acceleration,_h,3,1); //7*3 operations 00114 matrix_sum(_speed,acceleration,3,1); //9*3 operations 00115 00116 return; 00117 } 00118 00119 //********************************************************************************** 00120 00121 Angle_variables::Angle_variables(){ 00122 _angle=new double[3]; 00123 _h=1; 00124 00125 for(int i=0;i<3;i++) _angle[i]=0; 00126 } 00127 00128 double* Angle_variables::get_angle(){ 00129 double* ptr=new double[3]; 00130 for(int i=0;i<3;i++) ptr[i]=_angle[i]; 00131 return ptr; 00132 } 00133 00134 void Angle_variables::set_h(double h){ 00135 _h=h; 00136 } 00137 00138 void Angle_variables::update_angle(double* angular_velocity,Serial* pc){ //48 operations 00139 matrix_mult(angular_velocity,_h,3,1); //7*3 operations 00140 matrix_sum(_angle,angular_velocity,3,1); //9*3 operations 00141 00142 return; 00143 }
Generated on Thu Jul 28 2022 22:04:10 by
1.7.2
SDI