gotta check ctrl algo - it gives negative value

Dependencies:   mbed-rtos mbed

Fork of BAE_hw_test1_3 by sakthi priya amirtharaj

Files at this revision

API Documentation at this revision

Comitter:
sakthipriya
Date:
Wed Mar 04 17:15:15 2015 +0000
Parent:
10:8bfa80bf4263
Commit message:
control algo - torque converted to moment

Changed in this revision

ACS.cpp Show annotated file Show diff for this revision Revisions of this file
ACS.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 8bfa80bf4263 -r 6b00f3b1be17 ACS.cpp
--- 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++)
         {
diff -r 8bfa80bf4263 -r 6b00f3b1be17 ACS.h
--- a/ACS.h	Sun Mar 01 08:26:04 2015 +0000
+++ b/ACS.h	Wed Mar 04 17:15:15 2015 +0000
@@ -5,6 +5,7 @@
 float *  FUNC_ACS_MAG_EXEC(void);
 void FUNC_ACS_MAG_INIT();
 //void  Read_data_acs()
+void moment_calc (float* , float* , float* );
 void  FUNC_ACS_CNTRLALGO(float*,float*,float a[]);
 float * FUNC_ACS_EXEC_GYR();
 void FUNC_ACS_INIT_GYR();
diff -r 8bfa80bf4263 -r 6b00f3b1be17 main.cpp
--- a/main.cpp	Sun Mar 01 08:26:04 2015 +0000
+++ b/main.cpp	Wed Mar 04 17:15:15 2015 +0000
@@ -98,6 +98,7 @@
     float *omega;
     float mag_field1[3];
     float omega1[1];
+    float tauc1[3];
     float moment[3];
     float *mnm_data;
     while(1)
@@ -128,12 +129,19 @@
         }
         if(acs_pflag == 1)
         {
-        FUNC_ACS_CNTRLALGO(mag_field1,omega1,moment);
+        FUNC_ACS_CNTRLALGO(mag_field1,omega1,tauc1);
         printf("\n\r control algo values ");
         for(int i=0; i<3; i++) 
         {
+        printf("%f\t",tauc1[i]);
+        }
+        moment_calc (tauc1, mag_field1,moment);
+        printf("\n\r moment values ");
+        for(int i=0; i<3; i++) 
+        {
         printf("%f\t",moment[i]);
         }
+        
         FUNC_ACS_GENPWM(moment);                     
         }