Double integration library

Dependents:   Lab_1-2_dec

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers double_integration_classes.h Source File

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 }