File content as of revision 0:019699da1769:

#include "mbed.h"

#include "math.h"

Serial pc(USBTX, USBRX);

 

/////////// Variable declarations ////////////////////

float b[3], db[3], omega[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, 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;

//structure parameters

float inputs[9];

void inverse (float mat[3][3], float inv[3][3]); 

void getInput (float x[9]);

 //functions

int main() {

////////// 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++)

{

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);

// 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];

}

/////////// Output to Matlab //////////////////

 for(i=0;i<3;i++) {

 printf("%f\n",tauc[i]*10000000);

 wait_ms(10);

 }

 }

 return 0;

}

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]) {
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] = pc.getc();
if(i<8) {
tempchar[i] = c[i];
}
}
sscanf (tempchar, "%8x", &n);
memcpy(&flval, &n, sizeof(long));
printf("%f\n", flval);
x[j] = flval;
}
}