sakthi priya amirtharaj / Mbed 2 deprecated BAE_vr2_gingerbread2

mag n gyro included

Dependencies:   mbed-rtos mbed

Fork of BAE_vr2_gingerbread2 by Seeker of Truth ,

ACS.cpp@3:3d9e5f48b0c1, 2014-12-17 (annotated)

Committer:
raizel_varun
Date:
Wed Dec 17 09:52:27 2014 +0000
Revision:
3:3d9e5f48b0c1
Parent:
1:bd715ccef1bb 
AFTER PINS;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:e9c32e1df869 1 #include "ACS.h"
sakthipriya 0:e9c32e1df869 2 #include "MPU3300.h"
sakthipriya 0:e9c32e1df869 3
sakthipriya 0:e9c32e1df869 4 //PwmOut PWM1(PTD4); //Functions used to generate PWM signal
raizel_varun 3:3d9e5f48b0c1 5 //PWM output comes from pins p6
sakthipriya 0:e9c32e1df869 6 Serial pc1(USBTX, USBRX);
raizel_varun 3:3d9e5f48b0c1 7 SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk
raizel_varun 3:3d9e5f48b0c1 8 DigitalOut SSN_MAG (PIN61); // ssn for magnetometer
raizel_varun 3:3d9e5f48b0c1 9 DigitalIn DRDY (PIN47); // drdy for magnetometer
raizel_varun 3:3d9e5f48b0c1 10 DigitalOut ssn_gyr (PIN62); //Slave Select pin of gyroscope
raizel_varun 3:3d9e5f48b0c1 11 InterruptIn dr(PIN81); //Interrupt pin for gyro
sakthipriya 1:bd715ccef1bb 12 PwmOut PWM1(A3); //Functions used to generate PWM signal
sakthipriya 1:bd715ccef1bb 13 PwmOut PWM2(A4);
sakthipriya 1:bd715ccef1bb 14 PwmOut PWM3(A5); //PWM output comes from pins p6
sakthipriya 0:e9c32e1df869 15 Ticker tr; //Ticker function to give values for limited amount of time for gyro
sakthipriya 0:e9c32e1df869 16 Timeout tr_mag;
sakthipriya 0:e9c32e1df869 17 uint8_t trflag_mag;
sakthipriya 0:e9c32e1df869 18 uint8_t trFlag; //ticker Flag for gyro
sakthipriya 0:e9c32e1df869 19 uint8_t drFlag; //data-ready interrupt flag for gyro
sakthipriya 0:e9c32e1df869 20
sakthipriya 0:e9c32e1df869 21 //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//
sakthipriya 0:e9c32e1df869 22
sakthipriya 0:e9c32e1df869 23 void FUNC_ACS_GENPWM(float M[3])
sakthipriya 0:e9c32e1df869 24 {
sakthipriya 0:e9c32e1df869 25
sakthipriya 0:e9c32e1df869 26
sakthipriya 0:e9c32e1df869 27 printf("\nEnterd PWMGEN function\n");
sakthipriya 0:e9c32e1df869 28 float DCx = 0; //Duty cycle of Moment in x, y, z directions
sakthipriya 0:e9c32e1df869 29 float ix = 0; //Current sent in x, y, z TR's
sakthipriya 0:e9c32e1df869 30 float timep = 0.02 ;
sakthipriya 0:e9c32e1df869 31 float Mx=M[0]; //Time period is set to 0.02s
sakthipriya 0:e9c32e1df869 32 //Moment in x, y, z directions
sakthipriya 0:e9c32e1df869 33
sakthipriya 0:e9c32e1df869 34
sakthipriya 0:e9c32e1df869 35 ix = Mx * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:e9c32e1df869 36
sakthipriya 0:e9c32e1df869 37 if( ix>0&& ix < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:e9c32e1df869 38 {
sakthipriya 0:e9c32e1df869 39 DCx = 6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008;
sakthipriya 0:e9c32e1df869 40 PWM1.period(timep);
sakthipriya 0:e9c32e1df869 41 PWM1 = DCx/100 ;
sakthipriya 0:e9c32e1df869 42 }
sakthipriya 0:e9c32e1df869 43 else if( ix >= 0.006&& ix < 0.0116)
sakthipriya 0:e9c32e1df869 44 {
sakthipriya 0:e9c32e1df869 45 DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648;
sakthipriya 0:e9c32e1df869 46 PWM1.period(timep);
sakthipriya 0:e9c32e1df869 47 PWM1 = DCx/100 ;
sakthipriya 0:e9c32e1df869 48 }
sakthipriya 0:e9c32e1df869 49 else if (ix >= 0.0116&& ix < 0.0624)
sakthipriya 0:e9c32e1df869 50 {
sakthipriya 0:e9c32e1df869 51
sakthipriya 0:e9c32e1df869 52 DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878;
sakthipriya 0:e9c32e1df869 53 PWM1.period(timep);
sakthipriya 0:e9c32e1df869 54 PWM1 = DCx/100 ;
sakthipriya 0:e9c32e1df869 55 }
sakthipriya 0:e9c32e1df869 56 else if(ix >= 0.0624&& ix < 0.555)
sakthipriya 0:e9c32e1df869 57 {
sakthipriya 0:e9c32e1df869 58 printf("\nACS entered if\n");
sakthipriya 0:e9c32e1df869 59 DCx = 331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338;
sakthipriya 0:e9c32e1df869 60 PWM1.period(timep);
sakthipriya 0:e9c32e1df869 61 PWM1 = DCx/100 ;
sakthipriya 0:e9c32e1df869 62 }
sakthipriya 0:e9c32e1df869 63 else if(ix==0)
sakthipriya 0:e9c32e1df869 64 {
sakthipriya 0:e9c32e1df869 65 DCx = 0;
sakthipriya 0:e9c32e1df869 66 PWM1.period(timep);
sakthipriya 0:e9c32e1df869 67 PWM1 = DCx/100 ;
sakthipriya 0:e9c32e1df869 68 }
sakthipriya 0:e9c32e1df869 69 else
sakthipriya 0:e9c32e1df869 70 {
sakthipriya 0:e9c32e1df869 71 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:e9c32e1df869 72 }
sakthipriya 0:e9c32e1df869 73 float DCy = 0; //Duty cycle of Moment in x, y, z directions
sakthipriya 0:e9c32e1df869 74 float iy = 0; //Current sent in x, y, z TR's
sakthipriya 0:e9c32e1df869 75
sakthipriya 0:e9c32e1df869 76 float My=M[1]; //Time period is set to 0.2s
sakthipriya 0:e9c32e1df869 77 //Moment in x, y, z directions
sakthipriya 0:e9c32e1df869 78
sakthipriya 0:e9c32e1df869 79
sakthipriya 0:e9c32e1df869 80 iy = My * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:e9c32e1df869 81
sakthipriya 0:e9c32e1df869 82 if( iy>0&& iy < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:e9c32e1df869 83 {
sakthipriya 0:e9c32e1df869 84 DCy = 6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008;
sakthipriya 0:e9c32e1df869 85 PWM2.period(timep);
sakthipriya 0:e9c32e1df869 86 PWM2 = DCy/100 ;
sakthipriya 0:e9c32e1df869 87 }
sakthipriya 0:e9c32e1df869 88 else if( iy >= 0.006&& iy < 0.0116)
sakthipriya 0:e9c32e1df869 89 {
sakthipriya 0:e9c32e1df869 90 DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648;
sakthipriya 0:e9c32e1df869 91 PWM2.period(timep);
sakthipriya 0:e9c32e1df869 92 PWM2 = DCy/100 ;
sakthipriya 0:e9c32e1df869 93 }
sakthipriya 0:e9c32e1df869 94 else if (iy >= 0.0116&& iy < 0.0624)
sakthipriya 0:e9c32e1df869 95 {
sakthipriya 0:e9c32e1df869 96
sakthipriya 0:e9c32e1df869 97 DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878;
sakthipriya 0:e9c32e1df869 98 PWM2.period(timep);
sakthipriya 0:e9c32e1df869 99 PWM2 = DCy/100 ;
sakthipriya 0:e9c32e1df869 100 }
sakthipriya 0:e9c32e1df869 101 else if(iy >= 0.0624&& iy < 0.555)
sakthipriya 0:e9c32e1df869 102 {
sakthipriya 0:e9c32e1df869 103 printf("\nACS entered if\n");
sakthipriya 0:e9c32e1df869 104 DCy = 331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338;
sakthipriya 0:e9c32e1df869 105 PWM2.period(timep);
sakthipriya 0:e9c32e1df869 106 PWM2 = DCy/100 ;
sakthipriya 0:e9c32e1df869 107 }
sakthipriya 0:e9c32e1df869 108 else if(iy==0)
sakthipriya 0:e9c32e1df869 109 {
sakthipriya 0:e9c32e1df869 110 DCy = 0;
sakthipriya 0:e9c32e1df869 111 PWM2.period(timep);
sakthipriya 0:e9c32e1df869 112 PWM2 = DCy/100 ;
sakthipriya 0:e9c32e1df869 113 }
sakthipriya 0:e9c32e1df869 114 else
sakthipriya 0:e9c32e1df869 115 {
sakthipriya 0:e9c32e1df869 116 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:e9c32e1df869 117 }
sakthipriya 0:e9c32e1df869 118 float DCz = 0; //Duty cycle of Moment in x, y, z directions
sakthipriya 0:e9c32e1df869 119 float iz = 0; //Current sent in x, y, z TR's
sakthipriya 0:e9c32e1df869 120
sakthipriya 0:e9c32e1df869 121 float Mz=M[2]; //Time period is set to 0.2s
sakthipriya 0:e9c32e1df869 122 //Moment in x, y, z directions
sakthipriya 0:e9c32e1df869 123
sakthipriya 0:e9c32e1df869 124
sakthipriya 0:e9c32e1df869 125 iz = Mz * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:e9c32e1df869 126
sakthipriya 0:e9c32e1df869 127 if( iz>0&& iz < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:e9c32e1df869 128 {
sakthipriya 0:e9c32e1df869 129 DCz = 6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008;
sakthipriya 0:e9c32e1df869 130 PWM3.period(timep);
sakthipriya 0:e9c32e1df869 131 PWM3 = DCz/100 ;
sakthipriya 0:e9c32e1df869 132 }
sakthipriya 0:e9c32e1df869 133 else if( iz >= 0.006&& iz < 0.0116)
sakthipriya 0:e9c32e1df869 134 {
sakthipriya 0:e9c32e1df869 135 DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648;
sakthipriya 0:e9c32e1df869 136 PWM3.period(timep);
sakthipriya 0:e9c32e1df869 137 PWM3 = DCz/100 ;
sakthipriya 0:e9c32e1df869 138 }
sakthipriya 0:e9c32e1df869 139 else if (iz >= 0.0116&& iz < 0.0624)
sakthipriya 0:e9c32e1df869 140 {
sakthipriya 0:e9c32e1df869 141
sakthipriya 0:e9c32e1df869 142 DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878;
sakthipriya 0:e9c32e1df869 143 PWM3.period(timep);
sakthipriya 0:e9c32e1df869 144 PWM3 = DCz/100 ;
sakthipriya 0:e9c32e1df869 145 }
sakthipriya 0:e9c32e1df869 146 else if(iz >= 0.0624&& iz < 0.555)
sakthipriya 0:e9c32e1df869 147 {
sakthipriya 0:e9c32e1df869 148 printf("\nACS entered if\n");
sakthipriya 0:e9c32e1df869 149 DCz = 331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338;
sakthipriya 0:e9c32e1df869 150 PWM3.period(timep);
sakthipriya 0:e9c32e1df869 151 PWM3 = DCz/100 ;
sakthipriya 0:e9c32e1df869 152 }
sakthipriya 0:e9c32e1df869 153 else if(iz==0)
sakthipriya 0:e9c32e1df869 154 {
sakthipriya 0:e9c32e1df869 155 DCz = 0;
sakthipriya 0:e9c32e1df869 156 PWM3.period(timep);
sakthipriya 0:e9c32e1df869 157 PWM3 = DCz/100 ;
sakthipriya 0:e9c32e1df869 158 }
sakthipriya 0:e9c32e1df869 159 else
sakthipriya 0:e9c32e1df869 160 {
sakthipriya 0:e9c32e1df869 161 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:e9c32e1df869 162 }
sakthipriya 0:e9c32e1df869 163
sakthipriya 0:e9c32e1df869 164 printf("\nExited PWMGEN function\n");
sakthipriya 0:e9c32e1df869 165 }
sakthipriya 0:e9c32e1df869 166 /*-------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:e9c32e1df869 167 -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/
sakthipriya 0:e9c32e1df869 168
sakthipriya 0:e9c32e1df869 169 void trsub_mag()
sakthipriya 0:e9c32e1df869 170 {
sakthipriya 0:e9c32e1df869 171 trflag_mag=0;
sakthipriya 0:e9c32e1df869 172 }
sakthipriya 0:e9c32e1df869 173
sakthipriya 0:e9c32e1df869 174 void FUNC_ACS_MAG_INIT()
sakthipriya 0:e9c32e1df869 175 {
sakthipriya 0:e9c32e1df869 176
sakthipriya 0:e9c32e1df869 177 SSN_MAG=1; //pin is disabled
sakthipriya 0:e9c32e1df869 178 spi_acs.format(8,0); // 8bits,Mode 0
sakthipriya 0:e9c32e1df869 179 spi_acs.frequency(100000); //clock frequency
sakthipriya 0:e9c32e1df869 180
sakthipriya 0:e9c32e1df869 181 SSN_MAG=0; // Selecting pin
sakthipriya 0:e9c32e1df869 182 wait_ms(10); //accounts for delay.can be minimised.
sakthipriya 0:e9c32e1df869 183
sakthipriya 0:e9c32e1df869 184 spi_acs.write(0x83); //
sakthipriya 0:e9c32e1df869 185
sakthipriya 0:e9c32e1df869 186 wait_ms(10);
sakthipriya 0:e9c32e1df869 187
sakthipriya 0:e9c32e1df869 188 unsigned char i;
sakthipriya 0:e9c32e1df869 189 for(i=0;i<3;i++)//initialising values.
sakthipriya 0:e9c32e1df869 190 {
sakthipriya 0:e9c32e1df869 191 spi_acs.write(0x00); //MSB of X,y,Z
sakthipriya 0:e9c32e1df869 192 spi_acs.write(0xc8); //LSB of X,Y,z;pointer increases automatically.
sakthipriya 0:e9c32e1df869 193 }
sakthipriya 0:e9c32e1df869 194 SSN_MAG=1;
sakthipriya 0:e9c32e1df869 195
sakthipriya 0:e9c32e1df869 196 }
sakthipriya 0:e9c32e1df869 197
sakthipriya 0:e9c32e1df869 198 float* FUNC_ACS_MAG_EXEC()
sakthipriya 0:e9c32e1df869 199 {
sakthipriya 0:e9c32e1df869 200 printf("\nEntered magnetometer function\n");
sakthipriya 0:e9c32e1df869 201 SSN_MAG=0; //enabling slave to measure the values
sakthipriya 0:e9c32e1df869 202 wait_ms(10);
sakthipriya 0:e9c32e1df869 203 spi_acs.write(0x82); //initiates measurement
sakthipriya 0:e9c32e1df869 204 wait_ms(10);
sakthipriya 0:e9c32e1df869 205 spi_acs.write(0x01); //selecting x,y and z axes, measurement starts now
sakthipriya 0:e9c32e1df869 206 SSN_MAG=1;
sakthipriya 0:e9c32e1df869 207 wait_ms(10);
sakthipriya 0:e9c32e1df869 208
sakthipriya 0:e9c32e1df869 209 trflag_mag=1;
sakthipriya 0:e9c32e1df869 210 tr_mag.attach(&trsub_mag,1); //runs in background,makes trflag_mag=0 after 1s
sakthipriya 0:e9c32e1df869 211
sakthipriya 0:e9c32e1df869 212 while(trflag_mag) /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else
sakthipriya 0:e9c32e1df869 213 loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and
sakthipriya 0:e9c32e1df869 214 programme is terminated*/
sakthipriya 0:e9c32e1df869 215 {
sakthipriya 0:e9c32e1df869 216 wait_ms(5);
sakthipriya 0:e9c32e1df869 217 if(DRDY==1)
sakthipriya 0:e9c32e1df869 218 {
sakthipriya 0:e9c32e1df869 219 SSN_MAG=0;
sakthipriya 0:e9c32e1df869 220 spi_acs.write(0xc9); //command byte for retrieving data
sakthipriya 0:e9c32e1df869 221
sakthipriya 0:e9c32e1df869 222 unsigned char axis;
sakthipriya 0:e9c32e1df869 223 float Bnewvalue[3]={0.0,0.0,0.0};
sakthipriya 0:e9c32e1df869 224 int32_t Bvalue[3]={0,0,0};
sakthipriya 0:e9c32e1df869 225 int32_t a= pow(2.0,24.0);
sakthipriya 0:e9c32e1df869 226 int32_t b= pow(2.0,23.0);
sakthipriya 0:e9c32e1df869 227
sakthipriya 0:e9c32e1df869 228 for(axis=0;axis<3;axis++)
sakthipriya 0:e9c32e1df869 229 {
sakthipriya 0:e9c32e1df869 230 Bvalue[axis]=spi_acs.write(0x00)<<16; //MSB 1 is send first
sakthipriya 0:e9c32e1df869 231 wait_ms(10);
sakthipriya 0:e9c32e1df869 232 Bvalue[axis]|=spi_acs.write(0x00)<<8; //MSB 2 is send next
sakthipriya 0:e9c32e1df869 233 wait_ms(10);
sakthipriya 0:e9c32e1df869 234 Bvalue[axis]|=spi_acs.write(0x00); //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration
sakthipriya 0:e9c32e1df869 235
sakthipriya 0:e9c32e1df869 236
sakthipriya 0:e9c32e1df869 237 if((Bvalue[axis]&b)==b)
sakthipriya 0:e9c32e1df869 238 {
sakthipriya 0:e9c32e1df869 239 Bvalue[axis]=Bvalue[axis]-a; //converting 2s complement to signed decimal
sakthipriya 0:e9c32e1df869 240
sakthipriya 0:e9c32e1df869 241 }
sakthipriya 0:e9c32e1df869 242 Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0); //1 LSB=(22nT)...final value of field obtained in micro tesla
sakthipriya 0:e9c32e1df869 243
sakthipriya 0:e9c32e1df869 244 wait_ms(10);
sakthipriya 0:e9c32e1df869 245 printf("\t%lf\n",Bnewvalue[axis]);
sakthipriya 0:e9c32e1df869 246
sakthipriya 0:e9c32e1df869 247 }
sakthipriya 0:e9c32e1df869 248 SSN_MAG=1;
sakthipriya 0:e9c32e1df869 249
sakthipriya 0:e9c32e1df869 250 return Bnewvalue; //return here? doubt..
sakthipriya 0:e9c32e1df869 251 break;
sakthipriya 0:e9c32e1df869 252 }
sakthipriya 0:e9c32e1df869 253
sakthipriya 0:e9c32e1df869 254 }
sakthipriya 0:e9c32e1df869 255
sakthipriya 0:e9c32e1df869 256 }
sakthipriya 0:e9c32e1df869 257 /*------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:e9c32e1df869 258 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
sakthipriya 0:e9c32e1df869 259
sakthipriya 0:e9c32e1df869 260 float * FUNC_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:e9c32e1df869 261 {
sakthipriya 0:e9c32e1df869 262 float db[3]; /// inputs
sakthipriya 0:e9c32e1df869 263 //initialization
sakthipriya 0:e9c32e1df869 264 float bb[3] = {0, 0, 0};
sakthipriya 0:e9c32e1df869 265 float d[3] = {0, 0, 0};
sakthipriya 0:e9c32e1df869 266 float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}};
sakthipriya 0:e9c32e1df869 267 float den = 0;
sakthipriya 0:e9c32e1df869 268 float den2;
sakthipriya 0:e9c32e1df869 269 int i, j; //temporary variables
sakthipriya 0:e9c32e1df869 270 float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs
sakthipriya 0:e9c32e1df869 271 float invJm[3][3];
sakthipriya 0:e9c32e1df869 272 float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4;
sakthipriya 0:e9c32e1df869 273 printf("Entered cntrl algo\n");
sakthipriya 0:e9c32e1df869 274 //structure parameters
sakthipriya 0:e9c32e1df869 275
sakthipriya 0:e9c32e1df869 276 void inverse (float mat[3][3], float inv[3][3]);
sakthipriya 0:e9c32e1df869 277 void getInput (float x[9]);
sakthipriya 0:e9c32e1df869 278 //functions
sakthipriya 0:e9c32e1df869 279
sakthipriya 0:e9c32e1df869 280 ////////// Input from Matlab //////////////
sakthipriya 0:e9c32e1df869 281 while(1)
sakthipriya 0:e9c32e1df869 282 {
sakthipriya 0:e9c32e1df869 283
sakthipriya 0:e9c32e1df869 284 /*getInput(inputs);
sakthipriya 0:e9c32e1df869 285 //while(1)
sakthipriya 0:e9c32e1df869 286 b[0] = inputs[0];
sakthipriya 0:e9c32e1df869 287 b[1] = inputs[1];
sakthipriya 0:e9c32e1df869 288 b[2] = inputs[2];
sakthipriya 0:e9c32e1df869 289 db[0] = inputs[3];
sakthipriya 0:e9c32e1df869 290 db[1] = inputs[4];
sakthipriya 0:e9c32e1df869 291 db[2] = inputs[5];
sakthipriya 0:e9c32e1df869 292 omega[0] = inputs[6];
sakthipriya 0:e9c32e1df869 293 omega[1] = inputs[7];
sakthipriya 0:e9c32e1df869 294 omega[2] = inputs[8];*/
sakthipriya 0:e9c32e1df869 295 /////////// Control Algorithm //////////////////////
sakthipriya 0:e9c32e1df869 296 // calculate norm b, norm db
sakthipriya 0:e9c32e1df869 297 den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2]));
sakthipriya 0:e9c32e1df869 298 den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]);
sakthipriya 0:e9c32e1df869 299
sakthipriya 0:e9c32e1df869 300 for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 301 {
sakthipriya 0:e9c32e1df869 302 db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3));
sakthipriya 0:e9c32e1df869 303 //db[i]/=den*den*den;
sakthipriya 0:e9c32e1df869 304 }
sakthipriya 0:e9c32e1df869 305
sakthipriya 0:e9c32e1df869 306 for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 307 {
sakthipriya 0:e9c32e1df869 308 printf("\nreached here\n");
sakthipriya 0:e9c32e1df869 309 if(den!=0)
sakthipriya 0:e9c32e1df869 310 //b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required
sakthipriya 0:e9c32e1df869 311 ;
sakthipriya 0:e9c32e1df869 312 }
sakthipriya 0:e9c32e1df869 313
sakthipriya 0:e9c32e1df869 314 // select kz, kmu, gamma
sakthipriya 0:e9c32e1df869 315 if(b[0]>0.9 || b[0]<-0.9)
sakthipriya 0:e9c32e1df869 316 {
sakthipriya 0:e9c32e1df869 317 kz = kz2;
sakthipriya 0:e9c32e1df869 318 kmu = kmu2;
sakthipriya 0:e9c32e1df869 319 gamma = gamma2;
sakthipriya 0:e9c32e1df869 320 }
sakthipriya 0:e9c32e1df869 321 // calculate Mu, v, dv, z, u
sakthipriya 0:e9c32e1df869 322 for(i=0;i<2;i++)
sakthipriya 0:e9c32e1df869 323 {
sakthipriya 0:e9c32e1df869 324 Mu[i] = b[i+1];
sakthipriya 0:e9c32e1df869 325 v[i] = -kmu*Mu[i];
sakthipriya 0:e9c32e1df869 326 dv[i] = -kmu*db[i+1];
sakthipriya 0:e9c32e1df869 327 z[i] = db[i+1] - v[i];
sakthipriya 0:e9c32e1df869 328 u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma);
sakthipriya 0:e9c32e1df869 329 }
sakthipriya 0:e9c32e1df869 330 inverse(Jm, invJm);
sakthipriya 0:e9c32e1df869 331 // calculate cross(omega,J*omega)for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 332
sakthipriya 0:e9c32e1df869 333 for(j=0;j<3;j++)
sakthipriya 0:e9c32e1df869 334 bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]);
sakthipriya 0:e9c32e1df869 335
sakthipriya 0:e9c32e1df869 336 // calculate invJm*cross(omega,J*omega) store in d
sakthipriya 0:e9c32e1df869 337 for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 338 {
sakthipriya 0:e9c32e1df869 339 for(j=0;j<3;j++)
sakthipriya 0:e9c32e1df869 340 d[i] += bb[j]*invJm[i][j];
sakthipriya 0:e9c32e1df869 341 }
sakthipriya 0:e9c32e1df869 342 // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db)
sakthipriya 0:e9c32e1df869 343 // bb =[0;u-d(2:3)]
sakthipriya 0:e9c32e1df869 344 // store in bb
sakthipriya 0:e9c32e1df869 345 bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]);
sakthipriya 0:e9c32e1df869 346 bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]);
sakthipriya 0:e9c32e1df869 347 bb[0] = 0;
sakthipriya 0:e9c32e1df869 348 // calculate N
sakthipriya 0:e9c32e1df869 349 // reusing invJm as N
sakthipriya 0:e9c32e1df869 350
sakthipriya 0:e9c32e1df869 351 for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 352 {
sakthipriya 0:e9c32e1df869 353 d[i] = invJm[1][i];
sakthipriya 0:e9c32e1df869 354 invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i];
sakthipriya 0:e9c32e1df869 355 invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i];
sakthipriya 0:e9c32e1df869 356 invJm[0][i] = b[i];
sakthipriya 0:e9c32e1df869 357 }
sakthipriya 0:e9c32e1df869 358 // calculate inv(N) store in Jm
sakthipriya 0:e9c32e1df869 359 inverse(invJm, Jm);
sakthipriya 0:e9c32e1df869 360 // calculate tauc
sakthipriya 0:e9c32e1df869 361 for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 362 {
sakthipriya 0:e9c32e1df869 363 for(j=0;j<3;j++)
sakthipriya 0:e9c32e1df869 364 tauc[i] += Jm[i][j]*bb[j];
sakthipriya 0:e9c32e1df869 365 }
sakthipriya 0:e9c32e1df869 366
sakthipriya 0:e9c32e1df869 367 return(tauc);
sakthipriya 0:e9c32e1df869 368 }
sakthipriya 0:e9c32e1df869 369 }
sakthipriya 0:e9c32e1df869 370 /////////// Output to Matlab //////////////////
sakthipriya 0:e9c32e1df869 371 /* for(i=0;i<3;i++) {
sakthipriya 0:e9c32e1df869 372 printf("%f\n",tauc[i]*10000000);
sakthipriya 0:e9c32e1df869 373 wait_ms(10);
sakthipriya 0:e9c32e1df869 374 }
sakthipriya 0:e9c32e1df869 375 }
sakthipriya 0:e9c32e1df869 376
sakthipriya 0:e9c32e1df869 377 }*/
sakthipriya 0:e9c32e1df869 378 void inverse(float mat[3][3], float inv[3][3])
sakthipriya 0:e9c32e1df869 379 {
sakthipriya 0:e9c32e1df869 380 int i, j;
sakthipriya 0:e9c32e1df869 381 float det = 0;
sakthipriya 0:e9c32e1df869 382 for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 383 { for(j=0;j<3;j++)
sakthipriya 0:e9c32e1df869 384 inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3]
sakthipriya 0:e9c32e1df869 385 [(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
sakthipriya 0:e9c32e1df869 386 }
sakthipriya 0:e9c32e1df869 387 det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]);
sakthipriya 0:e9c32e1df869 388 for(i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 389 { for(j=0;j<3;j++)
sakthipriya 0:e9c32e1df869 390 inv[i][j] /= det;
sakthipriya 0:e9c32e1df869 391 }
sakthipriya 0:e9c32e1df869 392 }/*
sakthipriya 0:e9c32e1df869 393 void getInput (float x[9]) {
sakthipriya 0:e9c32e1df869 394 //Functions used to generate PWM signal
sakthipriya 0:e9c32e1df869 395 //PWM output comes from pins p6
sakthipriya 0:e9c32e1df869 396 Serial pc1(USBTX, USBRX);
sakthipriya 0:e9c32e1df869 397 char c[10];
sakthipriya 0:e9c32e1df869 398 char tempchar[8];
sakthipriya 0:e9c32e1df869 399 int i, j;
sakthipriya 0:e9c32e1df869 400 //float f[9];
sakthipriya 0:e9c32e1df869 401 long n = 0;
sakthipriya 0:e9c32e1df869 402 float flval = 0;
sakthipriya 0:e9c32e1df869 403 for(j=0;j<9;j++) {
sakthipriya 0:e9c32e1df869 404 for(i=0;i<9;i++) {
sakthipriya 0:e9c32e1df869 405 c[i] = pc1.getc(); if(i<8) {
sakthipriya 0:e9c32e1df869 406 tempchar[i] = c[i];
sakthipriya 0:e9c32e1df869 407 }
sakthipriya 0:e9c32e1df869 408 }
sakthipriya 0:e9c32e1df869 409 sscanf (tempchar, "%8x", &n);
sakthipriya 0:e9c32e1df869 410 memcpy(&flval, &n, sizeof(long));
sakthipriya 0:e9c32e1df869 411 printf("%f\n", flval);
sakthipriya 0:e9c32e1df869 412 x[j] = flval;
sakthipriya 0:e9c32e1df869 413 }
sakthipriya 0:e9c32e1df869 414 }*/
sakthipriya 0:e9c32e1df869 415
sakthipriya 0:e9c32e1df869 416 void trSub();
sakthipriya 0:e9c32e1df869 417 void drSub();
sakthipriya 0:e9c32e1df869 418 void init_gyro();
sakthipriya 0:e9c32e1df869 419 float * FUNC_ACS_EXEC_GYR();
sakthipriya 0:e9c32e1df869 420
sakthipriya 0:e9c32e1df869 421 void drSub() //In this function we setting data-ready flag to 1
sakthipriya 0:e9c32e1df869 422 {
sakthipriya 0:e9c32e1df869 423 drFlag=1;
sakthipriya 0:e9c32e1df869 424 }
sakthipriya 0:e9c32e1df869 425 void trSub() //In this function we are setting ticker flag to 0
sakthipriya 0:e9c32e1df869 426 {
sakthipriya 0:e9c32e1df869 427 trFlag=0;
sakthipriya 0:e9c32e1df869 428 }
sakthipriya 0:e9c32e1df869 429 void FUNC_ACS_INIT_GYR()
sakthipriya 0:e9c32e1df869 430 {
sakthipriya 0:e9c32e1df869 431 uint8_t response;
sakthipriya 0:e9c32e1df869 432 ssn_gyr=1; //Deselecting the chip
sakthipriya 0:e9c32e1df869 433 spi_acs.format(8,3); // Spi format is 8 bits, and clock mode 3
sakthipriya 0:e9c32e1df869 434 spi_acs.frequency(1000000); //frequency to be set as 1MHz
sakthipriya 0:e9c32e1df869 435 drFlag=0; //Intially defining data-ready flag to be 0
sakthipriya 0:e9c32e1df869 436 dr.mode(PullDown);
sakthipriya 0:e9c32e1df869 437 dr.rise(&drSub);
sakthipriya 0:e9c32e1df869 438 __disable_irq();
sakthipriya 0:e9c32e1df869 439
sakthipriya 0:e9c32e1df869 440 /*Following the above mentioned algorithm for initializing the register and changing its configuration*/
sakthipriya 0:e9c32e1df869 441 ssn_gyr=0; //Selecting chip(Mpu-3300)
sakthipriya 0:e9c32e1df869 442 spi_acs.write(USER_CTRL|READFLAG); //sending USER_CTRL address with read bit
sakthipriya 0:e9c32e1df869 443 response=spi_acs.write(DUMMYBIT); //sending dummy bit to get default values of the register
sakthipriya 0:e9c32e1df869 444
sakthipriya 0:e9c32e1df869 445 ssn_gyr=1; //Deselecting the chip
sakthipriya 0:e9c32e1df869 446 wait(0.1); //waiting according the product specifications
sakthipriya 0:e9c32e1df869 447
sakthipriya 0:e9c32e1df869 448 ssn_gyr=0; //again selecting the chip
sakthipriya 0:e9c32e1df869 449 spi_acs.write(USER_CTRL); //sending USER_CTRL address without read bit
sakthipriya 0:e9c32e1df869 450 spi_acs.write(response|BIT_I2C_IF_DIS); //disabling the I2C mode in the register
sakthipriya 0:e9c32e1df869 451 ssn_gyr=1; //deselecting the chip
sakthipriya 0:e9c32e1df869 452 wait(0.1); // waiting for 100ms before going for another register
sakthipriya 0:e9c32e1df869 453
sakthipriya 0:e9c32e1df869 454 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 455 spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1
sakthipriya 0:e9c32e1df869 456 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e9c32e1df869 457 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 458 wait(0.1);
sakthipriya 0:e9c32e1df869 459
sakthipriya 0:e9c32e1df869 460 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 461 spi_acs.write(PWR_MGMT_1);
sakthipriya 0:e9c32e1df869 462 response=spi_acs.write(response|BIT_CLKSEL_X); //Selecting the X axis gyroscope as clock as mentioned above
sakthipriya 0:e9c32e1df869 463 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 464 wait(0.1);
sakthipriya 0:e9c32e1df869 465
sakthipriya 0:e9c32e1df869 466 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 467 spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit
sakthipriya 0:e9c32e1df869 468 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e9c32e1df869 469 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 470 wait(0.1);
sakthipriya 0:e9c32e1df869 471
sakthipriya 0:e9c32e1df869 472 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 473 spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register
sakthipriya 0:e9c32e1df869 474 spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec
sakthipriya 0:e9c32e1df869 475 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 476 wait(0.1);
sakthipriya 0:e9c32e1df869 477
sakthipriya 0:e9c32e1df869 478 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 479 spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit
sakthipriya 0:e9c32e1df869 480 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e9c32e1df869 481 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 482 wait(0.1);
sakthipriya 0:e9c32e1df869 483
sakthipriya 0:e9c32e1df869 484 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 485 spi_acs.write(CONFIG); //sending CONFIG address to write to register
sakthipriya 0:e9c32e1df869 486 spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms
sakthipriya 0:e9c32e1df869 487 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 488 wait(0.1);
sakthipriya 0:e9c32e1df869 489
sakthipriya 0:e9c32e1df869 490 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 491 spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit
sakthipriya 0:e9c32e1df869 492 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e9c32e1df869 493 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 494 wait(0.1);
sakthipriya 0:e9c32e1df869 495
sakthipriya 0:e9c32e1df869 496 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 497 spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register
sakthipriya 0:e9c32e1df869 498 spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate
sakthipriya 0:e9c32e1df869 499 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 500 wait(0.1);
sakthipriya 0:e9c32e1df869 501
sakthipriya 0:e9c32e1df869 502 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 503 spi_acs.write(INT_ENABLE|READFLAG); //sending address of INT_ENABLE with readflag
sakthipriya 0:e9c32e1df869 504 response=spi_acs.write(DUMMYBIT); //sending dummy byte to get the default values of the
sakthipriya 0:e9c32e1df869 505 // regiser
sakthipriya 0:e9c32e1df869 506 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 507 wait(0.1);
sakthipriya 0:e9c32e1df869 508
sakthipriya 0:e9c32e1df869 509 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 510 spi_acs.write(INT_ENABLE); //sending INT_ENABLE address to write to register
sakthipriya 0:e9c32e1df869 511 spi_acs.write(response|BIT_DATA_RDY_ENABLE); //Enbling data ready interrupt
sakthipriya 0:e9c32e1df869 512 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 513 wait(0.1);
sakthipriya 0:e9c32e1df869 514
sakthipriya 0:e9c32e1df869 515 __enable_irq();
sakthipriya 0:e9c32e1df869 516 }
sakthipriya 0:e9c32e1df869 517
sakthipriya 0:e9c32e1df869 518 float * FUNC_ACS_EXEC_GYR()
sakthipriya 0:e9c32e1df869 519 {
sakthipriya 0:e9c32e1df869 520 printf("\nEntered gyro\n");
sakthipriya 0:e9c32e1df869 521 uint8_t response;
sakthipriya 0:e9c32e1df869 522 uint8_t MSB,LSB;
sakthipriya 0:e9c32e1df869 523 int16_t bit_data;
sakthipriya 0:e9c32e1df869 524 float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required
sakthipriya 0:e9c32e1df869 525 float senstivity = 145.6; //senstivity is 145.6 for full scale mode of +/-225 deg/sec
sakthipriya 0:e9c32e1df869 526 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 527 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag
sakthipriya 0:e9c32e1df869 528 response=spi_acs.write(DUMMYBIT); //
sakthipriya 0:e9c32e1df869 529 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 530 wait(0.1);
sakthipriya 0:e9c32e1df869 531
sakthipriya 0:e9c32e1df869 532 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 533 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
sakthipriya 0:e9c32e1df869 534 response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep
sakthipriya 0:e9c32e1df869 535 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 536 wait(0.1);
sakthipriya 0:e9c32e1df869 537
sakthipriya 0:e9c32e1df869 538 trFlag=1;
sakthipriya 0:e9c32e1df869 539 tr.attach(&trSub,1); //executes the function trSub afer 1sec
sakthipriya 0:e9c32e1df869 540
sakthipriya 0:e9c32e1df869 541 while(trFlag)
sakthipriya 0:e9c32e1df869 542 {
sakthipriya 0:e9c32e1df869 543 wait_ms(5); //This is required for this while loop to exit. I don't know why.
sakthipriya 0:e9c32e1df869 544 if(drFlag==1)
sakthipriya 0:e9c32e1df869 545 {
sakthipriya 0:e9c32e1df869 546 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 547 spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag
sakthipriya 0:e9c32e1df869 548 for(int i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 549 {
sakthipriya 0:e9c32e1df869 550 MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively
sakthipriya 0:e9c32e1df869 551 LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively
sakthipriya 0:e9c32e1df869 552 bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values
sakthipriya 0:e9c32e1df869 553 data[i]=(float)bit_data;
sakthipriya 0:e9c32e1df869 554 data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec
sakthipriya 0:e9c32e1df869 555 data[i]+=error[i]; //adding with error to remove offset errors
sakthipriya 0:e9c32e1df869 556 }
sakthipriya 0:e9c32e1df869 557 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 558 for (int i=0;i<3;i++)
sakthipriya 0:e9c32e1df869 559 {
sakthipriya 0:e9c32e1df869 560 printf("%f\t",data[i]); //printing the angular velocity values
sakthipriya 0:e9c32e1df869 561 }
sakthipriya 0:e9c32e1df869 562 printf("\n");
sakthipriya 0:e9c32e1df869 563 break;
sakthipriya 0:e9c32e1df869 564 }
sakthipriya 0:e9c32e1df869 565 drFlag=0;
sakthipriya 0:e9c32e1df869 566 }
sakthipriya 0:e9c32e1df869 567 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 568 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag
sakthipriya 0:e9c32e1df869 569 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e9c32e1df869 570 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 571 wait(0.1);
sakthipriya 0:e9c32e1df869 572
sakthipriya 0:e9c32e1df869 573 ssn_gyr=0;
sakthipriya 0:e9c32e1df869 574 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
sakthipriya 0:e9c32e1df869 575 response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode
sakthipriya 0:e9c32e1df869 576 ssn_gyr=1;
sakthipriya 0:e9c32e1df869 577 wait(0.1);
sakthipriya 0:e9c32e1df869 578 printf("\nExited gyro\n");
sakthipriya 0:e9c32e1df869 579 return data;
sakthipriya 0:e9c32e1df869 580 }
sakthipriya 0:e9c32e1df869 581
sakthipriya 0:e9c32e1df869 582
sakthipriya 0:e9c32e1df869 583
sakthipriya 0:e9c32e1df869 584
sakthipriya 0:e9c32e1df869 585
sakthipriya 0:e9c32e1df869 586