![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Control_Algo
main.cpp@0:019699da1769, 2015-02-13 (annotated)
- Committer:
- gkumar
- Date:
- Fri Feb 13 17:08:51 2015 +0000
- Revision:
- 0:019699da1769
Control_Algo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gkumar | 0:019699da1769 | 1 | #include "mbed.h" |
gkumar | 0:019699da1769 | 2 | |
gkumar | 0:019699da1769 | 3 | #include "math.h" |
gkumar | 0:019699da1769 | 4 | |
gkumar | 0:019699da1769 | 5 | Serial pc(USBTX, USBRX); |
gkumar | 0:019699da1769 | 6 | |
gkumar | 0:019699da1769 | 7 | |
gkumar | 0:019699da1769 | 8 | |
gkumar | 0:019699da1769 | 9 | /////////// Variable declarations //////////////////// |
gkumar | 0:019699da1769 | 10 | |
gkumar | 0:019699da1769 | 11 | float b[3], db[3], omega[3]; /// inputs |
gkumar | 0:019699da1769 | 12 | |
gkumar | 0:019699da1769 | 13 | //initialization |
gkumar | 0:019699da1769 | 14 | |
gkumar | 0:019699da1769 | 15 | float bb[3] = {0, 0, 0}; |
gkumar | 0:019699da1769 | 16 | |
gkumar | 0:019699da1769 | 17 | float d[3] = {0, 0, 0}; |
gkumar | 0:019699da1769 | 18 | |
gkumar | 0:019699da1769 | 19 | float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}}; |
gkumar | 0:019699da1769 | 20 | |
gkumar | 0:019699da1769 | 21 | float den = 0, den2; |
gkumar | 0:019699da1769 | 22 | |
gkumar | 0:019699da1769 | 23 | int i, j; //temporary variables |
gkumar | 0:019699da1769 | 24 | |
gkumar | 0:019699da1769 | 25 | float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs |
gkumar | 0:019699da1769 | 26 | |
gkumar | 0:019699da1769 | 27 | float invJm[3][3]; |
gkumar | 0:019699da1769 | 28 | |
gkumar | 0:019699da1769 | 29 | float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4; |
gkumar | 0:019699da1769 | 30 | |
gkumar | 0:019699da1769 | 31 | //structure parameters |
gkumar | 0:019699da1769 | 32 | |
gkumar | 0:019699da1769 | 33 | float inputs[9]; |
gkumar | 0:019699da1769 | 34 | |
gkumar | 0:019699da1769 | 35 | void inverse (float mat[3][3], float inv[3][3]); |
gkumar | 0:019699da1769 | 36 | |
gkumar | 0:019699da1769 | 37 | void getInput (float x[9]); |
gkumar | 0:019699da1769 | 38 | |
gkumar | 0:019699da1769 | 39 | //functions |
gkumar | 0:019699da1769 | 40 | |
gkumar | 0:019699da1769 | 41 | int main() { |
gkumar | 0:019699da1769 | 42 | |
gkumar | 0:019699da1769 | 43 | ////////// Input from Matlab ////////////// |
gkumar | 0:019699da1769 | 44 | |
gkumar | 0:019699da1769 | 45 | while(1) { |
gkumar | 0:019699da1769 | 46 | |
gkumar | 0:019699da1769 | 47 | getInput(inputs); |
gkumar | 0:019699da1769 | 48 | |
gkumar | 0:019699da1769 | 49 | //while(1) |
gkumar | 0:019699da1769 | 50 | |
gkumar | 0:019699da1769 | 51 | b[0] = inputs[0]; |
gkumar | 0:019699da1769 | 52 | |
gkumar | 0:019699da1769 | 53 | b[1] = inputs[1]; |
gkumar | 0:019699da1769 | 54 | |
gkumar | 0:019699da1769 | 55 | b[2] = inputs[2]; |
gkumar | 0:019699da1769 | 56 | |
gkumar | 0:019699da1769 | 57 | db[0] = inputs[3]; |
gkumar | 0:019699da1769 | 58 | |
gkumar | 0:019699da1769 | 59 | db[1] = inputs[4]; |
gkumar | 0:019699da1769 | 60 | |
gkumar | 0:019699da1769 | 61 | db[2] = inputs[5]; |
gkumar | 0:019699da1769 | 62 | |
gkumar | 0:019699da1769 | 63 | omega[0] = inputs[6]; |
gkumar | 0:019699da1769 | 64 | |
gkumar | 0:019699da1769 | 65 | omega[1] = inputs[7]; |
gkumar | 0:019699da1769 | 66 | |
gkumar | 0:019699da1769 | 67 | omega[2] = inputs[8]; |
gkumar | 0:019699da1769 | 68 | |
gkumar | 0:019699da1769 | 69 | /////////// Control Algorithm ////////////////////// |
gkumar | 0:019699da1769 | 70 | |
gkumar | 0:019699da1769 | 71 | // calculate norm b, norm db |
gkumar | 0:019699da1769 | 72 | |
gkumar | 0:019699da1769 | 73 | den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2])); |
gkumar | 0:019699da1769 | 74 | |
gkumar | 0:019699da1769 | 75 | den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]); |
gkumar | 0:019699da1769 | 76 | |
gkumar | 0:019699da1769 | 77 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 78 | |
gkumar | 0:019699da1769 | 79 | { |
gkumar | 0:019699da1769 | 80 | |
gkumar | 0:019699da1769 | 81 | db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3)); |
gkumar | 0:019699da1769 | 82 | |
gkumar | 0:019699da1769 | 83 | //db[i]/=den*den*den; |
gkumar | 0:019699da1769 | 84 | |
gkumar | 0:019699da1769 | 85 | } |
gkumar | 0:019699da1769 | 86 | |
gkumar | 0:019699da1769 | 87 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 88 | |
gkumar | 0:019699da1769 | 89 | { |
gkumar | 0:019699da1769 | 90 | |
gkumar | 0:019699da1769 | 91 | b[i] /= den; |
gkumar | 0:019699da1769 | 92 | |
gkumar | 0:019699da1769 | 93 | } |
gkumar | 0:019699da1769 | 94 | |
gkumar | 0:019699da1769 | 95 | // select kz, kmu, gamma |
gkumar | 0:019699da1769 | 96 | |
gkumar | 0:019699da1769 | 97 | if(b[0]>0.9 || b[0]<-0.9) |
gkumar | 0:019699da1769 | 98 | |
gkumar | 0:019699da1769 | 99 | { |
gkumar | 0:019699da1769 | 100 | |
gkumar | 0:019699da1769 | 101 | kz = kz2; |
gkumar | 0:019699da1769 | 102 | |
gkumar | 0:019699da1769 | 103 | kmu = kmu2; |
gkumar | 0:019699da1769 | 104 | |
gkumar | 0:019699da1769 | 105 | gamma = gamma2; |
gkumar | 0:019699da1769 | 106 | |
gkumar | 0:019699da1769 | 107 | } |
gkumar | 0:019699da1769 | 108 | |
gkumar | 0:019699da1769 | 109 | // calculate Mu, v, dv, z, u |
gkumar | 0:019699da1769 | 110 | |
gkumar | 0:019699da1769 | 111 | for(i=0;i<2;i++) |
gkumar | 0:019699da1769 | 112 | |
gkumar | 0:019699da1769 | 113 | { |
gkumar | 0:019699da1769 | 114 | |
gkumar | 0:019699da1769 | 115 | Mu[i] = b[i+1]; |
gkumar | 0:019699da1769 | 116 | |
gkumar | 0:019699da1769 | 117 | v[i] = -kmu*Mu[i]; |
gkumar | 0:019699da1769 | 118 | |
gkumar | 0:019699da1769 | 119 | dv[i] = -kmu*db[i+1]; |
gkumar | 0:019699da1769 | 120 | |
gkumar | 0:019699da1769 | 121 | z[i] = db[i+1] - v[i]; |
gkumar | 0:019699da1769 | 122 | |
gkumar | 0:019699da1769 | 123 | u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma); |
gkumar | 0:019699da1769 | 124 | |
gkumar | 0:019699da1769 | 125 | } |
gkumar | 0:019699da1769 | 126 | |
gkumar | 0:019699da1769 | 127 | inverse(Jm, invJm); |
gkumar | 0:019699da1769 | 128 | |
gkumar | 0:019699da1769 | 129 | // calculate cross(omega,J*omega) |
gkumar | 0:019699da1769 | 130 | |
gkumar | 0:019699da1769 | 131 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 132 | |
gkumar | 0:019699da1769 | 133 | { |
gkumar | 0:019699da1769 | 134 | |
gkumar | 0:019699da1769 | 135 | for(j=0;j<3;j++) |
gkumar | 0:019699da1769 | 136 | |
gkumar | 0:019699da1769 | 137 | bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - |
gkumar | 0:019699da1769 | 138 | |
gkumar | 0:019699da1769 | 139 | omega[(i+2)%3]*Jm[(i+1)%3][j]); |
gkumar | 0:019699da1769 | 140 | |
gkumar | 0:019699da1769 | 141 | } |
gkumar | 0:019699da1769 | 142 | |
gkumar | 0:019699da1769 | 143 | // calculate invJm*cross(omega,J*omega) store in d |
gkumar | 0:019699da1769 | 144 | |
gkumar | 0:019699da1769 | 145 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 146 | |
gkumar | 0:019699da1769 | 147 | { |
gkumar | 0:019699da1769 | 148 | |
gkumar | 0:019699da1769 | 149 | for(j=0;j<3;j++) |
gkumar | 0:019699da1769 | 150 | |
gkumar | 0:019699da1769 | 151 | d[i] += bb[j]*invJm[i][j]; |
gkumar | 0:019699da1769 | 152 | |
gkumar | 0:019699da1769 | 153 | } |
gkumar | 0:019699da1769 | 154 | |
gkumar | 0:019699da1769 | 155 | // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db) |
gkumar | 0:019699da1769 | 156 | |
gkumar | 0:019699da1769 | 157 | // bb =[0;u-d(2:3)] |
gkumar | 0:019699da1769 | 158 | |
gkumar | 0:019699da1769 | 159 | // store in bb |
gkumar | 0:019699da1769 | 160 | |
gkumar | 0:019699da1769 | 161 | bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]); |
gkumar | 0:019699da1769 | 162 | bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]); |
gkumar | 0:019699da1769 | 163 | bb[0] = 0; |
gkumar | 0:019699da1769 | 164 | |
gkumar | 0:019699da1769 | 165 | // calculate N |
gkumar | 0:019699da1769 | 166 | |
gkumar | 0:019699da1769 | 167 | // reusing invJm as N |
gkumar | 0:019699da1769 | 168 | |
gkumar | 0:019699da1769 | 169 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 170 | |
gkumar | 0:019699da1769 | 171 | { |
gkumar | 0:019699da1769 | 172 | |
gkumar | 0:019699da1769 | 173 | d[i] = invJm[1][i]; |
gkumar | 0:019699da1769 | 174 | |
gkumar | 0:019699da1769 | 175 | invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i]; |
gkumar | 0:019699da1769 | 176 | |
gkumar | 0:019699da1769 | 177 | invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i]; |
gkumar | 0:019699da1769 | 178 | |
gkumar | 0:019699da1769 | 179 | invJm[0][i] = b[i]; |
gkumar | 0:019699da1769 | 180 | |
gkumar | 0:019699da1769 | 181 | } |
gkumar | 0:019699da1769 | 182 | |
gkumar | 0:019699da1769 | 183 | // calculate inv(N) store in Jm |
gkumar | 0:019699da1769 | 184 | |
gkumar | 0:019699da1769 | 185 | inverse(invJm, Jm); |
gkumar | 0:019699da1769 | 186 | |
gkumar | 0:019699da1769 | 187 | // calculate tauc |
gkumar | 0:019699da1769 | 188 | |
gkumar | 0:019699da1769 | 189 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 190 | |
gkumar | 0:019699da1769 | 191 | { |
gkumar | 0:019699da1769 | 192 | |
gkumar | 0:019699da1769 | 193 | for(j=0;j<3;j++) |
gkumar | 0:019699da1769 | 194 | |
gkumar | 0:019699da1769 | 195 | tauc[i] += Jm[i][j]*bb[j]; |
gkumar | 0:019699da1769 | 196 | |
gkumar | 0:019699da1769 | 197 | } |
gkumar | 0:019699da1769 | 198 | |
gkumar | 0:019699da1769 | 199 | /////////// Output to Matlab ////////////////// |
gkumar | 0:019699da1769 | 200 | |
gkumar | 0:019699da1769 | 201 | for(i=0;i<3;i++) { |
gkumar | 0:019699da1769 | 202 | |
gkumar | 0:019699da1769 | 203 | printf("%f\n",tauc[i]*10000000); |
gkumar | 0:019699da1769 | 204 | |
gkumar | 0:019699da1769 | 205 | wait_ms(10); |
gkumar | 0:019699da1769 | 206 | |
gkumar | 0:019699da1769 | 207 | } |
gkumar | 0:019699da1769 | 208 | |
gkumar | 0:019699da1769 | 209 | } |
gkumar | 0:019699da1769 | 210 | |
gkumar | 0:019699da1769 | 211 | return 0; |
gkumar | 0:019699da1769 | 212 | |
gkumar | 0:019699da1769 | 213 | } |
gkumar | 0:019699da1769 | 214 | |
gkumar | 0:019699da1769 | 215 | void inverse(float mat[3][3], float inv[3][3]) |
gkumar | 0:019699da1769 | 216 | { |
gkumar | 0:019699da1769 | 217 | int i, j; |
gkumar | 0:019699da1769 | 218 | float det = 0; |
gkumar | 0:019699da1769 | 219 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 220 | { for(j=0;j<3;j++) |
gkumar | 0:019699da1769 | 221 | inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3] |
gkumar | 0:019699da1769 | 222 | [(j+1)%3]*mat[(i+1)%3][(j+2)%3]); |
gkumar | 0:019699da1769 | 223 | } |
gkumar | 0:019699da1769 | 224 | det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]); |
gkumar | 0:019699da1769 | 225 | for(i=0;i<3;i++) |
gkumar | 0:019699da1769 | 226 | { for(j=0;j<3;j++) |
gkumar | 0:019699da1769 | 227 | inv[i][j] /= det; |
gkumar | 0:019699da1769 | 228 | } |
gkumar | 0:019699da1769 | 229 | } |
gkumar | 0:019699da1769 | 230 | void getInput (float x[9]) { |
gkumar | 0:019699da1769 | 231 | char c[10]; |
gkumar | 0:019699da1769 | 232 | char tempchar[8]; |
gkumar | 0:019699da1769 | 233 | int i, j; |
gkumar | 0:019699da1769 | 234 | //float f[9]; |
gkumar | 0:019699da1769 | 235 | long n = 0; |
gkumar | 0:019699da1769 | 236 | float flval = 0; |
gkumar | 0:019699da1769 | 237 | for(j=0;j<9;j++) { |
gkumar | 0:019699da1769 | 238 | for(i=0;i<9;i++) { |
gkumar | 0:019699da1769 | 239 | c[i] = pc.getc(); |
gkumar | 0:019699da1769 | 240 | if(i<8) { |
gkumar | 0:019699da1769 | 241 | tempchar[i] = c[i]; |
gkumar | 0:019699da1769 | 242 | } |
gkumar | 0:019699da1769 | 243 | } |
gkumar | 0:019699da1769 | 244 | sscanf (tempchar, "%8x", &n); |
gkumar | 0:019699da1769 | 245 | memcpy(&flval, &n, sizeof(long)); |
gkumar | 0:019699da1769 | 246 | printf("%f\n", flval); |
gkumar | 0:019699da1769 | 247 | x[j] = flval; |
gkumar | 0:019699da1769 | 248 | } |
gkumar | 0:019699da1769 | 249 | } |