Team Fox
To be debugged
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
TFR_BAE_vr1_1_Debug153
by 
Bragadeesh S
ACS.cpp
- Committer:
- Bragadeesh153
- Date:
- 2016-02-03
- Revision:
- 9:f66c57a01e05
- Parent:
- 8:aad4f22221b1
File content as of revision 9:f66c57a01e05:
/*------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
#include <mbed.h>
#include <math.h>
#include "pni.h" //pni header file
#include "pin_config.h"
#include "ACS.h"
#include "EPS.h"
//********************************flags******************************************//
extern uint32_t BAE_STATUS;
extern uint32_t BAE_ENABLE;
extern char ACS_INIT_STATUS;
extern char ACS_DATA_ACQ_STATUS;
extern char ACS_ATS_STATUS;
extern char ACS_MAIN_STATUS;
extern char ACS_STATUS;
extern char ACS_ATS_ENABLE;
extern char ACS_DATA_ACQ_ENABLE;
extern char ACS_STATE;
DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
DigitalOut TRZ_EN(PIN88);
//extern PwmOut PWM1; //x //Functions used to generate PWM signal
//extern PwmOut PWM2; //y
//extern PwmOut PWM3; //z //PWM output comes from pins p6
PwmOut PWM1(PIN93); //x //Functions used to generate PWM signal
PwmOut PWM2(PIN94); //y
PwmOut PWM3(PIN95); //z //PWM output comes from pins p6
int g_err_flag_TR_x=0; // setting x-flag to zero
int g_err_flag_TR_y=0; // setting y-flag to zero
int g_err_flag_TR_z=0; // setting z-flag to zero
extern float data[6];
extern BAE_HK_actual actual_data;
//DigitalOut gpo1(PTC0); // enable of att sens2 switch
//DigitalOut gpo2(PTC16); // enable of att sens switch
Serial pc_acs(USBTX,USBRX); //for usb communication
void inverse(float mat[3][3],float inv[3][3]);
int ctrl_count = 0;
float bcopy[3];
float moment[3];
///////algo working well
void FCTN_ACS_CNTRLALGO (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode)
{
float db1[3]; // Unit: Tesla/Second
float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds
float MmntMax=1.1; // Unit: Ampere*Meter^2
float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second
float normalising_fact;
float b1_copy[3], omega1_copy[3], db1_copy[3];
int i, j;
if(flag_firsttime==1)
{
for(i=0;i<3;i++)
{
db1[i]=0; // Unit: Tesla/Second
}
flag_firsttime=0;
}
else
{
for(i=0;i<3;i++)
{
db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
}
}
if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
{
alarmmode=0;
}
else if(max_array(omega1)>OmegaMax && alarmmode==0)
{
alarmmode=1;
}
for (i=0;i<3;i++)
{
b1_copy[i]=b1[i];
db1_copy[i]=db1[i];
omega1_copy[i]=omega1[i];
}
if(alarmmode==0)
{
controlmode=0;
controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
for (i=0;i<3;i++)
{
b1[i]=b1_copy[i];
db1[i]=db1_copy[i];
omega1[i]=omega1_copy[i];
}
if(max_array(moment)>MmntMax)
{
controlmode=1;
controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
for (i=0;i<3;i++)
{
b1[i]=b1_copy[i];
db1[i]=db1_copy[i];
omega1[i]=omega1_copy[i];
}
if(max_array(moment)>MmntMax)
{
normalising_fact=max_array(moment)/MmntMax;
for(i=0;i<3;i++)
{
moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
}
}
}
}
else
{
controlmode=1;
controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
for (i=0;i<3;i++)
{
b1[i]=b1_copy[i];
db1[i]=db1_copy[i];
omega1[i]=omega1_copy[i];
}
if(max_array(moment)>MmntMax)
{
normalising_fact=max_array(moment)/MmntMax;
for(i=0;i<3;i++)
{
moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
}
}
}
for (i=0;i<3;i++)
{
b_old[i]=b1[i];
}
}
void inverse(float mat[3][3],float inv[3][3],int &singularity_flag)
{
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]);
if (det==0)
{
singularity_flag=1;
}
else
{
singularity_flag=0;
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
{
inv[i][j]/=det;
}
}
}
}
float max_array(float arr[3])
{
int i;
float temp_max=fabs(arr[0]);
for(i=1;i<3;i++)
{
if(fabs(arr[i])>temp_max)
{
temp_max=fabs(arr[i]);
}
}
return temp_max;
}
void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
{
float bb[3]={0,0,0};
float d[3]={0,0,0};
float Jm[3][3]={{0.2271,0.0014,-0.0026},{0.0014,0.2167,-0.004},{-0.0026,-0.004,0.2406}}; // Unit: Kilogram*Meter^2. Jm may change depending on the final satellite structure
float den=0,den2;
float bcopy[3];
int i, j;//temporary variables
float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//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,kdetumble=2000000;
int singularity_flag=0;
if(controlmode1==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]);
if (den==0)
{
singularity_flag=1;
}
if (singularity_flag==0)
{
for(i=0;i<3;i++)
{
db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
}
for(i=0;i<3;i++)
{
b[i]/=den; // Mormalized b. Hence no unit.
}
if(b[2]>0.9 || b[2]<-0.9)
{
kz=kz2;
kmu=kmu2;
gamma=gamma2;
}
for(i=0;i<2;i++)
{
Mu[i]=b[i];
v[i]=-kmu*Mu[i];
dv[i]=-kmu*db[i];
z[i]=db[i]-v[i];
u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
}
inverse(Jm,invJm,singularity_flag);
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]);
}
}
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
{
d[i]+=bb[j]*invJm[i][j];
}
}
bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
bb[0]=0;
for(i=0;i<3;i++)
{
d[i]=invJm[2][i];
invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
invJm[0][i]=b[i];
}
inverse(invJm,Jm,singularity_flag);
if (singularity_flag==0)
{
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
{
tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
}
}
for(i=0;i<3;i++)
{
bcopy[i]=b[i]*den;
}
for(i=0;i<3;i++)
{
Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
}
}
}
if (singularity_flag==1)
{
for (i=0;i<3;i++)
{
Mmnt[i]=2*MmntMax;
}
}
}
else if(controlmode1==1)
{
for(i=0;i<3;i++)
{
Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
}
}
for(i=0;i<3;i++)
{
moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
}
}
//void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
//{
// float db[3];
// 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,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;
//
// //................. calculating db values...........................
// if(ctrl_count!=0)
// {
// for(i=0;i<3;i++)
// db[i]= (b[i]-bcopy[i])/10;
// }
// else
// {
// for(i=0;i<3;i++)
// db[i]= 0;
// }
// ctrl_count++;
// //..................................................................
// 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]);
// }
//
// //.........................algo......................................
// 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++)
// {
// b[i]/=den;
// }
// // 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);
// 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]);
// }
// for(i=0;i<3;i++)
// {
// for(j=0;j<3;j++)
// d[i]+=bb[j]*invJm[i][j];
// }
// 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;
// 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];
// }
// inverse(invJm,Jm);
// printf("\n \r calculating tauc");
// for(i=0;i<3;i++)
// {
// for(j=0;j<3;j++)
// tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values
// printf(" %f \t",tauc[i]);
// }
// //..........................tauc to moment conversion..........................
// printf("\n \r calculating moment");
// for(i=0;i<3;i++)
// bcopy[i]=b[i]*den;
// for(i=0;i<3;i++)
// {
// moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
// moment[i]/=den;
// printf(" %f \t",moment[i]);
// }
// printf("\n\r exited control algo\n");
//}
//..........................function to find inverse..................
//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;
// }
//}
I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
void FCTN_ACS_INIT(void); //initialization of registers happens
void FCTN_ATS_DATA_ACQ(); //data is obtained
void T_OUT(); //timeout function to stop infinite loop
Timeout to; //Timeout variable to
int toFlag;
int count =0; // Time for which the BAE uC is running (in seconds)
void T_OUT()
{
toFlag=0; //as T_OUT function gets called the while loop gets terminated
}
//DEFINING VARIABLES
char cmd[2];
char raw_gyro[6];
char raw_mag[6];
char store,status;
int16_t bit_data;
float gyro_data[3], mag_data[3],combined_values[6];
float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
void FCTN_ACS_INIT()
{
TRZ_EN = 1;
ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag
//FLAG();
pc_acs.printf("Attitude sensor init called \n \r");
//FLAG();
cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
wait_ms(2000); //waiting for loading configuration file stored in EEPROM
cmd[0]=SENTRALSTATUS;
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,&store,1);
wait_ms(100);
//to check whether EEPROM is uploaded
switch((int)store) {
case(3): {
break;
}
case(11): {
break;
}
default: {
cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(2000);
}
}
pc_acs.printf("Sentral Status is %x\n \r",(int)store);
cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
cmd[1]=BIT_RUN_ENB;
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(100);
cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
cmd[1]=BIT_MAGODR;
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(100);
cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
cmd[1]=BIT_GYROODR;
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(100);
cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
cmd[1]=0x00;
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(100);
cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
cmd[1]=BIT_EVT_ENB;
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(100);
ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
}
void FCTN_ATS_DATA_ACQ()
{
ACS_DATA_ACQ_STATUS = 's'; //set ACS_DATA_ACQ_STATUS flag for att sens 2
if( ACS_ATS_ENABLE == 'e')
{
FLAG();
pc_acs.printf("attitude sensor execution called \n \r");
toFlag=1; //toFlag is set to 1 so that it enters while loop
to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated
while(toFlag) {
cmd[0]=EVT_STATUS;
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,&status,1);
wait_ms(100);
pc_acs.printf("Event Status is %x\n \r",(int)status);
//if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
if(((int)status&40)==40) {
cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
cmd[0]=MAG_XOUT_H; //LSB of x
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,raw_mag,6);
// pc_acs.printf("\nGyro Values:\n");
for(int i=0; i<3; i++) {
//concatenating gyro LSB and MSB to get 16 bit signed data values
bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
gyro_data[i]=(float)bit_data;
gyro_data[i]=gyro_data[i]/senstivity_gyro;
gyro_data[i]+=gyro_error[i];
// pc_acs.printf("%f\t",gyro_data[i]);
}
gyro_data[0] = (gyro_data[0] + 1.52) * (3.1415/180);
gyro_data[1] = (gyro_data[1] - 2.44) * (3.1415/180);
gyro_data[2] = (gyro_data[2] - 1.06) * (3.1415/180); // in radians
// pc_acs.printf("\nMag Values:\n");
for(int i=0; i<3; i++) {
//concatenating mag LSB and MSB to get 16 bit signed data values
bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
mag_data[i]=(float)bit_data;
mag_data[i]=mag_data[i]/senstivity_mag;
mag_data[i]+=mag_error[i];
// pc_acs.printf("%f\t",mag_data[i]);
}
mag_data[0] = ((mag_data[0] - 0.8089)/0.9566)/1000000;
mag_data[1] = ((mag_data[1] + 1.6722)/1.095)/1000000;
mag_data[2] = ((mag_data[2] + 4.7846 )/1.03)/1000000; // in Tesla
for(int i=0; i<3; i++) {
// data[i]=gyro_data[i];
actual_data.AngularSpeed_actual[i] = gyro_data[i];
actual_data.Bvalue_actual[i] = mag_data[i];
//data[i+3]=mag_data[i];
}
// return(combined_values); //returning poiter combined values
}
//checking for the error
else if (((int)status&2)==2) {
FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
}
}
}
else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
{
ACS_DATA_ACQ_STATUS = 'f';
}
ACS_DATA_ACQ_STATUS = 'c'; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
}
void FCTN_ACS_GENPWM_MAIN(float Moment[3])
{
printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
float l_current_x=0; //Current sent in x TR's
float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
float l_current_y=0; //Current sent in y TR's
float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
float l_current_z=0; //Current sent in z TR's
//for(int i = 0 ; i<3;i++)
{
// printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
}
//----------------------------- x-direction TR --------------------------------------------//
float l_moment_x = Moment[0]; //Moment in x direction
phase_TR_x = 1; // setting the default current direction
if (l_moment_x <0)
{
phase_TR_x = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
l_moment_x = abs(l_moment_x);
}
// l_current_x = l_moment_x * TR_CONSTANT ;
l_current_x = 0.153; //Moment and Current always have the linear relationship
pc_acs.printf("current in trx is %f \r \n",l_current_x);
if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
{
l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
}
else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
{
l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
}
else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
{
l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
}
else if(l_current_x==0)
{
printf("\n \r l_current_x====0");
l_duty_cycle_x = 0; // default value of duty cycle
pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
}
else //not necessary
{
g_err_flag_TR_x = 1;
}
//------------------------------------- y-direction TR--------------------------------------//
float l_moment_y = Moment[1]; //Moment in y direction
phase_TR_y = 1; // setting the default current direction
if (l_moment_y <0)
{
phase_TR_y = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
l_moment_y = abs(l_moment_y);
}
//l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
l_current_y = 0.153;
pc_acs.printf("current in try is %f \r \n",l_current_y);
if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
{
l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
{
l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
{
l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
else if(l_current_y==0)
{
printf("\n \r l_current_y====0");
l_duty_cycle_y = 0; // default value of duty cycle
pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
else // not necessary
{
g_err_flag_TR_y = 1;
}
//----------------------------------------------- z-direction TR -------------------------//
float l_moment_z = Moment[2]; //Moment in z direction
phase_TR_z = 1; // setting the default current direction
if (l_moment_z <0)
{
phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
l_moment_z = abs(l_moment_z);
}
// l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
l_current_z = 0.153;
pc_acs.printf("current in trz is %f \r \n",l_current_z);
if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
{
l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
{
l_duty_cycle_z = - 76880*pow(l_current_z,3) + 1280.8*pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
{
l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
else if(l_current_z==0)
{
printf("\n \r l_current_z====0");
l_duty_cycle_z = 0; // default value of duty cycle
pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
else // not necessary
{
g_err_flag_TR_z = 1;
}
//-----------------------------------------exiting the function-----------------------------------//
printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
//PWM1 = 0.95;
//PWM2 = 0.95;
//PWM3 = 0.95;
}