GOPA KUMAR K C / Mbed 2 deprecated Contro_Algo

Control_Algo

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }