Zhan Tu
/
fish_control
fish
Diff: main_copy.cpp
- Revision:
- 0:561f7672eaad
diff -r 000000000000 -r 561f7672eaad main_copy.cpp --- /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); + +}