Diff: ACS.cpp

Revision:

11:6b00f3b1be17

Parent:

10:8bfa80bf4263

Child:

12:da058cfe9753

--- a/ACS.cpp	Sun Mar 01 08:26:04 2015 +0000
+++ b/ACS.cpp	Wed Mar 04 17:15:15 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 pwm;
+float pwm1;
+float pwm2;
+float pwm3;
 //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//
 
 void FUNC_ACS_GENPWM(float M[3])
@@ -39,7 +42,7 @@
       
      
         ix = Mx * 0.3 ;      //Moment and Current always have the linear relationship
-        ix = 0.554999;
+       // 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;
@@ -78,10 +81,10 @@
          {
             // printf("!!!!!!!!!!Error!!!!!!!!!");
          } 
-    pwm = PWM1;
+    pwm1 = PWM1;
     //DCx = 25; 
     //PWM1 = 0.50;    
-    printf("\n\r icx :%f pwm : %f \n\r",ix,pwm);
+    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
        
@@ -128,8 +131,8 @@
             // printf("!!!!!!!!!!Error!!!!!!!!!");
          } 
     
-    pwm = PWM2; 
-    printf("\n\r icy :%f pwm : %f \n\r",iy,pwm);    
+    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
@@ -177,6 +180,8 @@
          {
             // printf("!!!!!!!!!!Error!!!!!!!!!");
          }   
+    pwm3 = PWM3; 
+    printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3); 
     
 printf("\n\rExited PWMGEN function\n\r");
 }
@@ -282,6 +287,17 @@
  }
  
 } 
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------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;
+  
+}
 
 
 /*------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -298,8 +314,8 @@
     float den2;
     int i, j; //temporary variables
     float Mu[2], z[2], dv[2], v[2], u[2]; //outputs
-    //float tauc[3];
-    float *tauc1;
+   // 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("\n\r Entered cntrl algo\n\r");
@@ -326,9 +342,7 @@
  omega[2] = inputs[8];*/
 /////////// Control Algorithm //////////////////////
 // calculate norm b, norm db
-        tauc[0]=0;
-        tauc[1]=0;
-        tauc[2]=0;
+        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]);
@@ -365,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++)
         {