![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
integrated code with threads to be removed
Fork of BAE_vr3honeycomb1 by
Diff: ACS.cpp
- Revision:
- 0:ebdf4f859dca
- Child:
- 2:edd107ea4740
diff -r 000000000000 -r ebdf4f859dca ACS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ACS.cpp Fri Dec 19 06:47:25 2014 +0000 @@ -0,0 +1,587 @@ +#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 (PIN61); // ssn for magnetometer PTB11 +DigitalIn DRDY (PIN47); // drdy for magnetometer PTA17 +DigitalOut ssn_gyr (PIN62); //Slave Select pin of gyroscope PTB16 +InterruptIn dr(PIN81); //Interrupt pin for gyro PTC5 +PwmOut PWM1(A3); //Functions used to generate PWM signal +PwmOut PWM2(A4); +PwmOut PWM3(A5); //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 = 0; + PWM1.period(timep); + PWM1 = DCx/100 ; + } + else + { + // printf("!!!!!!!!!!Error!!!!!!!!!"); + } + 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() + { + + 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"); + 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 + + 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) + { + 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; + + 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,3); // 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; +} + + + + + +