Seeker of Truth ,
i2c testing for integration
Fork of
pcb_test_v1_1_1
by 
sakthi priya amirtharaj
Diff: ACS.cpp
- Revision:
- 0:e91ee0e99213
- Child:
- 1:bbddd1763652
diff -r 000000000000 -r e91ee0e99213 ACS.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ACS.cpp Tue Apr 07 16:11:54 2015 +0000
@@ -0,0 +1,566 @@
+#include "ACS.h"
+#include "MPU3300.h"
+#include "pin_config.h"
+#include "math.h"
+
+Serial pc1(USBTX, USBRX);
+SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk PTE18,19,17
+DigitalOut SSN_MAG (PIN61); // ssn for magnetometer PTB11
+DigitalInOut 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(PIN93); //Functions used to generate PWM signal
+PwmOut PWM2(PIN94);
+PwmOut PWM3(PIN88); //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
+float pwm1;
+float pwm2;
+float pwm3;
+//--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//
+
+void FUNC_ACS_GENPWM(float M[3])
+{
+
+ printf("\n\rEnterd PWMGEN function\n");
+ for(int i = 0 ; i<3;i++)
+ {
+ printf(" %f \t ",M[i]);
+ }
+ float timep = 0.02 ; //Time period is set to 0.02s
+
+
+ float DCx = 0; //Duty cycle of Moment in x direction
+ float ix = 0; //Current sent in x TR's
+ float Mx=M[0]; //Moment in x direction
+ 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("\n\rACS entered if\n\r"); //gotta check what this means
+ 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)
+ {
+ printf("\n \r ix====0");
+ DCx = 75;
+ PWM1.period(timep);
+ PWM1 = DCx/100 ;
+ }
+ else //not necessary
+ {
+ // printf("!!!!!!!!!!Error!!!!!!!!!");
+ }
+ pwm1 = PWM1;
+ printf("\n\r icx :%f pwm : %f \n\r",ix,pwm1);
+
+
+ float DCy = 0; //Duty cycle of Moment in y direction
+ float iy = 0; //Current sent in y TR's
+ float My=M[1]; //Moment in y direction
+ 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("\n\rACS entered if\n\r");
+ 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!!!!!!!!!");
+ }
+ pwm2 = PWM2;
+ printf("\n\r icy :%f pwm : %f \n\r",iy,pwm2);
+
+
+ float DCz = 0; //Duty cycle of Moment in z direction
+ float iz = 0; //Current sent in z TR's
+ float Mz=M[2]; //Moment in z direction
+ 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("\n\rACS entered if\n\r");
+ 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!!!!!!!!!");
+ }
+ pwm3 = PWM3;
+ printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3);
+
+ printf("\n\rExited PWMGEN function\n\r");
+}
+/*-------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/
+
+void trsub_mag()
+{
+ trflag_mag=0;
+}
+
+void FUNC_ACS_MAG_INIT()
+{
+ DRDY = 0;
+ int a ;
+ a=DRDY;
+ printf("\n\r DRDY is %d\n\r",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;
+}
+
+void FUNC_ACS_MAG_EXEC(float mag_field[])
+{
+ printf("\n\rEntered magnetometer function\n\r");
+ DRDY.write(0);
+ int a;
+ a = DRDY;
+ printf("\n\r DRDY is %d\n\r",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("\n\r DRDY is high\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);
+
+ mag_field[axis] = Bnewvalue[axis];
+ printf("\t%lf\n\r",mag_field[axis]);
+ }
+ SSN_MAG=1;
+ printf("\n\r exited magnetometer function\n");
+ //return Bnewvalue; //return here? doubt..
+
+ break;
+ }
+ }
+}
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------torque to moment conversion------------------------------------------------------------------------------------------*/
+void moment_calc (float tauc[3], float b[3], float moment[3])
+{
+ float b1;
+ b1 = pow(b[0],2) + pow(b[1],2) +pow(b[2],2) ;
+ printf("\nvalue of b is %f\n",b1);
+ moment[0] = ((tauc[1]*b[2])-(tauc[2]*b[1]))/b1;
+ moment[1] = ((tauc[2]*b[0])-(tauc[0]*b[2]))/b1;
+ moment[2] = ((tauc[0]*b[1])-(tauc[1]*b[0]))/b1;
+
+}
+
+
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
+
+void FUNC_ACS_CNTRLALGO(float *b,float *omega,float tauc[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]; //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("\n\r Entered cntrl algo\n\r");
+ for(int i=0; i<3; i++)
+ {
+ printf("%f\t",omega[i]);
+ }
+ for(int i=0; i<3; i++)
+ {
+ printf("%f\t",b[i]);
+ }
+ //structure parameters
+ void inverse (float mat[3][3], float inv[3][3]);
+ void getInput (float x[9]);
+ tauc[0] =tauc[1] =tauc[2]=0 ;
+
+ 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++) //gotta check what is this
+ {
+ printf("\n\rreached here\n\r");
+ 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(i=0; i<3; i++) // for loop included after checking original code
+ {
+ 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
+ printf("\n \r calculatin tauc");
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ tauc[i] += Jm[i][j]*bb[j];
+ printf(" %f \t",tauc[i]);
+ }
+}
+
+
+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;
+}
+}
+
+
+/*-------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------GYROSCOPE-------------------------------------------------------------------------------------------------*/
+
+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;
+ //printf("\n dr interrupt detected");
+}
+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();
+
+ /*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();
+}
+
+void FUNC_ACS_EXEC_GYR(float*omega)
+{
+ printf("\n\rEntered gyro\n\r");
+ 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)
+ {
+ // printf("\n\r check1");
+ wait_ms(5); //This is required for this while loop to exit. I don't know why.
+ if(drFlag==1)
+ {
+ //printf("\n\r check2");
+ 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
+ omega[i] = data[i];
+ }
+ printf("\n\r");
+ 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("\n\rExited gyro\n\r");
+
+}
+
+
+
+
+
+