ACS.cpp

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