GOPA KUMAR K C
/
Contro_Algo
Control_Algo
Revision 0:019699da1769, committed 2015-02-13
- Comitter:
- gkumar
- Date:
- Fri Feb 13 17:08:51 2015 +0000
- Commit message:
- Control_Algo
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 019699da1769 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Feb 13 17:08:51 2015 +0000 @@ -0,0 +1,249 @@ +#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; +} +}
diff -r 000000000000 -r 019699da1769 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Feb 13 17:08:51 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e188a91d3eaa \ No newline at end of file