ACS.cpp

00001 #include "ACS.h"
00002 #include "MPU3300.h"
00003 #include "pin_config.h"
00004 #include "math.h"
00005 
00006 //PwmOut PWM1(PTD4);        //Functions used to generate PWM signal 
00007                         //PWM output comes from pins p6
00008 Serial pc1(USBTX, USBRX);
00009 SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk  PTE18,19,17
00010 DigitalOut SSN_MAG (PIN61); // ssn for magnetometer PTB11
00011 DigitalInOut DRDY (PIN47); // drdy for magnetometer PTA17
00012 DigitalOut ssn_gyr (PTE2);         //Slave Select pin of gyroscope  PTB16
00013 InterruptIn dr(PTC6);       //Interrupt pin for gyro PTC5
00014 PwmOut PWM1(D2);        //Functions used to generate PWM signal 
00015 PwmOut PWM2(D3); 
00016 PwmOut PWM3(D4);                   //PWM output comes from pins p6
00017 Ticker tr;              //Ticker function to give values for limited amount of time for gyro
00018 Timeout tr_mag;
00019 uint8_t trflag_mag;
00020 uint8_t trFlag;         //ticker Flag for gyro
00021 uint8_t drFlag;         //data-ready interrupt flag for gyro
00022 float pwm1;
00023 float pwm2;
00024 float pwm3;
00025 //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//
00026 
00027 void FUNC_ACS_GENPWM(float M[3])
00028  {
00029      
00030 
00031      printf("\n\rEnterd PWMGEN function\n\r\r");
00032      for(int i = 0 ; i<3;i++)
00033         {
00034             printf(" %f \t ",M[i]);
00035         }
00036      float DCx = 0;         //Duty cycle of Moment in x, y, z directions
00037      float ix = 0;          //Current sent in x, y, z TR's
00038      float timep = 0.02 ;  
00039      float Mx=M[0];            //Time period is set to 0.02s  
00040                              //Moment in x, y, z directions
00041      
00042       
00043      
00044         ix = Mx * 0.3 ;      //Moment and Current always have the linear relationship
00045        // ix = 0.554999;
00046         if( ix>0&& ix < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
00047          {
00048              DCx =  6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008;
00049              PWM1.period(timep);
00050              PWM1 = DCx/100 ;
00051          }
00052         else if( ix >= 0.006&& ix < 0.0116)
00053          { 
00054             DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648;
00055             PWM1.period(timep);
00056             PWM1 = DCx/100 ;             
00057          }
00058         else if (ix >= 0.0116&& ix < 0.0624)
00059          {
00060               
00061             DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878;
00062             PWM1.period(timep);
00063             PWM1 = DCx/100 ;            
00064          }
00065         else if(ix >= 0.0624&& ix < 0.555)
00066          {
00067             printf("\n\rACS entered if\n\r");
00068             DCx =  331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338;
00069             PWM1.period(timep);
00070             PWM1 = DCx/100 ;            
00071          }
00072          //printf("\n \r b4 %f ",ix);
00073           else if(ix==0)
00074          {
00075              printf("\n \r ix====0");
00076              DCx = 75;
00077             PWM1.period(timep);
00078             PWM1 = DCx/100 ;            
00079          }
00080          else
00081          {
00082             // printf("!!!!!!!!!!Error!!!!!!!!!");
00083          } 
00084     pwm1 = PWM1;
00085     //DCx = 25; 
00086     //PWM1 = 0.50;    
00087     printf("\n\r icx :%f pwm : %f \n\r",ix,pwm1);
00088     float DCy = 0;         //Duty cycle of Moment in x, y, z directions
00089      float iy = 0;          //Current sent in x, y, z TR's
00090        
00091     float My=M[1];            //Time period is set to 0.2s  
00092                              //Moment in x, y, z directions
00093       
00094      
00095         iy = My * 0.3 ;      //Moment and Current always have the linear relationship
00096      
00097         if( iy>0&& iy < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
00098          {
00099              DCy =  6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008;
00100              PWM2.period(timep);
00101              PWM2 = DCy/100 ;
00102          }
00103         else if( iy >= 0.006&& iy < 0.0116)
00104          { 
00105             DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648;
00106             PWM2.period(timep);
00107             PWM2 = DCy/100 ;             
00108          }
00109         else if (iy >= 0.0116&& iy < 0.0624)
00110          {
00111               
00112             DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878;
00113             PWM2.period(timep);
00114             PWM2 = DCy/100 ;            
00115          }
00116         else if(iy >= 0.0624&& iy < 0.555)
00117          {
00118             printf("\n\rACS entered if\n\r");
00119             DCy =  331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338;
00120             PWM2.period(timep);
00121             PWM2 = DCy/100 ;            
00122          }
00123          else if(iy==0)
00124          {
00125              DCy = 0;
00126             PWM2.period(timep);
00127             PWM2 = DCy/100 ;            
00128          }
00129          else
00130          {
00131             // printf("!!!!!!!!!!Error!!!!!!!!!");
00132          } 
00133     
00134     pwm2 = PWM2; 
00135     printf("\n\r icy :%f pwm : %f \n\r",iy,pwm2);    
00136          
00137          
00138     float DCz = 0;         //Duty cycle of Moment in x, y, z directions
00139     float iz = 0;          //Current sent in x, y, z TR's
00140        
00141      float Mz=M[2];            //Time period is set to 0.2s  
00142                              //Moment in x, y, z directions
00143       
00144      
00145         iz = Mz * 0.3 ;      //Moment and Current always have the linear relationship
00146      
00147         if( iz>0&& iz < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
00148          {
00149              DCz =  6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008;
00150              PWM3.period(timep);
00151              PWM3 = DCz/100 ;
00152          }
00153         else if( iz >= 0.006&& iz < 0.0116)
00154          { 
00155             DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648;
00156             PWM3.period(timep);
00157             PWM3 = DCz/100 ;             
00158          }
00159         else if (iz >= 0.0116&& iz < 0.0624)
00160          {
00161               
00162             DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878;
00163             PWM3.period(timep);
00164             PWM3 = DCz/100 ;            
00165          }
00166         else if(iz >= 0.0624&& iz < 0.555)
00167          {
00168             printf("\n\rACS entered if\n\r");
00169             DCz =  331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338;
00170             PWM3.period(timep);
00171             PWM3 = DCz/100 ;            
00172          }
00173          else if(iz==0)
00174          {
00175              DCz = 0;
00176             PWM3.period(timep);
00177             PWM3 = DCz/100 ;            
00178          }
00179          else
00180          {
00181             // printf("!!!!!!!!!!Error!!!!!!!!!");
00182          }   
00183     pwm3 = PWM3; 
00184     printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3); 
00185     
00186 printf("\n\rExited PWMGEN function\n\r");
00187 }
00188 /*-------------------------------------------------------------------------------------------------------------------------------------------------------
00189 -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/
00190 
00191 void trsub_mag()
00192 {
00193   trflag_mag=0;
00194 } 
00195 
00196 void FUNC_ACS_MAG_INIT()
00197  {
00198   //DRDY.output();
00199   DRDY = 0;
00200   int a ;
00201   a=DRDY;
00202   printf("\n\r DRDY is %d\n\r",a);
00203   SSN_MAG=1;                                    //pin is disabled
00204   spi_acs.format(8,0);                         //   8bits,Mode 0
00205   spi_acs.frequency(100000);                   //clock frequency
00206   
00207   SSN_MAG=0;                                   // Selecting pin
00208   wait_ms(10);                            //accounts for delay.can be minimised.
00209   
00210   spi_acs.write(0x83);                      //
00211   
00212   wait_ms(10);              
00213   
00214   unsigned char i;
00215   for(i=0;i<3;i++)//initialising values.
00216       {
00217          spi_acs.write(0x00);              //MSB of X,y,Z
00218          spi_acs.write(0xc8);             //LSB of X,Y,z;pointer increases automatically.
00219         }
00220    SSN_MAG=1;
00221  
00222 }
00223 
00224 float* FUNC_ACS_MAG_EXEC()
00225 {
00226    printf("\n\rEntered magnetometer function\n\r");
00227    //DRDY.output();
00228    DRDY.write(0);
00229    int a;
00230    a = DRDY;
00231    //printf("\n\r DRDY is %d\n\r",a);
00232    SSN_MAG=0;                                //enabling slave to measure the values
00233    wait_ms(10);
00234    spi_acs.write(0x82);                     //initiates measurement
00235    wait_ms(10);
00236    spi_acs.write(0x01);                   //selecting x,y and z axes, measurement starts now
00237    SSN_MAG=1;
00238    wait_ms(10);
00239 
00240    trflag_mag=1;        
00241    tr_mag.attach(&trsub_mag,1);   //runs in background,makes trflag_mag=0 after 1s
00242    DRDY.input();
00243    while(trflag_mag)              /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else 
00244                                  loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and 
00245                                programme is terminated*/
00246   {
00247     wait_ms(5);
00248     if(DRDY==1)
00249     {
00250         printf("\n\rwth\n");
00251         SSN_MAG=0;
00252         spi_acs.write(0xc9);                  //command  byte for retrieving data
00253  
00254         unsigned char axis;
00255         float Bnewvalue[3]={0.0,0.0,0.0};
00256         int32_t Bvalue[3]={0,0,0}; 
00257         int32_t a= pow(2.0,24.0);
00258         int32_t b= pow(2.0,23.0);
00259  
00260         for(axis=0;axis<3;axis++)
00261         {
00262             Bvalue[axis]=spi_acs.write(0x00)<<16;    //MSB 1 is send first 
00263             wait_ms(10);
00264             Bvalue[axis]|=spi_acs.write(0x00)<<8;    //MSB 2 is send next
00265             wait_ms(10);
00266             Bvalue[axis]|=spi_acs.write(0x00);       //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration
00267   
00268    
00269             if((Bvalue[axis]&b)==b)              
00270             {
00271                 Bvalue[axis]=Bvalue[axis]-a;   //converting 2s complement to  signed decimal
00272 
00273             }
00274             Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0);  //1 LSB=(22nT)...final value of field obtained in micro tesla
00275   
00276             wait_ms(10);
00277             printf("\t%lf\n\r",Bnewvalue[axis]);
00278 
00279         }
00280         SSN_MAG=1;
00281         /* for test only to removed */
00282         Bnewvalue[0]=Bnewvalue[1]=Bnewvalue[2]=100;
00283         return Bnewvalue;          //return here? doubt..
00284         break;
00285     }
00286     
00287  }
00288  
00289 } 
00290 /*------------------------------------------------------------------------------------------------------------------------------------------------------
00291 -------------------------------------------torque to moment conversion------------------------------------------------------------------------------------------*/
00292 void moment_calc (float tauc[3], float b[3], float moment[3])
00293 {
00294   float b1;
00295   b1 = pow(b[0],2) + pow(b[1],2) +pow(b[2],2) ;
00296   moment[0] = ((tauc[1]*b[2])-(tauc[2]*b[1]))/b1;
00297   moment[1] = ((tauc[2]*b[0])-(tauc[0]*b[2]))/b1;
00298   moment[2] = ((tauc[0]*b[1])-(tauc[1]*b[0]))/b1;
00299   
00300 }
00301 
00302 
00303 /*------------------------------------------------------------------------------------------------------------------------------------------------------
00304 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
00305 
00306 void FUNC_ACS_CNTRLALGO(float b[3],float omega[3],float tauc[3])
00307 {
00308     float db[3]; /// inputs
00309 //initialization
00310     float bb[3] = {0, 0, 0};
00311     float d[3] = {0, 0, 0};
00312     float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}};
00313     float den = 0; 
00314     float den2;
00315     int i, j; //temporary variables
00316     float Mu[2], z[2], dv[2], v[2], u[2]; //outputs
00317    // float tauc[3];
00318     //float *tauc1;
00319     float invJm[3][3];
00320     float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4;
00321     printf("\n\r Entered cntrl algo\n\r");
00322     //structure parameters
00323 
00324     void inverse (float mat[3][3], float inv[3][3]); 
00325     void getInput (float x[9]);
00326     //functions
00327  
00328 ////////// Input from Matlab //////////////
00329   //  while(1)                          //removed assumin while is used coz of matlab
00330     //{
00331        
00332  /*getInput(inputs);
00333 //while(1)
00334  b[0] = inputs[0];
00335  b[1] = inputs[1];
00336  b[2] = inputs[2];
00337  db[0] = inputs[3];
00338  db[1] = inputs[4];
00339  db[2] = inputs[5]; 
00340  omega[0] = inputs[6];
00341  omega[1] = inputs[7];
00342  omega[2] = inputs[8];*/
00343 /////////// Control Algorithm //////////////////////
00344 // calculate norm b, norm db
00345         tauc[0] =tauc[1] =tauc[2]=0 ;
00346         
00347         den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2]));
00348         den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]);
00349        
00350         for(i=0;i<3;i++)
00351         {
00352             db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3));
00353 //db[i]/=den*den*den;
00354         }
00355         
00356         for(i=0;i<3;i++)
00357         {
00358          //   printf("\n\rreached here\n\r");
00359             if(den!=0)
00360                 b[i]=b[i]/den;                                      //there is a problem here. The code gets stuck here.  Maf value is required 
00361                 
00362         }
00363         //printf("\nhere\n");
00364 // select kz, kmu, gamma
00365         if(b[0]>0.9 || b[0]<-0.9)
00366         {
00367             kz = kz2;
00368             kmu = kmu2;
00369             gamma = gamma2;
00370         }
00371         printf("\nhere\n");
00372 // calculate Mu, v, dv, z, u
00373         for(i=0;i<2;i++)
00374         {
00375             Mu[i] = b[i+1];
00376             v[i] = -kmu*Mu[i];
00377             dv[i] = -kmu*db[i+1];
00378             z[i] = db[i+1] - v[i];
00379             u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma);
00380         }
00381         inverse(Jm, invJm);
00382 // calculate cross(omega,J*omega)for(i=0;i<3;i++)
00383         for(i=0; i<3; i++)                          // for loop included after checking original code
00384         { 
00385         for(j=0;j<3;j++)
00386             bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]);
00387         }
00388 // calculate invJm*cross(omega,J*omega) store in d
00389         for(i=0;i<3;i++)
00390         {
00391             for(j=0;j<3;j++)
00392                 d[i] += bb[j]*invJm[i][j];
00393         }
00394 // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db) 
00395 // bb =[0;u-d(2:3)] 
00396 // store in bb
00397         bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]);
00398         bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]);
00399         bb[0] = 0;
00400 // calculate N 
00401 // reusing invJm as N
00402        
00403         for(i=0;i<3;i++)
00404         {
00405             d[i] = invJm[1][i];
00406             invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i];
00407             invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i];
00408             invJm[0][i] = b[i];
00409         }
00410 // calculate inv(N) store in Jm
00411         inverse(invJm, Jm);
00412 // calculate tauc
00413          printf("\n \r calculatin tauc");
00414         for(i=0;i<3;i++)
00415         {
00416            
00417             for(j=0;j<3;j++)
00418                 tauc[i] += Jm[i][j]*bb[j];
00419             //printf(" %d \t",i);
00420             //tauc1[i] = tauc[i];
00421             printf(" %f \t",tauc[i]);    
00422         }
00423         
00424         //printf("    %f \n ", tauc[2]);
00425         //return tauc;
00426     
00427 }
00428 /////////// Output to Matlab //////////////////
00429 /* for(i=0;i<3;i++) {
00430  printf("%f\n\r",tauc[i]*10000000);
00431  wait_ms(10);
00432  }
00433  }
00434  
00435 }*/
00436  void inverse(float mat[3][3], float inv[3][3])
00437 {
00438 int i, j;
00439 float det = 0;
00440 for(i=0;i<3;i++)
00441 { for(j=0;j<3;j++)
00442 inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3]
00443 [(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
00444 }
00445 det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]);
00446 for(i=0;i<3;i++)
00447 { for(j=0;j<3;j++)
00448 inv[i][j] /= det;
00449 }
00450 }/*
00451 void getInput (float x[9]) {
00452          //Functions used to generate PWM signal 
00453                         //PWM output comes from pins p6
00454 Serial pc1(USBTX, USBRX);
00455  char c[10];
00456  char tempchar[8];
00457  int i, j;
00458  //float f[9];
00459  long n = 0;
00460  float flval = 0;
00461  for(j=0;j<9;j++) {
00462  for(i=0;i<9;i++) {
00463  c[i] = pc1.getc(); if(i<8) {
00464  tempchar[i] = c[i];
00465  }
00466  }
00467  sscanf (tempchar, "%8x", &n);
00468  memcpy(&flval, &n, sizeof(long));
00469  printf("%f\n\r", flval);
00470  x[j] = flval;
00471  }
00472 }*/
00473 
00474 void trSub();               
00475 void drSub(); 
00476 void init_gyro();       
00477 float * FUNC_ACS_EXEC_GYR();
00478 
00479 void drSub()            //In this function we setting data-ready flag to 1              
00480 {
00481     drFlag=1;
00482 }
00483 void trSub()                    //In this function we are setting ticker flag to 0
00484 {
00485     trFlag=0;
00486 }
00487 void FUNC_ACS_INIT_GYR()
00488 {
00489     uint8_t response;               
00490     ssn_gyr=1;                  //Deselecting the chip 
00491     spi_acs.format(8,0);                // Spi format is 8 bits, and clock mode 3 
00492     spi_acs.frequency(1000000);     //frequency to be set as 1MHz
00493     drFlag=0;                   //Intially defining data-ready flag to be 0 
00494     dr.mode(PullDown);          
00495     dr.rise(&drSub);
00496     __disable_irq();
00497     
00498 /*Following the above mentioned algorithm for initializing the register and changing its configuration*/
00499     ssn_gyr=0;                      //Selecting chip(Mpu-3300)
00500     spi_acs.write(USER_CTRL|READFLAG);   //sending USER_CTRL address with read bit
00501     response=spi_acs.write(DUMMYBIT);   //sending dummy bit to get default values of the register
00502                         
00503     ssn_gyr=1;                  //Deselecting the chip  
00504     wait(0.1);                  //waiting according the product specifications 
00505     
00506     ssn_gyr=0;                  //again selecting the chip  
00507     spi_acs.write(USER_CTRL);           //sending USER_CTRL address without read bit 
00508     spi_acs.write(response|BIT_I2C_IF_DIS);  //disabling the I2C mode in the register
00509     ssn_gyr=1;                  //deselecting the chip 
00510     wait(0.1);                  // waiting for 100ms before going for another register 
00511     
00512     ssn_gyr=0;
00513     spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1 
00514     response=spi_acs.write(DUMMYBIT);
00515     ssn_gyr=1;
00516     wait(0.1);
00517         
00518     ssn_gyr=0;
00519     spi_acs.write(PWR_MGMT_1);
00520     response=spi_acs.write(response|BIT_CLKSEL_X);  //Selecting the X axis gyroscope as clock as mentioned above 
00521     ssn_gyr=1;                          
00522     wait(0.1);
00523     
00524     ssn_gyr=0;
00525     spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit
00526     response=spi_acs.write(DUMMYBIT);
00527     ssn_gyr=1;
00528     wait(0.1);
00529     
00530     ssn_gyr=0;
00531     spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register
00532     spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec
00533     ssn_gyr=1;
00534     wait(0.1);
00535     
00536     ssn_gyr=0;
00537     spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit
00538     response=spi_acs.write(DUMMYBIT);
00539     ssn_gyr=1;
00540     wait(0.1);
00541     
00542     ssn_gyr=0;
00543     spi_acs.write(CONFIG); //sending CONFIG address to write to register
00544     spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms
00545     ssn_gyr=1;
00546     wait(0.1);
00547     
00548     ssn_gyr=0;
00549     spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit
00550     response=spi_acs.write(DUMMYBIT);
00551     ssn_gyr=1;
00552     wait(0.1);
00553         
00554     ssn_gyr=0;
00555     spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register
00556     spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate
00557     ssn_gyr=1;
00558     wait(0.1);
00559     
00560     ssn_gyr=0;
00561     spi_acs.write(INT_ENABLE|READFLAG);       //sending address of INT_ENABLE with readflag
00562     response=spi_acs.write(DUMMYBIT);              //sending dummy byte to get the default values of the
00563                                                                           // regiser
00564     ssn_gyr=1;   
00565     wait(0.1);
00566     
00567     ssn_gyr=0;
00568     spi_acs.write(INT_ENABLE);                           //sending INT_ENABLE address to write to register
00569     spi_acs.write(response|BIT_DATA_RDY_ENABLE);  //Enbling data ready interrupt
00570     ssn_gyr=1;
00571     wait(0.1);
00572     
00573     __enable_irq();
00574 }
00575 
00576 float * FUNC_ACS_EXEC_GYR()
00577 {
00578     printf("\n\rEntered gyro\n\r");
00579     uint8_t response;
00580     uint8_t MSB,LSB;
00581     int16_t bit_data;
00582     float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required
00583     float senstivity = 145.6;     //senstivity is 145.6 for full scale mode of +/-225 deg/sec
00584     ssn_gyr=0;
00585     spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag
00586     response=spi_acs.write(DUMMYBIT); //
00587     ssn_gyr=1;
00588     wait(0.1);
00589         
00590     ssn_gyr=0;
00591     spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
00592     response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep
00593     ssn_gyr=1;
00594     wait(0.1);
00595     
00596     trFlag=1;
00597     tr.attach(&trSub,1); //executes the function trSub afer 1sec
00598     
00599     while(trFlag)
00600     {
00601         wait_ms(5);   //This is required for this while loop to exit. I don't know why.
00602         if(drFlag==1)
00603         {
00604             ssn_gyr=0;
00605             spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag
00606             for(int i=0;i<3;i++)
00607             {
00608                 MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively
00609                 LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively
00610                 bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values
00611                 data[i]=(float)bit_data;
00612                 data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec
00613                 data[i]+=error[i]; //adding with error to remove offset errors
00614             }
00615             ssn_gyr=1;      
00616             for (int i=0;i<3;i++)
00617             {
00618                 printf("%f\t",data[i]); //printing the angular velocity values
00619             }
00620             printf("\n\r");
00621             break;
00622         }
00623             drFlag=0;
00624     }
00625     ssn_gyr=0;
00626     spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag
00627     response=spi_acs.write(DUMMYBIT);
00628     ssn_gyr=1;
00629     wait(0.1);
00630         
00631     ssn_gyr=0;
00632     spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
00633     response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode
00634     ssn_gyr=1;
00635     wait(0.1);
00636     printf("\n\rExited gyro\n\r");
00637     return data;
00638 }
00639 
00640  
00641  
00642 
00643 
00644