Zhan Tu / Mbed 2 deprecated fish_control

fish

Dependencies:   mbed

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?

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