Control_Algo

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 
00003 #include "math.h"
00004 
00005 Serial pc(USBTX, USBRX);
00006 
00007  
00008 
00009 /////////// Variable declarations ////////////////////
00010 
00011 float b[3], db[3], omega[3]; /// inputs
00012 
00013 //initialization
00014 
00015 float bb[3] = {0, 0, 0};
00016 
00017 float d[3] = {0, 0, 0};
00018 
00019 float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}};
00020 
00021 float den = 0, den2;
00022 
00023 int i, j; //temporary variables
00024 
00025 float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs
00026 
00027 float invJm[3][3];
00028 
00029 float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4;
00030 
00031 //structure parameters
00032 
00033 float inputs[9];
00034 
00035 void inverse (float mat[3][3], float inv[3][3]); 
00036 
00037 void getInput (float x[9]);
00038 
00039  //functions
00040 
00041 int main() {
00042 
00043 ////////// Input from Matlab //////////////
00044 
00045 while(1) {
00046 
00047  getInput(inputs);
00048 
00049 //while(1)
00050 
00051  b[0] = inputs[0];
00052 
00053  b[1] = inputs[1];
00054 
00055  b[2] = inputs[2];
00056 
00057  db[0] = inputs[3];
00058 
00059  db[1] = inputs[4];
00060 
00061  db[2] = inputs[5];
00062 
00063  omega[0] = inputs[6];
00064 
00065  omega[1] = inputs[7];
00066 
00067  omega[2] = inputs[8];
00068 
00069 /////////// Control Algorithm //////////////////////
00070 
00071 // calculate norm b, norm db
00072 
00073 den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2]));
00074 
00075 den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]);
00076 
00077 for(i=0;i<3;i++)
00078 
00079 {
00080 
00081 db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3));
00082 
00083 //db[i]/=den*den*den;
00084 
00085 }
00086 
00087 for(i=0;i<3;i++)
00088 
00089 {
00090 
00091 b[i] /= den;
00092 
00093 }
00094 
00095 // select kz, kmu, gamma
00096 
00097 if(b[0]>0.9 || b[0]<-0.9)
00098 
00099 {
00100 
00101 kz = kz2;
00102 
00103 kmu = kmu2;
00104 
00105 gamma = gamma2;
00106 
00107 }
00108 
00109 // calculate Mu, v, dv, z, u
00110 
00111 for(i=0;i<2;i++)
00112 
00113 {
00114 
00115 Mu[i] = b[i+1];
00116 
00117 v[i] = -kmu*Mu[i];
00118 
00119 dv[i] = -kmu*db[i+1];
00120 
00121 z[i] = db[i+1] - v[i];
00122 
00123 u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma);
00124 
00125 }
00126 
00127 inverse(Jm, invJm);
00128 
00129 // calculate cross(omega,J*omega)
00130 
00131 for(i=0;i<3;i++)
00132 
00133 {
00134 
00135 for(j=0;j<3;j++)
00136 
00137 bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - 
00138 
00139 omega[(i+2)%3]*Jm[(i+1)%3][j]);
00140 
00141 }
00142 
00143 // calculate invJm*cross(omega,J*omega) store in d
00144 
00145 for(i=0;i<3;i++)
00146 
00147 {
00148 
00149 for(j=0;j<3;j++)
00150 
00151 d[i] += bb[j]*invJm[i][j];
00152 
00153 }
00154 
00155 // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db) 
00156 
00157 // bb =[0;u-d(2:3)] 
00158 
00159 // store in bb
00160 
00161 bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]);
00162 bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]);
00163 bb[0] = 0;
00164 
00165 // calculate N 
00166 
00167 // reusing invJm as N
00168 
00169 for(i=0;i<3;i++)
00170 
00171 {
00172 
00173 d[i] = invJm[1][i];
00174 
00175 invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i];
00176 
00177 invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i];
00178 
00179 invJm[0][i] = b[i];
00180 
00181 }
00182 
00183 // calculate inv(N) store in Jm
00184 
00185 inverse(invJm, Jm);
00186 
00187 // calculate tauc
00188 
00189 for(i=0;i<3;i++)
00190 
00191 {
00192 
00193 for(j=0;j<3;j++)
00194 
00195 tauc[i] += Jm[i][j]*bb[j];
00196 
00197 }
00198 
00199 /////////// Output to Matlab //////////////////
00200 
00201  for(i=0;i<3;i++) {
00202 
00203  printf("%f\n",tauc[i]*10000000);
00204 
00205  wait_ms(10);
00206 
00207  }
00208 
00209  }
00210 
00211  return 0;
00212 
00213 }
00214 
00215 void inverse(float mat[3][3], float inv[3][3])
00216 {
00217 int i, j;
00218 float det = 0;
00219 for(i=0;i<3;i++)
00220 { for(j=0;j<3;j++)
00221 inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3]
00222 [(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
00223 }
00224 det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]);
00225 for(i=0;i<3;i++)
00226 { for(j=0;j<3;j++)
00227 inv[i][j] /= det;
00228 }
00229 }
00230 void getInput (float x[9]) {
00231 char c[10];
00232 char tempchar[8];
00233 int i, j;
00234 //float f[9];
00235 long n = 0;
00236 float flval = 0;
00237 for(j=0;j<9;j++) {
00238 for(i=0;i<9;i++) {
00239 c[i] = pc.getc();
00240 if(i<8) {
00241 tempchar[i] = c[i];
00242 }
00243 }
00244 sscanf (tempchar, "%8x", &n);
00245 memcpy(&flval, &n, sizeof(long));
00246 printf("%f\n", flval);
00247 x[j] = flval;
00248 }
00249 }