Control_Algo
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Mon Aug 15 2022 00:47:31 by
1.7.2