Diff: main.cpp

Revision:

0:019699da1769

--- /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;
+}
+}