![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fish
main_copy.cpp@0:561f7672eaad, 2017-06-14 (annotated)
- Committer:
- tzxl10000
- Date:
- Wed Jun 14 20:22:45 2017 +0000
- Revision:
- 0:561f7672eaad
fish;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tzxl10000 | 0:561f7672eaad | 1 | #include "mbed.h" |
tzxl10000 | 0:561f7672eaad | 2 | #include "L3GD20.h" |
tzxl10000 | 0:561f7672eaad | 3 | #include "LSM303DLHC.h" |
tzxl10000 | 0:561f7672eaad | 4 | #include "Servo.h" |
tzxl10000 | 0:561f7672eaad | 5 | #include "math.h" |
tzxl10000 | 0:561f7672eaad | 6 | #define TS 10 // sample time [ms] |
tzxl10000 | 0:561f7672eaad | 7 | #define SIMULATION_TIME 10 // simulation time [s] |
tzxl10000 | 0:561f7672eaad | 8 | |
tzxl10000 | 0:561f7672eaad | 9 | LocalFileSystem local("local"); |
tzxl10000 | 0:561f7672eaad | 10 | |
tzxl10000 | 0:561f7672eaad | 11 | |
tzxl10000 | 0:561f7672eaad | 12 | //funtion declaration |
tzxl10000 | 0:561f7672eaad | 13 | void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3]); |
tzxl10000 | 0:561f7672eaad | 14 | void getEulerAngles(float angles[3], float RotMatrix[9]); |
tzxl10000 | 0:561f7672eaad | 15 | void matPrint(float *A, int r, int c); |
tzxl10000 | 0:561f7672eaad | 16 | void matSum(float *C, float *A, float *B, int r, int c); |
tzxl10000 | 0:561f7672eaad | 17 | void matProd(float *C, float *A, float *B, int r, int cA, int cB); |
tzxl10000 | 0:561f7672eaad | 18 | void matTimesVect(float *b, float *A, float *v, int r, int c); |
tzxl10000 | 0:561f7672eaad | 19 | void transpose(float *B, float *A, int n); |
tzxl10000 | 0:561f7672eaad | 20 | float norm(float *v, int r); |
tzxl10000 | 0:561f7672eaad | 21 | void hat(float *B, float *A); |
tzxl10000 | 0:561f7672eaad | 22 | void moveA(); |
tzxl10000 | 0:561f7672eaad | 23 | void moveB(); |
tzxl10000 | 0:561f7672eaad | 24 | void moveC(); |
tzxl10000 | 0:561f7672eaad | 25 | //variable definition |
tzxl10000 | 0:561f7672eaad | 26 | float right_pectoral_freq; |
tzxl10000 | 0:561f7672eaad | 27 | float left_pectoral_freq; |
tzxl10000 | 0:561f7672eaad | 28 | float tail_freq; |
tzxl10000 | 0:561f7672eaad | 29 | bool directionA=1; |
tzxl10000 | 0:561f7672eaad | 30 | bool directionB=1; |
tzxl10000 | 0:561f7672eaad | 31 | bool directionC=1; |
tzxl10000 | 0:561f7672eaad | 32 | float speedA=1; |
tzxl10000 | 0:561f7672eaad | 33 | float speedB=1; |
tzxl10000 | 0:561f7672eaad | 34 | float speedC=1; |
tzxl10000 | 0:561f7672eaad | 35 | |
tzxl10000 | 0:561f7672eaad | 36 | float gx ; |
tzxl10000 | 0:561f7672eaad | 37 | float gy ; |
tzxl10000 | 0:561f7672eaad | 38 | float gz ; |
tzxl10000 | 0:561f7672eaad | 39 | float ax ; |
tzxl10000 | 0:561f7672eaad | 40 | float ay ; |
tzxl10000 | 0:561f7672eaad | 41 | float az ; |
tzxl10000 | 0:561f7672eaad | 42 | float mx ; |
tzxl10000 | 0:561f7672eaad | 43 | float my ; |
tzxl10000 | 0:561f7672eaad | 44 | float mz ; |
tzxl10000 | 0:561f7672eaad | 45 | float GYRO[3], ACC[3], MAGN[3]; |
tzxl10000 | 0:561f7672eaad | 46 | float G[3], A[3], M[3], A_0[3], M_0[3]; |
tzxl10000 | 0:561f7672eaad | 47 | |
tzxl10000 | 0:561f7672eaad | 48 | const float pi = 3.1415926; |
tzxl10000 | 0:561f7672eaad | 49 | const float KRG = 57.2958; // radians to degrees |
tzxl10000 | 0:561f7672eaad | 50 | |
tzxl10000 | 0:561f7672eaad | 51 | //G offset: 0.918820 0.550812 -1.280107 |
tzxl10000 | 0:561f7672eaad | 52 | //A offset: -0.001172 0.989633 0.123766 |
tzxl10000 | 0:561f7672eaad | 53 | const float gx_static_offset=0.918820; |
tzxl10000 | 0:561f7672eaad | 54 | const float gy_static_offset=0.550812; |
tzxl10000 | 0:561f7672eaad | 55 | const float gz_static_offset=-1.280107; |
tzxl10000 | 0:561f7672eaad | 56 | |
tzxl10000 | 0:561f7672eaad | 57 | |
tzxl10000 | 0:561f7672eaad | 58 | |
tzxl10000 | 0:561f7672eaad | 59 | const float gain_gyro = 0.00013315; |
tzxl10000 | 0:561f7672eaad | 60 | const float gain_acc = 0.0098; |
tzxl10000 | 0:561f7672eaad | 61 | const float gain_magn = 0.005035; |
tzxl10000 | 0:561f7672eaad | 62 | float Ka = 0.0001; // accelerometer estimation gain |
tzxl10000 | 0:561f7672eaad | 63 | float Kg = 0.0004; // gyro estimation gain |
tzxl10000 | 0:561f7672eaad | 64 | float Km = 0; // magnetometer estiamtion gain |
tzxl10000 | 0:561f7672eaad | 65 | float g_0[3] = {0.0, 0.0, 9.8}; // gavity vector in S |
tzxl10000 | 0:561f7672eaad | 66 | float m_0[3] = {0.0, 0.0, 0.0}; |
tzxl10000 | 0:561f7672eaad | 67 | float angles[3] = {0.0, 0.0, 0.0}; |
tzxl10000 | 0:561f7672eaad | 68 | float R_est[9] = {0, 0, 1, 1, 0, 0, 0, 1, 0}; |
tzxl10000 | 0:561f7672eaad | 69 | float R_old[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; |
tzxl10000 | 0:561f7672eaad | 70 | float eye3[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; |
tzxl10000 | 0:561f7672eaad | 71 | |
tzxl10000 | 0:561f7672eaad | 72 | float rho=0; |
tzxl10000 | 0:561f7672eaad | 73 | float pitch=0; |
tzxl10000 | 0:561f7672eaad | 74 | float yaw=0; |
tzxl10000 | 0:561f7672eaad | 75 | |
tzxl10000 | 0:561f7672eaad | 76 | //sinusoidal trajectory parameters |
tzxl10000 | 0:561f7672eaad | 77 | float freq_l=3; |
tzxl10000 | 0:561f7672eaad | 78 | float freq_r=3; |
tzxl10000 | 0:561f7672eaad | 79 | float freq_t=3; |
tzxl10000 | 0:561f7672eaad | 80 | |
tzxl10000 | 0:561f7672eaad | 81 | float Amp_l=0; |
tzxl10000 | 0:561f7672eaad | 82 | float Amp_r=0; |
tzxl10000 | 0:561f7672eaad | 83 | float Amp_t=0; |
tzxl10000 | 0:561f7672eaad | 84 | |
tzxl10000 | 0:561f7672eaad | 85 | float center_l=0; |
tzxl10000 | 0:561f7672eaad | 86 | float center_r=0; |
tzxl10000 | 0:561f7672eaad | 87 | float center_t=0; |
tzxl10000 | 0:561f7672eaad | 88 | |
tzxl10000 | 0:561f7672eaad | 89 | float phi_l=0; |
tzxl10000 | 0:561f7672eaad | 90 | float phi_r=0; |
tzxl10000 | 0:561f7672eaad | 91 | float phi_t=0; |
tzxl10000 | 0:561f7672eaad | 92 | |
tzxl10000 | 0:561f7672eaad | 93 | |
tzxl10000 | 0:561f7672eaad | 94 | float IR_l_data=0.0; |
tzxl10000 | 0:561f7672eaad | 95 | float IR_r_data=0.0; |
tzxl10000 | 0:561f7672eaad | 96 | |
tzxl10000 | 0:561f7672eaad | 97 | |
tzxl10000 | 0:561f7672eaad | 98 | Ticker tickA; |
tzxl10000 | 0:561f7672eaad | 99 | Ticker tickB; |
tzxl10000 | 0:561f7672eaad | 100 | Ticker tickC; |
tzxl10000 | 0:561f7672eaad | 101 | Ticker t3; |
tzxl10000 | 0:561f7672eaad | 102 | //Timer realTime; |
tzxl10000 | 0:561f7672eaad | 103 | |
tzxl10000 | 0:561f7672eaad | 104 | //sensor declaration |
tzxl10000 | 0:561f7672eaad | 105 | LSM303DLHC compass(p28, p27); |
tzxl10000 | 0:561f7672eaad | 106 | L3GD20 gyro(p28, p27); |
tzxl10000 | 0:561f7672eaad | 107 | Serial pc(USBTX, USBRX); |
tzxl10000 | 0:561f7672eaad | 108 | //LocalFileSystem local("local"); |
tzxl10000 | 0:561f7672eaad | 109 | //actuator declaration |
tzxl10000 | 0:561f7672eaad | 110 | //PwmOut fin_l(p22);//was 23 |
tzxl10000 | 0:561f7672eaad | 111 | //PwmOut fin_r(p25);//was 25 |
tzxl10000 | 0:561f7672eaad | 112 | //PwmOut fin_t(p26);//was 21 |
tzxl10000 | 0:561f7672eaad | 113 | |
tzxl10000 | 0:561f7672eaad | 114 | AnalogIn IR_l(p19); |
tzxl10000 | 0:561f7672eaad | 115 | AnalogIn IR_r(p20); |
tzxl10000 | 0:561f7672eaad | 116 | |
tzxl10000 | 0:561f7672eaad | 117 | |
tzxl10000 | 0:561f7672eaad | 118 | PwmOut PWMA(p25);//RIGHT FIN |
tzxl10000 | 0:561f7672eaad | 119 | PwmOut PWMB(p23);//LEFT FIN |
tzxl10000 | 0:561f7672eaad | 120 | PwmOut PWMC(p21);//TAIL FIN |
tzxl10000 | 0:561f7672eaad | 121 | DigitalOut STBY1 (p30); |
tzxl10000 | 0:561f7672eaad | 122 | DigitalOut STBY2 (p29); |
tzxl10000 | 0:561f7672eaad | 123 | DigitalOut AIN1 (p8); |
tzxl10000 | 0:561f7672eaad | 124 | DigitalOut AIN2 (p11); |
tzxl10000 | 0:561f7672eaad | 125 | DigitalOut BIN1 (p7); |
tzxl10000 | 0:561f7672eaad | 126 | DigitalOut BIN2 (p6); |
tzxl10000 | 0:561f7672eaad | 127 | DigitalOut CIN1 (p16); |
tzxl10000 | 0:561f7672eaad | 128 | DigitalOut CIN2 (p17); |
tzxl10000 | 0:561f7672eaad | 129 | int inPin1; |
tzxl10000 | 0:561f7672eaad | 130 | int inPin2; |
tzxl10000 | 0:561f7672eaad | 131 | |
tzxl10000 | 0:561f7672eaad | 132 | float period_pwm; |
tzxl10000 | 0:561f7672eaad | 133 | void move(int motor, float speed, int direction); |
tzxl10000 | 0:561f7672eaad | 134 | void stop(); |
tzxl10000 | 0:561f7672eaad | 135 | void loop(); |
tzxl10000 | 0:561f7672eaad | 136 | |
tzxl10000 | 0:561f7672eaad | 137 | //time parameters |
tzxl10000 | 0:561f7672eaad | 138 | float t1=0.0; |
tzxl10000 | 0:561f7672eaad | 139 | int sample_interval_us=TS*1000; // dt=TS sec |
tzxl10000 | 0:561f7672eaad | 140 | float dt; |
tzxl10000 | 0:561f7672eaad | 141 | |
tzxl10000 | 0:561f7672eaad | 142 | |
tzxl10000 | 0:561f7672eaad | 143 | void loop() |
tzxl10000 | 0:561f7672eaad | 144 | { |
tzxl10000 | 0:561f7672eaad | 145 | |
tzxl10000 | 0:561f7672eaad | 146 | pc.printf("loop start"); |
tzxl10000 | 0:561f7672eaad | 147 | gyro.read(&G[0],&G[1], &G[2]); |
tzxl10000 | 0:561f7672eaad | 148 | compass.read(&A[0],&A[1],&A[2],&M[0],&M[1],&M[2]); |
tzxl10000 | 0:561f7672eaad | 149 | |
tzxl10000 | 0:561f7672eaad | 150 | G[0]=G[0] - gx_static_offset; |
tzxl10000 | 0:561f7672eaad | 151 | G[1]=G[1] - gy_static_offset; |
tzxl10000 | 0:561f7672eaad | 152 | G[2]=G[2] - gz_static_offset; |
tzxl10000 | 0:561f7672eaad | 153 | |
tzxl10000 | 0:561f7672eaad | 154 | //M[0]=mx; |
tzxl10000 | 0:561f7672eaad | 155 | //M[1]=my; |
tzxl10000 | 0:561f7672eaad | 156 | //M[2]=mz; |
tzxl10000 | 0:561f7672eaad | 157 | |
tzxl10000 | 0:561f7672eaad | 158 | A[0]=A[0]; |
tzxl10000 | 0:561f7672eaad | 159 | A[1]=A[1]; |
tzxl10000 | 0:561f7672eaad | 160 | A[2]=A[2]; |
tzxl10000 | 0:561f7672eaad | 161 | pc.printf("a0: %f %f %f \n \r",A[0], A[1], A[2]); |
tzxl10000 | 0:561f7672eaad | 162 | pc.printf("g0: %f %f %f \n \r ",G[0], G[1], G[2]); |
tzxl10000 | 0:561f7672eaad | 163 | //pc.printf("m0: %f %f %f \n \r \n \r",mx, my, mz); |
tzxl10000 | 0:561f7672eaad | 164 | pc.printf("\n\r"); |
tzxl10000 | 0:561f7672eaad | 165 | GYRO[0] = G[0]*gain_gyro; |
tzxl10000 | 0:561f7672eaad | 166 | GYRO[1] = G[1]*gain_gyro; |
tzxl10000 | 0:561f7672eaad | 167 | GYRO[2] = G[2]*gain_gyro; |
tzxl10000 | 0:561f7672eaad | 168 | MAGN[0] = M[0]*gain_magn; |
tzxl10000 | 0:561f7672eaad | 169 | MAGN[1] = M[1]*gain_magn; |
tzxl10000 | 0:561f7672eaad | 170 | MAGN[2] = M[2]*gain_magn; |
tzxl10000 | 0:561f7672eaad | 171 | ACC[0] = -A[0]*gain_acc; |
tzxl10000 | 0:561f7672eaad | 172 | ACC[1] = -A[1]*gain_acc; |
tzxl10000 | 0:561f7672eaad | 173 | ACC[2] = -A[2]*gain_acc; |
tzxl10000 | 0:561f7672eaad | 174 | //pc.printf("a1: %f %f %f \n \r",ACC[0], ACC[1], ACC[2]); |
tzxl10000 | 0:561f7672eaad | 175 | //pc.printf("g1: %f %f %f \n \r",GYRO[0], GYRO[1], GYRO[2]); |
tzxl10000 | 0:561f7672eaad | 176 | // pc.printf("m1: %f %f %f \n \r \n \r",MAGN[0], MAGN[1], MAGN[2]); |
tzxl10000 | 0:561f7672eaad | 177 | pc.printf("\n\r"); |
tzxl10000 | 0:561f7672eaad | 178 | //matProd(R_old, R_est, eye3, 3, 3, 3); |
tzxl10000 | 0:561f7672eaad | 179 | sensorFusion(R_est, R_old, G, A, M); |
tzxl10000 | 0:561f7672eaad | 180 | getEulerAngles(angles, R_est); |
tzxl10000 | 0:561f7672eaad | 181 | |
tzxl10000 | 0:561f7672eaad | 182 | rho=KRG*angles[0]; |
tzxl10000 | 0:561f7672eaad | 183 | yaw=KRG*angles[1]; |
tzxl10000 | 0:561f7672eaad | 184 | pitch=KRG*angles[2]; |
tzxl10000 | 0:561f7672eaad | 185 | pc.printf("angles= %f %f %f \n\r \n \r",rho,pitch,yaw); |
tzxl10000 | 0:561f7672eaad | 186 | //float thisTime=realTime.read(); |
tzxl10000 | 0:561f7672eaad | 187 | // FILE *fp = fopen("/local/test.txt", "w"); |
tzxl10000 | 0:561f7672eaad | 188 | // fprintf(fp, "angles= %f %f %f %f \n\r \n \r",rho,pitch,yaw,thisTime); |
tzxl10000 | 0:561f7672eaad | 189 | // fclose(fp); |
tzxl10000 | 0:561f7672eaad | 190 | //controller |
tzxl10000 | 0:561f7672eaad | 191 | //time |
tzxl10000 | 0:561f7672eaad | 192 | // t1=t1+dt; |
tzxl10000 | 0:561f7672eaad | 193 | } |
tzxl10000 | 0:561f7672eaad | 194 | |
tzxl10000 | 0:561f7672eaad | 195 | int main() |
tzxl10000 | 0:561f7672eaad | 196 | { |
tzxl10000 | 0:561f7672eaad | 197 | //initialization |
tzxl10000 | 0:561f7672eaad | 198 | pc.baud(9600); |
tzxl10000 | 0:561f7672eaad | 199 | |
tzxl10000 | 0:561f7672eaad | 200 | |
tzxl10000 | 0:561f7672eaad | 201 | |
tzxl10000 | 0:561f7672eaad | 202 | //realTime.start(); |
tzxl10000 | 0:561f7672eaad | 203 | |
tzxl10000 | 0:561f7672eaad | 204 | //setup for PWM |
tzxl10000 | 0:561f7672eaad | 205 | period_pwm=0.01; |
tzxl10000 | 0:561f7672eaad | 206 | PWMA.period(period_pwm); // servo requires a 20ms period |
tzxl10000 | 0:561f7672eaad | 207 | PWMB.period(period_pwm); // servo requires a 20ms period |
tzxl10000 | 0:561f7672eaad | 208 | |
tzxl10000 | 0:561f7672eaad | 209 | directionA=1; |
tzxl10000 | 0:561f7672eaad | 210 | directionB=1; |
tzxl10000 | 0:561f7672eaad | 211 | directionC=1; |
tzxl10000 | 0:561f7672eaad | 212 | speedA=1; |
tzxl10000 | 0:561f7672eaad | 213 | speedB=1; |
tzxl10000 | 0:561f7672eaad | 214 | speedC=1; |
tzxl10000 | 0:561f7672eaad | 215 | left_pectoral_freq=3; |
tzxl10000 | 0:561f7672eaad | 216 | right_pectoral_freq=0; |
tzxl10000 | 0:561f7672eaad | 217 | tail_freq=3; |
tzxl10000 | 0:561f7672eaad | 218 | |
tzxl10000 | 0:561f7672eaad | 219 | //dt=sample_interval_us/1000000.0 ; |
tzxl10000 | 0:561f7672eaad | 220 | pc.printf("loop attach \n \r"); |
tzxl10000 | 0:561f7672eaad | 221 | t3.attach(&loop, .5); |
tzxl10000 | 0:561f7672eaad | 222 | /*float d_freq=yaw*(.1); |
tzxl10000 | 0:561f7672eaad | 223 | if (d_freq<0) { |
tzxl10000 | 0:561f7672eaad | 224 | left_pectoral_freq=3-d_freq; |
tzxl10000 | 0:561f7672eaad | 225 | right_pectoral_freq=3; |
tzxl10000 | 0:561f7672eaad | 226 | } |
tzxl10000 | 0:561f7672eaad | 227 | if(d_freq>0) { |
tzxl10000 | 0:561f7672eaad | 228 | left_pectoral_freq=3; |
tzxl10000 | 0:561f7672eaad | 229 | right_pectoral_freq=3-d_freq; |
tzxl10000 | 0:561f7672eaad | 230 | } |
tzxl10000 | 0:561f7672eaad | 231 | |
tzxl10000 | 0:561f7672eaad | 232 | */ |
tzxl10000 | 0:561f7672eaad | 233 | pc.printf("after attach \n \r"); |
tzxl10000 | 0:561f7672eaad | 234 | if(left_pectoral_freq > 0) { |
tzxl10000 | 0:561f7672eaad | 235 | float right_interval=1/right_pectoral_freq; |
tzxl10000 | 0:561f7672eaad | 236 | tickA.attach(&moveA, right_interval); |
tzxl10000 | 0:561f7672eaad | 237 | } |
tzxl10000 | 0:561f7672eaad | 238 | |
tzxl10000 | 0:561f7672eaad | 239 | if(right_pectoral_freq > 0) { |
tzxl10000 | 0:561f7672eaad | 240 | float left_interval=1/left_pectoral_freq; |
tzxl10000 | 0:561f7672eaad | 241 | tickB.attach(&moveB, left_interval); |
tzxl10000 | 0:561f7672eaad | 242 | } |
tzxl10000 | 0:561f7672eaad | 243 | |
tzxl10000 | 0:561f7672eaad | 244 | if(tail_freq > 0) { |
tzxl10000 | 0:561f7672eaad | 245 | float tail_interval=1/tail_freq; |
tzxl10000 | 0:561f7672eaad | 246 | tickC.attach(&moveC, tail_interval); |
tzxl10000 | 0:561f7672eaad | 247 | } |
tzxl10000 | 0:561f7672eaad | 248 | |
tzxl10000 | 0:561f7672eaad | 249 | |
tzxl10000 | 0:561f7672eaad | 250 | |
tzxl10000 | 0:561f7672eaad | 251 | |
tzxl10000 | 0:561f7672eaad | 252 | |
tzxl10000 | 0:561f7672eaad | 253 | |
tzxl10000 | 0:561f7672eaad | 254 | |
tzxl10000 | 0:561f7672eaad | 255 | |
tzxl10000 | 0:561f7672eaad | 256 | } |
tzxl10000 | 0:561f7672eaad | 257 | |
tzxl10000 | 0:561f7672eaad | 258 | void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3]) |
tzxl10000 | 0:561f7672eaad | 259 | |
tzxl10000 | 0:561f7672eaad | 260 | { |
tzxl10000 | 0:561f7672eaad | 261 | // w = gyro + a + b |
tzxl10000 | 0:561f7672eaad | 262 | // a = KG*(acc x c) |
tzxl10000 | 0:561f7672eaad | 263 | // b = KB*(magn x d) |
tzxl10000 | 0:561f7672eaad | 264 | // c = RRt*gravity |
tzxl10000 | 0:561f7672eaad | 265 | // d = RRt*mfield |
tzxl10000 | 0:561f7672eaad | 266 | |
tzxl10000 | 0:561f7672eaad | 267 | // Rest = Rold*WW |
tzxl10000 | 0:561f7672eaad | 268 | // WW = I+II+III |
tzxl10000 | 0:561f7672eaad | 269 | // I = eye(3) |
tzxl10000 | 0:561f7672eaad | 270 | // II = alpha*T_c*what |
tzxl10000 | 0:561f7672eaad | 271 | // III = beta*T_c*T_c*W2 |
tzxl10000 | 0:561f7672eaad | 272 | // W2 = what*what |
tzxl10000 | 0:561f7672eaad | 273 | |
tzxl10000 | 0:561f7672eaad | 274 | |
tzxl10000 | 0:561f7672eaad | 275 | float RRt[9]; |
tzxl10000 | 0:561f7672eaad | 276 | transpose(RRt, Rold, 3); |
tzxl10000 | 0:561f7672eaad | 277 | |
tzxl10000 | 0:561f7672eaad | 278 | float a[3]; |
tzxl10000 | 0:561f7672eaad | 279 | float b[3]; |
tzxl10000 | 0:561f7672eaad | 280 | float c[3]; |
tzxl10000 | 0:561f7672eaad | 281 | float d[3]; |
tzxl10000 | 0:561f7672eaad | 282 | float acchat[9]; |
tzxl10000 | 0:561f7672eaad | 283 | float magnhat[9]; |
tzxl10000 | 0:561f7672eaad | 284 | float w[3]; |
tzxl10000 | 0:561f7672eaad | 285 | float what[9]; |
tzxl10000 | 0:561f7672eaad | 286 | float nw; |
tzxl10000 | 0:561f7672eaad | 287 | float alpha; |
tzxl10000 | 0:561f7672eaad | 288 | float beta; |
tzxl10000 | 0:561f7672eaad | 289 | float II[9]; |
tzxl10000 | 0:561f7672eaad | 290 | float III[9]; |
tzxl10000 | 0:561f7672eaad | 291 | float W2[9]; |
tzxl10000 | 0:561f7672eaad | 292 | float WW[9]; |
tzxl10000 | 0:561f7672eaad | 293 | |
tzxl10000 | 0:561f7672eaad | 294 | matTimesVect(c, RRt, g_0, 3, 3); |
tzxl10000 | 0:561f7672eaad | 295 | hat(acchat, ac); |
tzxl10000 | 0:561f7672eaad | 296 | matTimesVect(a, acchat, c, 3, 3); |
tzxl10000 | 0:561f7672eaad | 297 | |
tzxl10000 | 0:561f7672eaad | 298 | matTimesVect(d, RRt, m_0, 3, 3); |
tzxl10000 | 0:561f7672eaad | 299 | hat(magnhat, ma); |
tzxl10000 | 0:561f7672eaad | 300 | matTimesVect(b, magnhat, d, 3, 3); |
tzxl10000 | 0:561f7672eaad | 301 | int k; |
tzxl10000 | 0:561f7672eaad | 302 | for (k = 0; k < 3; k++) { |
tzxl10000 | 0:561f7672eaad | 303 | float sum = Kg*gy[k]+(Ka*a[k])+(Km*b[k]); |
tzxl10000 | 0:561f7672eaad | 304 | w[k] = sum; |
tzxl10000 | 0:561f7672eaad | 305 | } |
tzxl10000 | 0:561f7672eaad | 306 | //matPrint(w,k,1); |
tzxl10000 | 0:561f7672eaad | 307 | hat(what, w); |
tzxl10000 | 0:561f7672eaad | 308 | nw = norm(w, 3); |
tzxl10000 | 0:561f7672eaad | 309 | nw = TS*nw; |
tzxl10000 | 0:561f7672eaad | 310 | |
tzxl10000 | 0:561f7672eaad | 311 | alpha = 0; |
tzxl10000 | 0:561f7672eaad | 312 | beta = 0; |
tzxl10000 | 0:561f7672eaad | 313 | |
tzxl10000 | 0:561f7672eaad | 314 | if (nw < -0.000000001 || nw > 0.000000001) { |
tzxl10000 | 0:561f7672eaad | 315 | alpha = (sin(nw))/nw; |
tzxl10000 | 0:561f7672eaad | 316 | beta = (1-cos(nw))/(nw*nw); |
tzxl10000 | 0:561f7672eaad | 317 | } |
tzxl10000 | 0:561f7672eaad | 318 | |
tzxl10000 | 0:561f7672eaad | 319 | // printf("\t %f \n", alpha); |
tzxl10000 | 0:561f7672eaad | 320 | // printf("\t %f \n", beta); |
tzxl10000 | 0:561f7672eaad | 321 | |
tzxl10000 | 0:561f7672eaad | 322 | matProd(W2, what, what, 3, 3, 3); |
tzxl10000 | 0:561f7672eaad | 323 | |
tzxl10000 | 0:561f7672eaad | 324 | for (int j = 0; j < 9; j++) { |
tzxl10000 | 0:561f7672eaad | 325 | II[j] = alpha*TS*what[j]; |
tzxl10000 | 0:561f7672eaad | 326 | III[j] = beta*TS*TS*W2[j]; |
tzxl10000 | 0:561f7672eaad | 327 | WW[j] = eye3[j]+II[j]+III[j]; |
tzxl10000 | 0:561f7672eaad | 328 | } |
tzxl10000 | 0:561f7672eaad | 329 | |
tzxl10000 | 0:561f7672eaad | 330 | matProd(Rest, Rold, WW, 3, 3, 3); |
tzxl10000 | 0:561f7672eaad | 331 | //matPrint(Rest,3,3); |
tzxl10000 | 0:561f7672eaad | 332 | |
tzxl10000 | 0:561f7672eaad | 333 | } |
tzxl10000 | 0:561f7672eaad | 334 | |
tzxl10000 | 0:561f7672eaad | 335 | |
tzxl10000 | 0:561f7672eaad | 336 | void getEulerAngles(float angles[3], float RotMatrix[9]) |
tzxl10000 | 0:561f7672eaad | 337 | { |
tzxl10000 | 0:561f7672eaad | 338 | float theta = -asin(RotMatrix[2]); |
tzxl10000 | 0:561f7672eaad | 339 | float phi = atan2(RotMatrix[5]/cos(theta),RotMatrix[8]/cos(theta)); |
tzxl10000 | 0:561f7672eaad | 340 | float psi = atan2(RotMatrix[1]/cos(theta),RotMatrix[0]/cos(theta)); |
tzxl10000 | 0:561f7672eaad | 341 | angles[0] = phi; |
tzxl10000 | 0:561f7672eaad | 342 | angles[1] = theta; |
tzxl10000 | 0:561f7672eaad | 343 | angles[2] = psi; |
tzxl10000 | 0:561f7672eaad | 344 | return; |
tzxl10000 | 0:561f7672eaad | 345 | } |
tzxl10000 | 0:561f7672eaad | 346 | |
tzxl10000 | 0:561f7672eaad | 347 | |
tzxl10000 | 0:561f7672eaad | 348 | // Matrix operations (matrices are stored in a one dimensional array, columnwise) |
tzxl10000 | 0:561f7672eaad | 349 | // e.g.: [1 2 3; 4 5 6; 7 8 9] = [1 4 7 2 5 8 3 6 9] |
tzxl10000 | 0:561f7672eaad | 350 | |
tzxl10000 | 0:561f7672eaad | 351 | |
tzxl10000 | 0:561f7672eaad | 352 | void matPrint(float *A, int r, int c) |
tzxl10000 | 0:561f7672eaad | 353 | |
tzxl10000 | 0:561f7672eaad | 354 | // Print the content of the matrix A which is r x c |
tzxl10000 | 0:561f7672eaad | 355 | |
tzxl10000 | 0:561f7672eaad | 356 | { |
tzxl10000 | 0:561f7672eaad | 357 | int i, j; |
tzxl10000 | 0:561f7672eaad | 358 | pc.printf("printing \n \r"); |
tzxl10000 | 0:561f7672eaad | 359 | for(i = 0; i < r; i++) { |
tzxl10000 | 0:561f7672eaad | 360 | for(j = 0; j < c; j++) { |
tzxl10000 | 0:561f7672eaad | 361 | pc.printf("%f",A[i + j*r]); |
tzxl10000 | 0:561f7672eaad | 362 | if(j == c-1) { |
tzxl10000 | 0:561f7672eaad | 363 | pc.printf("\n \r"); |
tzxl10000 | 0:561f7672eaad | 364 | } else { |
tzxl10000 | 0:561f7672eaad | 365 | pc.printf("\t"); |
tzxl10000 | 0:561f7672eaad | 366 | } |
tzxl10000 | 0:561f7672eaad | 367 | } |
tzxl10000 | 0:561f7672eaad | 368 | } |
tzxl10000 | 0:561f7672eaad | 369 | |
tzxl10000 | 0:561f7672eaad | 370 | printf("\n"); |
tzxl10000 | 0:561f7672eaad | 371 | } |
tzxl10000 | 0:561f7672eaad | 372 | |
tzxl10000 | 0:561f7672eaad | 373 | void matSum(float *C, float *A, float *B, int r, int c) |
tzxl10000 | 0:561f7672eaad | 374 | |
tzxl10000 | 0:561f7672eaad | 375 | // C = A + B, where A, B and C are r x c |
tzxl10000 | 0:561f7672eaad | 376 | |
tzxl10000 | 0:561f7672eaad | 377 | { |
tzxl10000 | 0:561f7672eaad | 378 | int i, j; |
tzxl10000 | 0:561f7672eaad | 379 | for(i = 0; i < r; i++) { |
tzxl10000 | 0:561f7672eaad | 380 | for(j = 0; j < c; j++) { |
tzxl10000 | 0:561f7672eaad | 381 | C[i + r*j] = A[i + r*j] + B[i + r*j]; |
tzxl10000 | 0:561f7672eaad | 382 | } |
tzxl10000 | 0:561f7672eaad | 383 | } |
tzxl10000 | 0:561f7672eaad | 384 | return; |
tzxl10000 | 0:561f7672eaad | 385 | } |
tzxl10000 | 0:561f7672eaad | 386 | |
tzxl10000 | 0:561f7672eaad | 387 | |
tzxl10000 | 0:561f7672eaad | 388 | void matProd(float *C, float *A, float *B, int r, int cA, int cB) |
tzxl10000 | 0:561f7672eaad | 389 | |
tzxl10000 | 0:561f7672eaad | 390 | // C = A times B, where A is r x c, B is c x c and C is r x c |
tzxl10000 | 0:561f7672eaad | 391 | |
tzxl10000 | 0:561f7672eaad | 392 | { |
tzxl10000 | 0:561f7672eaad | 393 | float sum; |
tzxl10000 | 0:561f7672eaad | 394 | int i, j, k; |
tzxl10000 | 0:561f7672eaad | 395 | |
tzxl10000 | 0:561f7672eaad | 396 | for(i = 0; i < r; i++) { |
tzxl10000 | 0:561f7672eaad | 397 | for(j = 0; j < cB; j++) { |
tzxl10000 | 0:561f7672eaad | 398 | sum = 0.0; |
tzxl10000 | 0:561f7672eaad | 399 | for(k = 0; k < cA; k++) { |
tzxl10000 | 0:561f7672eaad | 400 | sum = sum + A[i + k*r] * B[j*cA + k]; |
tzxl10000 | 0:561f7672eaad | 401 | } |
tzxl10000 | 0:561f7672eaad | 402 | C[i + r*j] = sum; |
tzxl10000 | 0:561f7672eaad | 403 | } |
tzxl10000 | 0:561f7672eaad | 404 | } |
tzxl10000 | 0:561f7672eaad | 405 | |
tzxl10000 | 0:561f7672eaad | 406 | return; |
tzxl10000 | 0:561f7672eaad | 407 | } |
tzxl10000 | 0:561f7672eaad | 408 | |
tzxl10000 | 0:561f7672eaad | 409 | |
tzxl10000 | 0:561f7672eaad | 410 | void matTimesVect(float *b, float *A, float *v, int r, int c) |
tzxl10000 | 0:561f7672eaad | 411 | |
tzxl10000 | 0:561f7672eaad | 412 | // b = A times v, where A is r x c, and v and b are c-long column vectors |
tzxl10000 | 0:561f7672eaad | 413 | |
tzxl10000 | 0:561f7672eaad | 414 | { |
tzxl10000 | 0:561f7672eaad | 415 | float sum; |
tzxl10000 | 0:561f7672eaad | 416 | int i, k; |
tzxl10000 | 0:561f7672eaad | 417 | |
tzxl10000 | 0:561f7672eaad | 418 | for(i = 0; i < r; i++) { |
tzxl10000 | 0:561f7672eaad | 419 | sum = 0.00; |
tzxl10000 | 0:561f7672eaad | 420 | for(k = 0; k < c; k++) { |
tzxl10000 | 0:561f7672eaad | 421 | sum = sum + A[i + k*r] * v[k]; |
tzxl10000 | 0:561f7672eaad | 422 | } |
tzxl10000 | 0:561f7672eaad | 423 | |
tzxl10000 | 0:561f7672eaad | 424 | b[i] = sum; |
tzxl10000 | 0:561f7672eaad | 425 | } |
tzxl10000 | 0:561f7672eaad | 426 | |
tzxl10000 | 0:561f7672eaad | 427 | return; |
tzxl10000 | 0:561f7672eaad | 428 | } |
tzxl10000 | 0:561f7672eaad | 429 | |
tzxl10000 | 0:561f7672eaad | 430 | |
tzxl10000 | 0:561f7672eaad | 431 | void transpose(float *B, float *A, int n) |
tzxl10000 | 0:561f7672eaad | 432 | |
tzxl10000 | 0:561f7672eaad | 433 | // B = A^T, where A and B are n x n matrices |
tzxl10000 | 0:561f7672eaad | 434 | |
tzxl10000 | 0:561f7672eaad | 435 | { |
tzxl10000 | 0:561f7672eaad | 436 | int i, j; |
tzxl10000 | 0:561f7672eaad | 437 | |
tzxl10000 | 0:561f7672eaad | 438 | for(i = 0; i < n; i++) { |
tzxl10000 | 0:561f7672eaad | 439 | for(j = 0; j < n; j++) { |
tzxl10000 | 0:561f7672eaad | 440 | B[n*i+j] = A[i + n*j]; |
tzxl10000 | 0:561f7672eaad | 441 | } |
tzxl10000 | 0:561f7672eaad | 442 | } |
tzxl10000 | 0:561f7672eaad | 443 | |
tzxl10000 | 0:561f7672eaad | 444 | return; |
tzxl10000 | 0:561f7672eaad | 445 | } |
tzxl10000 | 0:561f7672eaad | 446 | |
tzxl10000 | 0:561f7672eaad | 447 | |
tzxl10000 | 0:561f7672eaad | 448 | float norm(float *v, int r) |
tzxl10000 | 0:561f7672eaad | 449 | |
tzxl10000 | 0:561f7672eaad | 450 | // 2-norm of a r-long vector |
tzxl10000 | 0:561f7672eaad | 451 | |
tzxl10000 | 0:561f7672eaad | 452 | { |
tzxl10000 | 0:561f7672eaad | 453 | float n = 0; |
tzxl10000 | 0:561f7672eaad | 454 | int i; |
tzxl10000 | 0:561f7672eaad | 455 | |
tzxl10000 | 0:561f7672eaad | 456 | for(i = 0; i < r; i++) { |
tzxl10000 | 0:561f7672eaad | 457 | n = n + v[i]*v[i]; |
tzxl10000 | 0:561f7672eaad | 458 | } |
tzxl10000 | 0:561f7672eaad | 459 | |
tzxl10000 | 0:561f7672eaad | 460 | n = sqrt(n); |
tzxl10000 | 0:561f7672eaad | 461 | |
tzxl10000 | 0:561f7672eaad | 462 | return n; |
tzxl10000 | 0:561f7672eaad | 463 | } |
tzxl10000 | 0:561f7672eaad | 464 | |
tzxl10000 | 0:561f7672eaad | 465 | |
tzxl10000 | 0:561f7672eaad | 466 | void hat(float *B, float *A) |
tzxl10000 | 0:561f7672eaad | 467 | |
tzxl10000 | 0:561f7672eaad | 468 | // hat operator (for 3-D arrays only) |
tzxl10000 | 0:561f7672eaad | 469 | |
tzxl10000 | 0:561f7672eaad | 470 | { |
tzxl10000 | 0:561f7672eaad | 471 | B[0] = 0; |
tzxl10000 | 0:561f7672eaad | 472 | B[1] = A[2]; |
tzxl10000 | 0:561f7672eaad | 473 | B[2] = -A[1]; |
tzxl10000 | 0:561f7672eaad | 474 | B[3] = -A[2]; |
tzxl10000 | 0:561f7672eaad | 475 | B[4] = 0; |
tzxl10000 | 0:561f7672eaad | 476 | B[5] = A[0]; |
tzxl10000 | 0:561f7672eaad | 477 | B[6] = A[1]; |
tzxl10000 | 0:561f7672eaad | 478 | B[7] = -A[0]; |
tzxl10000 | 0:561f7672eaad | 479 | B[8] = 0; |
tzxl10000 | 0:561f7672eaad | 480 | |
tzxl10000 | 0:561f7672eaad | 481 | return; |
tzxl10000 | 0:561f7672eaad | 482 | } |
tzxl10000 | 0:561f7672eaad | 483 | |
tzxl10000 | 0:561f7672eaad | 484 | void move(int motor, float speed, int direction) |
tzxl10000 | 0:561f7672eaad | 485 | { |
tzxl10000 | 0:561f7672eaad | 486 | //Move specific motor at speed and direction |
tzxl10000 | 0:561f7672eaad | 487 | //motor: 0 for B 1 for A |
tzxl10000 | 0:561f7672eaad | 488 | //speed: 0 is off, and 1 is full speed |
tzxl10000 | 0:561f7672eaad | 489 | //direction: 0 clockwise, 1 counter-clockwise |
tzxl10000 | 0:561f7672eaad | 490 | |
tzxl10000 | 0:561f7672eaad | 491 | STBY1=1; //disable standby |
tzxl10000 | 0:561f7672eaad | 492 | STBY2=1; //disable standby |
tzxl10000 | 0:561f7672eaad | 493 | |
tzxl10000 | 0:561f7672eaad | 494 | |
tzxl10000 | 0:561f7672eaad | 495 | if(direction == 0) { |
tzxl10000 | 0:561f7672eaad | 496 | inPin1 = 0; |
tzxl10000 | 0:561f7672eaad | 497 | inPin2 = 1; |
tzxl10000 | 0:561f7672eaad | 498 | } else if(direction == 1) { |
tzxl10000 | 0:561f7672eaad | 499 | |
tzxl10000 | 0:561f7672eaad | 500 | inPin1 = 1; |
tzxl10000 | 0:561f7672eaad | 501 | inPin2 = 0; |
tzxl10000 | 0:561f7672eaad | 502 | |
tzxl10000 | 0:561f7672eaad | 503 | } |
tzxl10000 | 0:561f7672eaad | 504 | |
tzxl10000 | 0:561f7672eaad | 505 | if(motor == 1) { |
tzxl10000 | 0:561f7672eaad | 506 | AIN1=inPin1; |
tzxl10000 | 0:561f7672eaad | 507 | AIN2=inPin2; |
tzxl10000 | 0:561f7672eaad | 508 | PWMA.pulsewidth(period_pwm*speed*1.0); |
tzxl10000 | 0:561f7672eaad | 509 | } else if(motor == 0) { |
tzxl10000 | 0:561f7672eaad | 510 | BIN1=inPin1; |
tzxl10000 | 0:561f7672eaad | 511 | BIN2=inPin2; |
tzxl10000 | 0:561f7672eaad | 512 | PWMB.pulsewidth(period_pwm*speed*1.0); |
tzxl10000 | 0:561f7672eaad | 513 | } |
tzxl10000 | 0:561f7672eaad | 514 | } |
tzxl10000 | 0:561f7672eaad | 515 | |
tzxl10000 | 0:561f7672eaad | 516 | |
tzxl10000 | 0:561f7672eaad | 517 | void stop() |
tzxl10000 | 0:561f7672eaad | 518 | { |
tzxl10000 | 0:561f7672eaad | 519 | //enable standby |
tzxl10000 | 0:561f7672eaad | 520 | STBY1=0; |
tzxl10000 | 0:561f7672eaad | 521 | STBY2=0; |
tzxl10000 | 0:561f7672eaad | 522 | |
tzxl10000 | 0:561f7672eaad | 523 | } |
tzxl10000 | 0:561f7672eaad | 524 | |
tzxl10000 | 0:561f7672eaad | 525 | void moveA() |
tzxl10000 | 0:561f7672eaad | 526 | { |
tzxl10000 | 0:561f7672eaad | 527 | STBY1=1; //disable standby |
tzxl10000 | 0:561f7672eaad | 528 | int inPin1=1; |
tzxl10000 | 0:561f7672eaad | 529 | int inPin2=0; |
tzxl10000 | 0:561f7672eaad | 530 | if(directionA==0) { |
tzxl10000 | 0:561f7672eaad | 531 | inPin1 = 0; |
tzxl10000 | 0:561f7672eaad | 532 | inPin2 = 1; |
tzxl10000 | 0:561f7672eaad | 533 | } |
tzxl10000 | 0:561f7672eaad | 534 | AIN1=inPin1; |
tzxl10000 | 0:561f7672eaad | 535 | AIN2=inPin2; |
tzxl10000 | 0:561f7672eaad | 536 | directionA=!directionA; |
tzxl10000 | 0:561f7672eaad | 537 | |
tzxl10000 | 0:561f7672eaad | 538 | //pc.printf("dirA = %d\n\r",directionA); |
tzxl10000 | 0:561f7672eaad | 539 | |
tzxl10000 | 0:561f7672eaad | 540 | |
tzxl10000 | 0:561f7672eaad | 541 | |
tzxl10000 | 0:561f7672eaad | 542 | PWMA.pulsewidth(period_pwm*speedA); |
tzxl10000 | 0:561f7672eaad | 543 | |
tzxl10000 | 0:561f7672eaad | 544 | } |
tzxl10000 | 0:561f7672eaad | 545 | void moveB() |
tzxl10000 | 0:561f7672eaad | 546 | { |
tzxl10000 | 0:561f7672eaad | 547 | STBY1=1; //disable standby |
tzxl10000 | 0:561f7672eaad | 548 | int inPin1=0; |
tzxl10000 | 0:561f7672eaad | 549 | int inPin2=1; |
tzxl10000 | 0:561f7672eaad | 550 | if(directionB==0) { |
tzxl10000 | 0:561f7672eaad | 551 | inPin1 = 1; |
tzxl10000 | 0:561f7672eaad | 552 | inPin2 = 0; |
tzxl10000 | 0:561f7672eaad | 553 | } |
tzxl10000 | 0:561f7672eaad | 554 | //pc.printf("dirB = %d\n\r",directionB); |
tzxl10000 | 0:561f7672eaad | 555 | BIN1=inPin1; |
tzxl10000 | 0:561f7672eaad | 556 | BIN2=inPin2; |
tzxl10000 | 0:561f7672eaad | 557 | directionB=!directionB; |
tzxl10000 | 0:561f7672eaad | 558 | |
tzxl10000 | 0:561f7672eaad | 559 | PWMB.pulsewidth(period_pwm*speedB); |
tzxl10000 | 0:561f7672eaad | 560 | |
tzxl10000 | 0:561f7672eaad | 561 | } |
tzxl10000 | 0:561f7672eaad | 562 | |
tzxl10000 | 0:561f7672eaad | 563 | void moveC() |
tzxl10000 | 0:561f7672eaad | 564 | { |
tzxl10000 | 0:561f7672eaad | 565 | STBY1=1; //disable standby |
tzxl10000 | 0:561f7672eaad | 566 | int inPin1=0; |
tzxl10000 | 0:561f7672eaad | 567 | int inPin2=1; |
tzxl10000 | 0:561f7672eaad | 568 | if(directionC==0) { |
tzxl10000 | 0:561f7672eaad | 569 | inPin1 = 1; |
tzxl10000 | 0:561f7672eaad | 570 | inPin2 = 0; |
tzxl10000 | 0:561f7672eaad | 571 | } |
tzxl10000 | 0:561f7672eaad | 572 | //pc.printf("dirC = %d\n\r",directionC); |
tzxl10000 | 0:561f7672eaad | 573 | CIN1=inPin1; |
tzxl10000 | 0:561f7672eaad | 574 | CIN2=inPin2; |
tzxl10000 | 0:561f7672eaad | 575 | directionC=!directionC; |
tzxl10000 | 0:561f7672eaad | 576 | |
tzxl10000 | 0:561f7672eaad | 577 | PWMC.pulsewidth(period_pwm*speedC); |
tzxl10000 | 0:561f7672eaad | 578 | |
tzxl10000 | 0:561f7672eaad | 579 | } |