Diff: ACS.cpp

Revision:

0:ebdf4f859dca

Child:

2:edd107ea4740

--- /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;
+}
+
+ 
+ 
+
+
+