Diff: ACS.cpp

Revision:

8:6d856d863537

Parent:

6:1cdbda747f99

Child:

9:7936b618a879

--- a/ACS.cpp	Fri Feb 27 19:06:04 2015 +0000
+++ b/ACS.cpp	Wed Mar 04 18:14:01 2015 +0000
@@ -1,6 +1,7 @@
 #include "ACS.h"
 #include "MPU3300.h"
 #include "pin_config.h"
+#include "math.h"
 
 //PwmOut PWM1(PTD4);        //Functions used to generate PWM signal 
                         //PWM output comes from pins p6
@@ -18,7 +19,9 @@
 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])
@@ -26,6 +29,10 @@
      
 
      printf("\n\rEnterd PWMGEN function\n\r\r");
+     for(int i = 0 ; i<3;i++)
+        {
+            printf(" %f \t ",M[i]);
+        }
      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 ;  
@@ -35,7 +42,7 @@
       
      
         ix = Mx * 0.3 ;      //Moment and Current always have the linear relationship
-     
+       // ix = 0.554999;
         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;
@@ -62,8 +69,10 @@
             PWM1.period(timep);
             PWM1 = DCx/100 ;            
          }
-         else if(ix==0)
+         //printf("\n \r b4 %f ",ix);
+          else if(ix==0)
          {
+             printf("\n \r ix====0");
              DCx = 75;
             PWM1.period(timep);
             PWM1 = DCx/100 ;            
@@ -72,8 +81,10 @@
          {
             // printf("!!!!!!!!!!Error!!!!!!!!!");
          } 
-    //DCx = 25;     
-    printf("\n\r icx :%f\n\r",ix);
+    pwm1 = PWM1;
+    //DCx = 25; 
+    //PWM1 = 0.50;    
+    printf("\n\r icx :%f pwm : %f \n\r",ix,pwm1);
     float DCy = 0;         //Duty cycle of Moment in x, y, z directions
      float iy = 0;          //Current sent in x, y, z TR's
        
@@ -119,8 +130,13 @@
          {
             // printf("!!!!!!!!!!Error!!!!!!!!!");
          } 
+    
+    pwm2 = PWM2; 
+    printf("\n\r icy :%f pwm : %f \n\r",iy,pwm2);    
+         
+         
     float DCz = 0;         //Duty cycle of Moment in x, y, z directions
-     float iz = 0;          //Current sent in x, y, z TR's
+    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
@@ -163,7 +179,9 @@
          else
          {
             // printf("!!!!!!!!!!Error!!!!!!!!!");
-         }    
+         }   
+    pwm3 = PWM3; 
+    printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3); 
     
 printf("\n\rExited PWMGEN function\n\r");
 }
@@ -269,12 +287,23 @@
  }
  
 } 
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------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) ;
+  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------------------------------------------------------------------------------------------*/
 
-float * FUNC_ACS_CNTRLALGO(float b[3],float omega[3])
+void FUNC_ACS_CNTRLALGO(float b[3],float omega[3],float tauc[3])
 {
     float db[3]; /// inputs
 //initialization
@@ -284,10 +313,12 @@
     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 Mu[2], z[2], dv[2], v[2], u[2]; //outputs
+   // float tauc[3];
+    //float *tauc1;
     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\r");
+    printf("\n\r Entered cntrl algo\n\r");
     //structure parameters
 
     void inverse (float mat[3][3], float inv[3][3]); 
@@ -295,8 +326,8 @@
     //functions
  
 ////////// Input from Matlab //////////////
-    while(1) 
-    {
+  //  while(1)                          //removed assumin while is used coz of matlab
+    //{
        
  /*getInput(inputs);
 //while(1)
@@ -311,6 +342,8 @@
  omega[2] = inputs[8];*/
 /////////// Control Algorithm //////////////////////
 // calculate norm b, norm db
+        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]);
        
@@ -324,8 +357,8 @@
         {
             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 
-                ;
+                b[i]=b[i]/den;                                      //there is a problem here. The code gets stuck here.  Maf value is required 
+                
         }
         
 // select kz, kmu, gamma
@@ -346,10 +379,11 @@
         }
         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++)
         {
@@ -375,14 +409,20 @@
 // 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(" %d \t",i);
+            //tauc1[i] = tauc[i];
+            printf(" %f \t",tauc[i]);    
         }
-         
-        return(tauc);
-    }
+        
+        //printf("    %f \n ", tauc[2]);
+        //return tauc;
+    
 }
 /////////// Output to Matlab //////////////////
 /* for(i=0;i<3;i++) {