![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
acs bea hk together
Fork of BAE_vr3honeycomb1_christmas by
ACS.cpp
- Committer:
- greenroshks
- Date:
- 2014-12-26
- Revision:
- 2:edd107ea4740
- Parent:
- 0:ebdf4f859dca
File content as of revision 2:edd107ea4740:
#include "ACS.h" #include "MPU3300.h" #include "pin_config.h" //PwmOut PWM1(PTD4); //Functions used to generate PWM signal //PWM output comes from pins p6 Serial pc1(USBTX, USBRX); SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk PTE18,19,17 DigitalOut SSN_MAG (PTC16); // ssn for magnetometer PTB11 DigitalInOut DRDY (PTE3); // drdy for magnetometer PTA17 DigitalOut ssn_gyr (PTE2); //Slave Select pin of gyroscope PTB16 InterruptIn dr(PTC6); //Interrupt pin for gyro PTC5 PwmOut PWM1(D2); //Functions used to generate PWM signal PwmOut PWM2(D3); PwmOut PWM3(D4); //PWM output comes from pins p6 Ticker tr; //Ticker function to give values for limited amount of time for gyro Timeout tr_mag; uint8_t trflag_mag; uint8_t trFlag; //ticker Flag for gyro uint8_t drFlag; //data-ready interrupt flag for gyro //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------// void FUNC_ACS_GENPWM(float M[3]) { printf("\nEnterd PWMGEN function\n"); float DCx = 0; //Duty cycle of Moment in x, y, z directions float ix = 0; //Current sent in x, y, z TR's float timep = 0.02 ; float Mx=M[0]; //Time period is set to 0.02s //Moment in x, y, z directions ix = Mx * 0.3 ; //Moment and Current always have the linear relationship if( ix>0&& ix < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% { DCx = 6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008; PWM1.period(timep); PWM1 = DCx/100 ; } else if( ix >= 0.006&& ix < 0.0116) { DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648; PWM1.period(timep); PWM1 = DCx/100 ; } else if (ix >= 0.0116&& ix < 0.0624) { DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878; PWM1.period(timep); PWM1 = DCx/100 ; } else if(ix >= 0.0624&& ix < 0.555) { printf("\nACS entered if\n"); DCx = 331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338; PWM1.period(timep); PWM1 = DCx/100 ; } else if(ix==0) { DCx = 50; PWM1.period(timep); PWM1 = DCx/100 ; } else { // printf("!!!!!!!!!!Error!!!!!!!!!"); } printf("\n moment :%f\n",DCx); float DCy = 0; //Duty cycle of Moment in x, y, z directions float iy = 0; //Current sent in x, y, z TR's float My=M[1]; //Time period is set to 0.2s //Moment in x, y, z directions iy = My * 0.3 ; //Moment and Current always have the linear relationship if( iy>0&& iy < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% { DCy = 6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008; PWM2.period(timep); PWM2 = DCy/100 ; } else if( iy >= 0.006&& iy < 0.0116) { DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648; PWM2.period(timep); PWM2 = DCy/100 ; } else if (iy >= 0.0116&& iy < 0.0624) { DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878; PWM2.period(timep); PWM2 = DCy/100 ; } else if(iy >= 0.0624&& iy < 0.555) { printf("\nACS entered if\n"); DCy = 331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338; PWM2.period(timep); PWM2 = DCy/100 ; } else if(iy==0) { DCy = 0; PWM2.period(timep); PWM2 = DCy/100 ; } else { // printf("!!!!!!!!!!Error!!!!!!!!!"); } float DCz = 0; //Duty cycle of Moment in x, y, z directions float iz = 0; //Current sent in x, y, z TR's float Mz=M[2]; //Time period is set to 0.2s //Moment in x, y, z directions iz = Mz * 0.3 ; //Moment and Current always have the linear relationship if( iz>0&& iz < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% { DCz = 6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008; PWM3.period(timep); PWM3 = DCz/100 ; } else if( iz >= 0.006&& iz < 0.0116) { DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648; PWM3.period(timep); PWM3 = DCz/100 ; } else if (iz >= 0.0116&& iz < 0.0624) { DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878; PWM3.period(timep); PWM3 = DCz/100 ; } else if(iz >= 0.0624&& iz < 0.555) { printf("\nACS entered if\n"); DCz = 331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338; PWM3.period(timep); PWM3 = DCz/100 ; } else if(iz==0) { DCz = 0; PWM3.period(timep); PWM3 = DCz/100 ; } else { // printf("!!!!!!!!!!Error!!!!!!!!!"); } printf("\nExited PWMGEN function\n"); } /*------------------------------------------------------------------------------------------------------------------------------------------------------- -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/ void trsub_mag() { trflag_mag=0; } void FUNC_ACS_MAG_INIT() { //DRDY.output(); DRDY = 0; int a ; a=DRDY; printf("\n DRDY is %d\n",a); SSN_MAG=1; //pin is disabled spi_acs.format(8,0); // 8bits,Mode 0 spi_acs.frequency(100000); //clock frequency SSN_MAG=0; // Selecting pin wait_ms(10); //accounts for delay.can be minimised. spi_acs.write(0x83); // wait_ms(10); unsigned char i; for(i=0;i<3;i++)//initialising values. { spi_acs.write(0x00); //MSB of X,y,Z spi_acs.write(0xc8); //LSB of X,Y,z;pointer increases automatically. } SSN_MAG=1; } float* FUNC_ACS_MAG_EXEC() { //printf("\nEntered magnetometer function\n"); //DRDY.output(); DRDY.write(0); int a; a = DRDY; printf("\n DRDY is %d\n",a); SSN_MAG=0; //enabling slave to measure the values wait_ms(10); spi_acs.write(0x82); //initiates measurement wait_ms(10); spi_acs.write(0x01); //selecting x,y and z axes, measurement starts now SSN_MAG=1; wait_ms(10); trflag_mag=1; tr_mag.attach(&trsub_mag,1); //runs in background,makes trflag_mag=0 after 1s DRDY.input(); while(trflag_mag) /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and programme is terminated*/ { wait_ms(5); if(DRDY==1) { printf("\nwth\n"); SSN_MAG=0; spi_acs.write(0xc9); //command byte for retrieving data unsigned char axis; float Bnewvalue[3]={0.0,0.0,0.0}; int32_t Bvalue[3]={0,0,0}; int32_t a= pow(2.0,24.0); int32_t b= pow(2.0,23.0); for(axis=0;axis<3;axis++) { Bvalue[axis]=spi_acs.write(0x00)<<16; //MSB 1 is send first wait_ms(10); Bvalue[axis]|=spi_acs.write(0x00)<<8; //MSB 2 is send next wait_ms(10); Bvalue[axis]|=spi_acs.write(0x00); //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration if((Bvalue[axis]&b)==b) { Bvalue[axis]=Bvalue[axis]-a; //converting 2s complement to signed decimal } Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0); //1 LSB=(22nT)...final value of field obtained in micro tesla wait_ms(10); printf("\t%lf\n",Bnewvalue[axis]); } SSN_MAG=1; /* for test only to removed */ Bnewvalue[0]=Bnewvalue[1]=Bnewvalue[2]=100; return Bnewvalue; //return here? doubt.. break; } } } /*------------------------------------------------------------------------------------------------------------------------------------------------------ -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ float * FUNC_ACS_CNTRLALGO(float b[3],float omega[3]) { float db[3]; /// inputs //initialization float bb[3] = {0, 0, 0}; float d[3] = {0, 0, 0}; float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}}; float den = 0; float den2; int i, j; //temporary variables float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs float invJm[3][3]; float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4; printf("Entered cntrl algo\n"); //structure parameters void inverse (float mat[3][3], float inv[3][3]); void getInput (float x[9]); //functions ////////// Input from Matlab ////////////// while(1) { /*getInput(inputs); //while(1) b[0] = inputs[0]; b[1] = inputs[1]; b[2] = inputs[2]; db[0] = inputs[3]; db[1] = inputs[4]; db[2] = inputs[5]; omega[0] = inputs[6]; omega[1] = inputs[7]; omega[2] = inputs[8];*/ /////////// Control Algorithm ////////////////////// // calculate norm b, norm db den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2])); den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]); for(i=0;i<3;i++) { db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3)); //db[i]/=den*den*den; } for(i=0;i<3;i++) { printf("\nreached here\n"); if(den!=0) //b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required ; } // select kz, kmu, gamma if(b[0]>0.9 || b[0]<-0.9) { kz = kz2; kmu = kmu2; gamma = gamma2; } // calculate Mu, v, dv, z, u for(i=0;i<2;i++) { Mu[i] = b[i+1]; v[i] = -kmu*Mu[i]; dv[i] = -kmu*db[i+1]; z[i] = db[i+1] - v[i]; u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma); } inverse(Jm, invJm); // calculate cross(omega,J*omega)for(i=0;i<3;i++) for(j=0;j<3;j++) bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]); // calculate invJm*cross(omega,J*omega) store in d for(i=0;i<3;i++) { for(j=0;j<3;j++) d[i] += bb[j]*invJm[i][j]; } // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db) // bb =[0;u-d(2:3)] // store in bb bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]); bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]); bb[0] = 0; // calculate N // reusing invJm as N for(i=0;i<3;i++) { d[i] = invJm[1][i]; invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i]; invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i]; invJm[0][i] = b[i]; } // calculate inv(N) store in Jm inverse(invJm, Jm); // calculate tauc for(i=0;i<3;i++) { for(j=0;j<3;j++) tauc[i] += Jm[i][j]*bb[j]; } return(tauc); } } /////////// Output to Matlab ////////////////// /* for(i=0;i<3;i++) { printf("%f\n",tauc[i]*10000000); wait_ms(10); } } }*/ void inverse(float mat[3][3], float inv[3][3]) { int i, j; float det = 0; for(i=0;i<3;i++) { for(j=0;j<3;j++) inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3] [(j+1)%3]*mat[(i+1)%3][(j+2)%3]); } det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]); for(i=0;i<3;i++) { for(j=0;j<3;j++) inv[i][j] /= det; } }/* void getInput (float x[9]) { //Functions used to generate PWM signal //PWM output comes from pins p6 Serial pc1(USBTX, USBRX); char c[10]; char tempchar[8]; int i, j; //float f[9]; long n = 0; float flval = 0; for(j=0;j<9;j++) { for(i=0;i<9;i++) { c[i] = pc1.getc(); if(i<8) { tempchar[i] = c[i]; } } sscanf (tempchar, "%8x", &n); memcpy(&flval, &n, sizeof(long)); printf("%f\n", flval); x[j] = flval; } }*/ void trSub(); void drSub(); void init_gyro(); float * FUNC_ACS_EXEC_GYR(); void drSub() //In this function we setting data-ready flag to 1 { drFlag=1; } void trSub() //In this function we are setting ticker flag to 0 { trFlag=0; } void FUNC_ACS_INIT_GYR() { uint8_t response; ssn_gyr=1; //Deselecting the chip spi_acs.format(8,0); // Spi format is 8 bits, and clock mode 3 spi_acs.frequency(1000000); //frequency to be set as 1MHz drFlag=0; //Intially defining data-ready flag to be 0 dr.mode(PullDown); dr.rise(&drSub); __disable_irq(); /*Following the above mentioned algorithm for initializing the register and changing its configuration*/ ssn_gyr=0; //Selecting chip(Mpu-3300) spi_acs.write(USER_CTRL|READFLAG); //sending USER_CTRL address with read bit response=spi_acs.write(DUMMYBIT); //sending dummy bit to get default values of the register ssn_gyr=1; //Deselecting the chip wait(0.1); //waiting according the product specifications ssn_gyr=0; //again selecting the chip spi_acs.write(USER_CTRL); //sending USER_CTRL address without read bit spi_acs.write(response|BIT_I2C_IF_DIS); //disabling the I2C mode in the register ssn_gyr=1; //deselecting the chip wait(0.1); // waiting for 100ms before going for another register ssn_gyr=0; spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1 response=spi_acs.write(DUMMYBIT); ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(PWR_MGMT_1); response=spi_acs.write(response|BIT_CLKSEL_X); //Selecting the X axis gyroscope as clock as mentioned above ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit response=spi_acs.write(DUMMYBIT); ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit response=spi_acs.write(DUMMYBIT); ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(CONFIG); //sending CONFIG address to write to register spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit response=spi_acs.write(DUMMYBIT); ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(INT_ENABLE|READFLAG); //sending address of INT_ENABLE with readflag response=spi_acs.write(DUMMYBIT); //sending dummy byte to get the default values of the // regiser ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(INT_ENABLE); //sending INT_ENABLE address to write to register spi_acs.write(response|BIT_DATA_RDY_ENABLE); //Enbling data ready interrupt ssn_gyr=1; wait(0.1); __enable_irq(); } float * FUNC_ACS_EXEC_GYR() { printf("\nEntered gyro\n"); uint8_t response; uint8_t MSB,LSB; int16_t bit_data; float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required float senstivity = 145.6; //senstivity is 145.6 for full scale mode of +/-225 deg/sec ssn_gyr=0; spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag response=spi_acs.write(DUMMYBIT); // ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep ssn_gyr=1; wait(0.1); trFlag=1; tr.attach(&trSub,1); //executes the function trSub afer 1sec while(trFlag) { wait_ms(5); //This is required for this while loop to exit. I don't know why. if(drFlag==1) { ssn_gyr=0; spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag for(int i=0;i<3;i++) { MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values data[i]=(float)bit_data; data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec data[i]+=error[i]; //adding with error to remove offset errors } ssn_gyr=1; for (int i=0;i<3;i++) { printf("%f\t",data[i]); //printing the angular velocity values } printf("\n"); break; } drFlag=0; } ssn_gyr=0; spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag response=spi_acs.write(DUMMYBIT); ssn_gyr=1; wait(0.1); ssn_gyr=0; spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode ssn_gyr=1; wait(0.1); printf("\nExited gyro\n"); return data; }