Control_Algo

Dependencies:   mbed

Files at this revision

API Documentation at this revision

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