Diff: main_copy.cpp
- Revision:
- 0:561f7672eaad
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main_copy.cpp Wed Jun 14 20:22:45 2017 +0000
@@ -0,0 +1,579 @@
+#include "mbed.h"
+#include "L3GD20.h"
+#include "LSM303DLHC.h"
+#include "Servo.h"
+#include "math.h"
+#define TS 10 // sample time [ms]
+#define SIMULATION_TIME 10 // simulation time [s]
+
+LocalFileSystem local("local");
+
+
+//funtion declaration
+void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3]);
+void getEulerAngles(float angles[3], float RotMatrix[9]);
+void matPrint(float *A, int r, int c);
+void matSum(float *C, float *A, float *B, int r, int c);
+void matProd(float *C, float *A, float *B, int r, int cA, int cB);
+void matTimesVect(float *b, float *A, float *v, int r, int c);
+void transpose(float *B, float *A, int n);
+float norm(float *v, int r);
+void hat(float *B, float *A);
+void moveA();
+void moveB();
+void moveC();
+//variable definition
+float right_pectoral_freq;
+float left_pectoral_freq;
+float tail_freq;
+bool directionA=1;
+bool directionB=1;
+bool directionC=1;
+float speedA=1;
+float speedB=1;
+float speedC=1;
+
+float gx ;
+float gy ;
+float gz ;
+float ax ;
+float ay ;
+float az ;
+float mx ;
+float my ;
+float mz ;
+float GYRO[3], ACC[3], MAGN[3];
+float G[3], A[3], M[3], A_0[3], M_0[3];
+
+const float pi = 3.1415926;
+const float KRG = 57.2958; // radians to degrees
+
+//G offset: 0.918820 0.550812 -1.280107
+//A offset: -0.001172 0.989633 0.123766
+const float gx_static_offset=0.918820;
+const float gy_static_offset=0.550812;
+const float gz_static_offset=-1.280107;
+
+
+
+const float gain_gyro = 0.00013315;
+const float gain_acc = 0.0098;
+const float gain_magn = 0.005035;
+float Ka = 0.0001; // accelerometer estimation gain
+float Kg = 0.0004; // gyro estimation gain
+float Km = 0; // magnetometer estiamtion gain
+float g_0[3] = {0.0, 0.0, 9.8}; // gavity vector in S
+float m_0[3] = {0.0, 0.0, 0.0};
+float angles[3] = {0.0, 0.0, 0.0};
+float R_est[9] = {0, 0, 1, 1, 0, 0, 0, 1, 0};
+float R_old[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+float eye3[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+
+float rho=0;
+float pitch=0;
+float yaw=0;
+
+//sinusoidal trajectory parameters
+float freq_l=3;
+float freq_r=3;
+float freq_t=3;
+
+float Amp_l=0;
+float Amp_r=0;
+float Amp_t=0;
+
+float center_l=0;
+float center_r=0;
+float center_t=0;
+
+float phi_l=0;
+float phi_r=0;
+float phi_t=0;
+
+
+float IR_l_data=0.0;
+float IR_r_data=0.0;
+
+
+Ticker tickA;
+Ticker tickB;
+Ticker tickC;
+Ticker t3;
+//Timer realTime;
+
+//sensor declaration
+LSM303DLHC compass(p28, p27);
+L3GD20 gyro(p28, p27);
+Serial pc(USBTX, USBRX);
+//LocalFileSystem local("local");
+//actuator declaration
+//PwmOut fin_l(p22);//was 23
+//PwmOut fin_r(p25);//was 25
+//PwmOut fin_t(p26);//was 21
+
+AnalogIn IR_l(p19);
+AnalogIn IR_r(p20);
+
+
+PwmOut PWMA(p25);//RIGHT FIN
+PwmOut PWMB(p23);//LEFT FIN
+PwmOut PWMC(p21);//TAIL FIN
+DigitalOut STBY1 (p30);
+DigitalOut STBY2 (p29);
+DigitalOut AIN1 (p8);
+DigitalOut AIN2 (p11);
+DigitalOut BIN1 (p7);
+DigitalOut BIN2 (p6);
+DigitalOut CIN1 (p16);
+DigitalOut CIN2 (p17);
+int inPin1;
+int inPin2;
+
+float period_pwm;
+void move(int motor, float speed, int direction);
+void stop();
+void loop();
+
+//time parameters
+float t1=0.0;
+int sample_interval_us=TS*1000; // dt=TS sec
+float dt;
+
+
+void loop()
+{
+
+pc.printf("loop start");
+ gyro.read(&G[0],&G[1], &G[2]);
+ compass.read(&A[0],&A[1],&A[2],&M[0],&M[1],&M[2]);
+
+ G[0]=G[0] - gx_static_offset;
+ G[1]=G[1] - gy_static_offset;
+ G[2]=G[2] - gz_static_offset;
+
+ //M[0]=mx;
+ //M[1]=my;
+ //M[2]=mz;
+
+ A[0]=A[0];
+ A[1]=A[1];
+ A[2]=A[2];
+ pc.printf("a0: %f %f %f \n \r",A[0], A[1], A[2]);
+ pc.printf("g0: %f %f %f \n \r ",G[0], G[1], G[2]);
+ //pc.printf("m0: %f %f %f \n \r \n \r",mx, my, mz);
+ pc.printf("\n\r");
+ GYRO[0] = G[0]*gain_gyro;
+ GYRO[1] = G[1]*gain_gyro;
+ GYRO[2] = G[2]*gain_gyro;
+ MAGN[0] = M[0]*gain_magn;
+ MAGN[1] = M[1]*gain_magn;
+ MAGN[2] = M[2]*gain_magn;
+ ACC[0] = -A[0]*gain_acc;
+ ACC[1] = -A[1]*gain_acc;
+ ACC[2] = -A[2]*gain_acc;
+ //pc.printf("a1: %f %f %f \n \r",ACC[0], ACC[1], ACC[2]);
+ //pc.printf("g1: %f %f %f \n \r",GYRO[0], GYRO[1], GYRO[2]);
+ // pc.printf("m1: %f %f %f \n \r \n \r",MAGN[0], MAGN[1], MAGN[2]);
+ pc.printf("\n\r");
+ //matProd(R_old, R_est, eye3, 3, 3, 3);
+ sensorFusion(R_est, R_old, G, A, M);
+ getEulerAngles(angles, R_est);
+
+ rho=KRG*angles[0];
+ yaw=KRG*angles[1];
+ pitch=KRG*angles[2];
+ pc.printf("angles= %f %f %f \n\r \n \r",rho,pitch,yaw);
+ //float thisTime=realTime.read();
+// FILE *fp = fopen("/local/test.txt", "w");
+// fprintf(fp, "angles= %f %f %f %f \n\r \n \r",rho,pitch,yaw,thisTime);
+// fclose(fp);
+//controller
+//time
+// t1=t1+dt;
+}
+
+int main()
+{
+//initialization
+ pc.baud(9600);
+
+
+
+ //realTime.start();
+
+ //setup for PWM
+ period_pwm=0.01;
+ PWMA.period(period_pwm); // servo requires a 20ms period
+ PWMB.period(period_pwm); // servo requires a 20ms period
+
+ directionA=1;
+ directionB=1;
+ directionC=1;
+ speedA=1;
+ speedB=1;
+ speedC=1;
+ left_pectoral_freq=3;
+ right_pectoral_freq=0;
+ tail_freq=3;
+
+ //dt=sample_interval_us/1000000.0 ;
+ pc.printf("loop attach \n \r");
+ t3.attach(&loop, .5);
+ /*float d_freq=yaw*(.1);
+ if (d_freq<0) {
+ left_pectoral_freq=3-d_freq;
+ right_pectoral_freq=3;
+ }
+ if(d_freq>0) {
+ left_pectoral_freq=3;
+ right_pectoral_freq=3-d_freq;
+ }
+
+ */
+ pc.printf("after attach \n \r");
+ if(left_pectoral_freq > 0) {
+ float right_interval=1/right_pectoral_freq;
+ tickA.attach(&moveA, right_interval);
+ }
+
+ if(right_pectoral_freq > 0) {
+ float left_interval=1/left_pectoral_freq;
+ tickB.attach(&moveB, left_interval);
+ }
+
+ if(tail_freq > 0) {
+ float tail_interval=1/tail_freq;
+ tickC.attach(&moveC, tail_interval);
+ }
+
+
+
+
+
+
+
+
+}
+
+void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3])
+
+{
+ // w = gyro + a + b
+ // a = KG*(acc x c)
+ // b = KB*(magn x d)
+ // c = RRt*gravity
+ // d = RRt*mfield
+
+ // Rest = Rold*WW
+ // WW = I+II+III
+ // I = eye(3)
+ // II = alpha*T_c*what
+ // III = beta*T_c*T_c*W2
+ // W2 = what*what
+
+
+ float RRt[9];
+ transpose(RRt, Rold, 3);
+
+ float a[3];
+ float b[3];
+ float c[3];
+ float d[3];
+ float acchat[9];
+ float magnhat[9];
+ float w[3];
+ float what[9];
+ float nw;
+ float alpha;
+ float beta;
+ float II[9];
+ float III[9];
+ float W2[9];
+ float WW[9];
+
+ matTimesVect(c, RRt, g_0, 3, 3);
+ hat(acchat, ac);
+ matTimesVect(a, acchat, c, 3, 3);
+
+ matTimesVect(d, RRt, m_0, 3, 3);
+ hat(magnhat, ma);
+ matTimesVect(b, magnhat, d, 3, 3);
+ int k;
+ for (k = 0; k < 3; k++) {
+ float sum = Kg*gy[k]+(Ka*a[k])+(Km*b[k]);
+ w[k] = sum;
+ }
+ //matPrint(w,k,1);
+ hat(what, w);
+ nw = norm(w, 3);
+ nw = TS*nw;
+
+ alpha = 0;
+ beta = 0;
+
+ if (nw < -0.000000001 || nw > 0.000000001) {
+ alpha = (sin(nw))/nw;
+ beta = (1-cos(nw))/(nw*nw);
+ }
+
+ // printf("\t %f \n", alpha);
+ // printf("\t %f \n", beta);
+
+ matProd(W2, what, what, 3, 3, 3);
+
+ for (int j = 0; j < 9; j++) {
+ II[j] = alpha*TS*what[j];
+ III[j] = beta*TS*TS*W2[j];
+ WW[j] = eye3[j]+II[j]+III[j];
+ }
+
+ matProd(Rest, Rold, WW, 3, 3, 3);
+ //matPrint(Rest,3,3);
+
+}
+
+
+void getEulerAngles(float angles[3], float RotMatrix[9])
+{
+ float theta = -asin(RotMatrix[2]);
+ float phi = atan2(RotMatrix[5]/cos(theta),RotMatrix[8]/cos(theta));
+ float psi = atan2(RotMatrix[1]/cos(theta),RotMatrix[0]/cos(theta));
+ angles[0] = phi;
+ angles[1] = theta;
+ angles[2] = psi;
+ return;
+}
+
+
+// Matrix operations (matrices are stored in a one dimensional array, columnwise)
+// e.g.: [1 2 3; 4 5 6; 7 8 9] = [1 4 7 2 5 8 3 6 9]
+
+
+void matPrint(float *A, int r, int c)
+
+// Print the content of the matrix A which is r x c
+
+{
+ int i, j;
+ pc.printf("printing \n \r");
+ for(i = 0; i < r; i++) {
+ for(j = 0; j < c; j++) {
+ pc.printf("%f",A[i + j*r]);
+ if(j == c-1) {
+ pc.printf("\n \r");
+ } else {
+ pc.printf("\t");
+ }
+ }
+ }
+
+ printf("\n");
+}
+
+void matSum(float *C, float *A, float *B, int r, int c)
+
+// C = A + B, where A, B and C are r x c
+
+{
+ int i, j;
+ for(i = 0; i < r; i++) {
+ for(j = 0; j < c; j++) {
+ C[i + r*j] = A[i + r*j] + B[i + r*j];
+ }
+ }
+ return;
+}
+
+
+void matProd(float *C, float *A, float *B, int r, int cA, int cB)
+
+// C = A times B, where A is r x c, B is c x c and C is r x c
+
+{
+ float sum;
+ int i, j, k;
+
+ for(i = 0; i < r; i++) {
+ for(j = 0; j < cB; j++) {
+ sum = 0.0;
+ for(k = 0; k < cA; k++) {
+ sum = sum + A[i + k*r] * B[j*cA + k];
+ }
+ C[i + r*j] = sum;
+ }
+ }
+
+ return;
+}
+
+
+void matTimesVect(float *b, float *A, float *v, int r, int c)
+
+// b = A times v, where A is r x c, and v and b are c-long column vectors
+
+{
+ float sum;
+ int i, k;
+
+ for(i = 0; i < r; i++) {
+ sum = 0.00;
+ for(k = 0; k < c; k++) {
+ sum = sum + A[i + k*r] * v[k];
+ }
+
+ b[i] = sum;
+ }
+
+ return;
+}
+
+
+void transpose(float *B, float *A, int n)
+
+// B = A^T, where A and B are n x n matrices
+
+{
+ int i, j;
+
+ for(i = 0; i < n; i++) {
+ for(j = 0; j < n; j++) {
+ B[n*i+j] = A[i + n*j];
+ }
+ }
+
+ return;
+}
+
+
+float norm(float *v, int r)
+
+// 2-norm of a r-long vector
+
+{
+ float n = 0;
+ int i;
+
+ for(i = 0; i < r; i++) {
+ n = n + v[i]*v[i];
+ }
+
+ n = sqrt(n);
+
+ return n;
+}
+
+
+void hat(float *B, float *A)
+
+// hat operator (for 3-D arrays only)
+
+{
+ B[0] = 0;
+ B[1] = A[2];
+ B[2] = -A[1];
+ B[3] = -A[2];
+ B[4] = 0;
+ B[5] = A[0];
+ B[6] = A[1];
+ B[7] = -A[0];
+ B[8] = 0;
+
+ return;
+}
+
+void move(int motor, float speed, int direction)
+{
+//Move specific motor at speed and direction
+//motor: 0 for B 1 for A
+//speed: 0 is off, and 1 is full speed
+//direction: 0 clockwise, 1 counter-clockwise
+
+ STBY1=1; //disable standby
+ STBY2=1; //disable standby
+
+
+ if(direction == 0) {
+ inPin1 = 0;
+ inPin2 = 1;
+ } else if(direction == 1) {
+
+ inPin1 = 1;
+ inPin2 = 0;
+
+ }
+
+ if(motor == 1) {
+ AIN1=inPin1;
+ AIN2=inPin2;
+ PWMA.pulsewidth(period_pwm*speed*1.0);
+ } else if(motor == 0) {
+ BIN1=inPin1;
+ BIN2=inPin2;
+ PWMB.pulsewidth(period_pwm*speed*1.0);
+ }
+}
+
+
+void stop()
+{
+//enable standby
+ STBY1=0;
+ STBY2=0;
+
+}
+
+void moveA()
+{
+ STBY1=1; //disable standby
+ int inPin1=1;
+ int inPin2=0;
+ if(directionA==0) {
+ inPin1 = 0;
+ inPin2 = 1;
+ }
+ AIN1=inPin1;
+ AIN2=inPin2;
+ directionA=!directionA;
+
+ //pc.printf("dirA = %d\n\r",directionA);
+
+
+
+ PWMA.pulsewidth(period_pwm*speedA);
+
+}
+void moveB()
+{
+ STBY1=1; //disable standby
+ int inPin1=0;
+ int inPin2=1;
+ if(directionB==0) {
+ inPin1 = 1;
+ inPin2 = 0;
+ }
+ //pc.printf("dirB = %d\n\r",directionB);
+ BIN1=inPin1;
+ BIN2=inPin2;
+ directionB=!directionB;
+
+ PWMB.pulsewidth(period_pwm*speedB);
+
+}
+
+void moveC()
+{
+ STBY1=1; //disable standby
+ int inPin1=0;
+ int inPin2=1;
+ if(directionC==0) {
+ inPin1 = 1;
+ inPin2 = 0;
+ }
+ //pc.printf("dirC = %d\n\r",directionC);
+ CIN1=inPin1;
+ CIN2=inPin2;
+ directionC=!directionC;
+
+ PWMC.pulsewidth(period_pwm*speedC);
+
+}
Zhan Tu
