SDI
/
prova_comunicazione_v2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 5:362e9455a952, committed 2017-01-15
- Comitter:
- Riccardo1994
- Date:
- Sun Jan 15 20:58:50 2017 +0000
- Parent:
- 4:217f645be612
- Child:
- 6:d282940588fc
- Commit message:
- Rev
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Jan 03 11:05:21 2017 +0000
+++ b/main.cpp Sun Jan 15 20:58:50 2017 +0000
@@ -14,34 +14,49 @@
//Oggetti per comunicazione seriale, i2c e timer
I2C bus_i2c(I2C_SDA, I2C_SCL);
Serial pc(SERIAL_TX, SERIAL_RX);
-//Ticker timer; //Timer da impostare all'occorrenza per campionare i valori restituiti dall'accelerometro
+
//Modalità scrittura o lettura scheda sensori
int LSM6DS0_WRITE = 0xD6;
int LSM6DS0_READ = 0xD7;
-int ax_tmp,ay_tmp,az_tmp,vx_tmp,vy_tmp,vz_tmp,va_conc,vb_conc,vg_conc,ax_conc,ay_conc,az_conc,tmpgx,tmpgy,tmpgz,va_tar,vb_tar,vg_tar,ax_grav,ay_grav,az_grav;
+int ax_tmp,ay_tmp,az_tmp,ax_conc1,ay_conc1,az_conc1,ax_conc2,ay_conc2,az_conc2,ax_conc3,ay_conc3,az_conc3,tmpgx,tmpgy,tmpgz,ax_grav,ay_grav,az_grav;
+int ax_conc,ay_conc,az_conc;
+int t1,t2;
-double va1,x_pos,tetax=0,s_tetax,ax0,tmpx;
-double vb1,y_pos,tetay=0,s_tetay,ay0,tmpy;
-double vd1,z_pos,tetaz=-45,s_tetaz,az0,tmpz;
+double x_pos,ax0,tmpx;
+double y_pos,ay0,tmpy;
+double z_pos,az0,tmpz;
char x_hl[] = {LSM6DS0_XG_OUT_X_H_XL , LSM6DS0_XG_OUT_X_L_XL};
char y_hl[] = {LSM6DS0_XG_OUT_Y_H_XL , LSM6DS0_XG_OUT_Y_L_XL};
char z_hl[] = {LSM6DS0_XG_OUT_Z_H_XL , LSM6DS0_XG_OUT_Z_L_XL};
-char a_hl[] = {LSM6DS0_XG_OUT_X_H_G , LSM6DS0_XG_OUT_X_L_G};
-char b_hl[] = {LSM6DS0_XG_OUT_Y_H_G , LSM6DS0_XG_OUT_Y_L_G};
-char g_hl[] = {LSM6DS0_XG_OUT_Z_H_G , LSM6DS0_XG_OUT_Z_L_G};
+//char a_hl[] = {LSM6DS0_XG_OUT_X_H_G , LSM6DS0_XG_OUT_X_L_G};
+//char b_hl[] = {LSM6DS0_XG_OUT_Y_H_G , LSM6DS0_XG_OUT_Y_L_G};
+//char g_hl[] = {LSM6DS0_XG_OUT_Z_H_G , LSM6DS0_XG_OUT_Z_L_G};
+
+//METODO CAVALIERI-SIMPSON
+char ax1[2],ax2[2],ax3[2];
+char ay1[2],ay2[2],ay3[2];
+char az1[2],az2[2],az3[2];
+
+int vx1;
+int vy1;
+int vz1;
//METODO CON FORMULA x = 1/2*at^2
char ax[2];
char ay[2];
char az[2];
-char va[2];
-char vb[2];
-char vd[2];
+//char va[2];
+//char vb[2];
+//char vd[2];
//Dichiarazione funzioni
-
+char start_bits[] = "0xFFFF0000";
+char stop_bits[] = "0x0000FFFF";
+char x_deg[] = "0x0000";
+char y_deg[] = "0x0000";
+char z_deg[] = "0x0000";
void write(char REG_ADDR, char REG_DATA);
void read(const char* REG_ADDR, char* buffer);
@@ -50,40 +65,42 @@
FUNZIONI CON FORMULA x = 1/2*at^2
*******************************************************************************/
+void taratura();
void xl_read();
-void gy_read();
-void taratura();
-void degrees();
void positions();
-//void callEverything();
+void Cavalieri_Simpson();
+//void gy_read();
+//void degrees();
+
+
int main() {
-
+
//Configurazione Registri
char xl_reg = 0x20;
- char xl_config = 0xD0; //Attivo accelerometro con ODR 952Hz, +-4g FS
- char gy_reg = 0x10;
- char gy_config = 0xD8; //Attivo giroscopio con ODR 952Hz, 2000 dps FS
+ char xl_config = 0xA0; //Attivo accelerometro con ODR 476Hz, +-2g FS
+ //char gy_reg = 0x10;
+ //char gy_config = 0xA0;
bus_i2c.frequency(100000);
write(xl_reg,xl_config); //Attivo accelerometro con ODR 952Hz, +-4g FS
- write(gy_reg,gy_config); //Attivo giroscopio con ODR 952Hz, 2000 dps FS
+ //write(gy_reg,gy_config);
wait(1);
taratura(); //Ricavo le componenti di gravità in postazione normale
pc.printf("Iniziano le misure\n\n");
-
- for(int i = 0;i<1000;i++){
- wait(0.05);
- xl_read();
- gy_read();
- degrees();
- positions();
-
- pc.printf("%d\n", va_conc-va_tar);
- }
+
+ while(1){
+
+ wait(0.05);
+ xl_read();
+ positions();
+
+ //Comunicazione seriale nella forma FFFF0000 - X - Y - Z - alfa - beta - gamma - 0000FFFF -> totale 14 Bytes
+ pc.printf("%c%g%g%g%g%g%g%c", start_bits,x_pos,y_pos,z_pos,x_deg,y_deg,z_deg,stop_bits);
+ }
}
@@ -103,31 +120,31 @@
bus_i2c.write(LSM6DS0_WRITE,REG_ADDR,1,true);
bus_i2c.read(LSM6DS0_READ,buffer,1,false);
}
-
+
/*******************************************************************************
METODO CON FORMULA x = 1/2*at^2 -> MANCA COMPENSAZIONE
*******************************************************************************/
void taratura(){
- for(int j=0;j<500;j++){
+ for(int j=0;j<100;j++){
wait(0.01);
- gy_read();
+ //gy_read();
xl_read();
tmpx += ax_conc;
tmpy += ay_conc;
tmpz += az_conc;
- tmpgx += va_conc;
- tmpgy += vb_conc;
- tmpgz += vg_conc;
+ //tmpgx += va_conc;
+ //tmpgy += vb_conc;
+ //tmpgz += vg_conc;
}
- ax_grav = tmpx/500;
- ay_grav = tmpy/500;
- az_grav = tmpz/500;
+ ax_grav = tmpx/100;
+ ay_grav = tmpy/100;
+ az_grav = tmpz/100;
- va_tar = tmpgx/500;
- vb_tar = tmpgy/500;
- vg_tar = tmpgz/500;
+ //va_tar = tmpgx/500;
+ //vb_tar = tmpgy/500;
+ //vg_tar = tmpgz/500;
}
@@ -155,7 +172,105 @@
}
-void gy_read(){
+
+
+//POSITION DEVE ESSERE COMPENSATA CON GLI ANGOLI
+void positions(){
+ //ax_conc = ax_conc + az_grav*cos((tetay*pi/180)-(pi/2)) + ax_grav*cos((tetay*pi/180)-(pi/2)) - ay_grav*sin((tetaz*pi/180)-pi) + ax_grav*cos((tetaz*pi/180)-pi);
+ //ay_conc = ay_conc + az_grav*sin((tetax*pi/180)-pi) - ay_grav*cos((tetax*pi/180)-pi) - ay_grav*cos((tetaz*pi/180)-pi) + ax_grav*sin((tetaz*pi/180)-pi);
+ //az_conc = az_conc + az_grav*cos((tetax*pi/180)-pi) - ay_grav*sin((tetax*pi/180)-pi) + az_grav*sin((tetay*pi/180)) + ax_grav*sin((tetay*pi/180));
+
+ x_pos += 2*vx1*0.05/32767 + ax_conc*0.025/32767; //Capire bene che accelerazione inserire
+ y_pos += 2*vy1*0.05/32767 + ay_conc*0.025/32767; //Capire bene che accelerazione inserire
+ z_pos += 2*vz1*0.05/32767 + az_conc*0.025/32767; //Capire bene che accelerazione inserire
+
+}
+
+
+/*******************************************************************************
+METODO CAVALIERI-SIMPSON
+*******************************************************************************/
+
+void Cavalieri_Simpson(){
+
+ //X(A),Y(A),Z(A)
+ read(&x_hl[1],&ax1[1]); //prima basso
+ read(&x_hl[0],&ax1[0]); //poi alto
+ read(&y_hl[1],&ay1[1]); //prima basso
+ read(&y_hl[0],&ay1[0]); //poi alto
+ read(&z_hl[1],&az1[1]); //prima basso
+ read(&z_hl[0],&az1[0]); //poi alto
+ wait(0.0005);
+
+ //X(A+B/2),Y(A+B/2),Z(A+B/2)
+ read(&x_hl[1],&ax2[1]); //prima basso
+ read(&x_hl[0],&ax2[0]); //poi alto
+ read(&y_hl[1],&ay2[1]); //prima basso
+ read(&y_hl[0],&ay2[0]); //poi alto
+ read(&z_hl[1],&az2[1]); //prima basso
+ read(&z_hl[0],&az2[0]); //poi alto
+ wait(0.0005);
+
+ //X(B),Y(B),Z(B)
+ read(&x_hl[1],&ax3[1]); //prima basso
+ read(&x_hl[0],&ax3[0]); //poi alto
+ read(&y_hl[1],&ay3[1]); //prima basso
+ read(&y_hl[0],&ay3[0]); //poi alto
+ read(&z_hl[1],&az3[1]); //prima basso
+ read(&z_hl[0],&az3[0]); //poi alto
+
+ //Concatenazione prime tre accelerazioni
+ ax_conc1 = *(signed char *)(&ax1[0]);
+ ax_conc1 *= 1 << CHAR_BIT;
+ ax_conc1 |= ax1[1];
+
+ ay_conc1 = *(signed char *)(&ay1[0]);
+ ay_conc1 *= 1 << CHAR_BIT;
+ ay_conc1 |= ay1[1];
+
+ az_conc1 = *(signed char *)(&az1[0]);
+ az_conc1 *= 1 << CHAR_BIT;
+ az_conc1 |= az1[1];
+
+ //Concatenazione prime tre accelerazioni
+ ax_conc2 = *(signed char *)(&ax2[0]);
+ ax_conc2 *= 1 << CHAR_BIT;
+ ax_conc2 |= ax2[1];
+
+ ay_conc2 = *(signed char *)(&ay2[0]);
+ ay_conc2 *= 1 << CHAR_BIT;
+ ay_conc2 |= ay2[1];
+
+ az_conc2 = *(signed char *)(&az2[0]);
+ az_conc2 *= 1 << CHAR_BIT;
+ az_conc2 |= az2[1];
+
+ //Concatenazione prime tre accelerazioni
+ ax_conc3 = *(signed char *)(&ax3[0]);
+ ax_conc3 *= 1 << CHAR_BIT;
+ ax_conc3 |= ax3[1];
+
+ ay_conc3 = *(signed char *)(&ay3[0]);
+ ay_conc3 *= 1 << CHAR_BIT;
+ ay_conc3 |= ay3[1];
+
+ az_conc3 = *(signed char *)(&az3[0]);
+ az_conc3 *= 1 << CHAR_BIT;
+ az_conc3 |= az3[1];
+
+ //Calcolo delle velocità
+ vx1 = (0.001/6)*(ax_conc1 + 4*ax_conc2 + ax_conc3);
+ vy1 = (0.001/6)*(ay_conc1 + 4*ay_conc2 + ay_conc3);
+ vz1 = (0.001/6)*(az_conc1 + 4*az_conc2 + az_conc3);
+}
+
+
+
+
+
+
+
+ /*void gy_read(){
read(&a_hl[1],&va[1]); //prima basso
read(&b_hl[1],&vb[1]);
@@ -181,42 +296,15 @@
void degrees(){
- s_tetax = 2000*(va_conc-va_tar)*0.05/32768;
- s_tetay = 2000*(vb_conc-vb_tar)*0.05/32768;
- s_tetaz = 2000*(vg_conc-vg_tar)*0.05/32768;
- //s_tetax = 2000*((va_conc-va_tar) + (vb_conc-vb_tar)*sin(tetax*pi/180)*tan(tetay*pi/180) + (vg_conc-vg_tar)*cos(tetax*pi/180)*tan(tetay*pi/180))*0.01/32768; //Spostamento ROLL(X)
- //s_tetay = 2000*((vb_conc-vb_tar)*cos(tetax*pi/180) - (vg_conc-vg_tar)*sin(tetax*pi/180))*0.01/32768; //Spostamento PITCH(Y)
- //s_tetaz = 2000*((vb_conc-vb_tar)*(sin(tetax*pi/180)/cos(tetay*pi/180)) + (vg_conc-vg_tar)*(cos(tetax*pi/180)/cos(tetay*pi/180)))*0.01/32768; //Spostamento YAW(Z)
-
- tetax += s_tetax; //phi
- tetay += s_tetay; //teta
- tetaz += s_tetaz; //psi
-}
-
-
-//POSITION DEVE ESSERE COMPENSATA CON GLI ANGOLI
-void positions(){
- ax_conc = ax_conc + az_grav*cos((tetay*pi/180)-(pi/2)) + ax_grav*cos((tetay*pi/180)-(pi/2)) - ay_grav*sin((tetaz*pi/180)-pi) + ax_grav*cos((tetaz*pi/180)-pi);
- ay_conc = ay_conc + az_grav*sin((tetax*pi/180)-pi) - ay_grav*cos((tetax*pi/180)-pi) - ay_grav*cos((tetaz*pi/180)-pi) + ax_grav*sin((tetaz*pi/180)-pi);
- az_conc = az_conc + az_grav*cos((tetax*pi/180)-pi) - ay_grav*sin((tetax*pi/180)-pi) + az_grav*sin((tetay*pi/180)) + ax_grav*sin((tetay*pi/180));
+ s_tetax = 1.4*245*(va_conc-va_tar)*0.05/32767;
+ s_tetay = 1.4*245*(vb_conc-vb_tar)*0.05/32767;
+ s_tetaz = 1.4*245*(vg_conc-vg_tar)*0.05/32767;
- //x_pos += ax1*0.5/32768;
- //y_pos += ay1*0.5/32768;
- //z_pos += az1*0.5/32768;
- }
-
-
-/*******************************************************************************
-PARTE RELATIVA AL GIROSCOPIO
-*******************************************************************************/
-
-/*
- -000XY0AB Power Down
- -001XY0AB ODR 14.9Hz, XY Gyro full scale, AB Gyro bandwidth (Low Power)
- -010XY0AB ODR 59.9Hz, XY Gyro full scale, AB Gyro bandwidth (Low Power)
- -011XY0AB ODR 119 Hz, XY Gyro full scale, AB Gyro bandwidth (Low Power)
- -100XY0AB ODR 238 Hz, XY Gyro full scale, AB Gyro bandwidth (Normal mode)
- -101XY0AB ODR 476 Hz, XY Gyro full scale, AB Gyro bandwidth (Normal mode)
- -110XY0AB ODR 952 Hz, XY Gyro full scale, AB Gyro bandwidth (Normal mode)
- -111XY0AB ODR n.a.
-*/
\ No newline at end of file
+ if(s_tetax < - 0.05 || s_tetax > 0.05)
+ tetax += s_tetax; //Nonostante la taratura (fatta su una media di 500 campioni) si nota un offset di 1.4 circa (in codice) che causa una deriva MAX di +-0.6 gradi ogni 25 secondi da ferma
+ if(s_tetay < - 0.05 || s_tetay > 0.05)
+ tetay += s_tetay; //Nonostante la taratura (fatta su una media di 500 campioni) si nota un offset di 1.15 circa (in codice) che causa una deriva MAX di +-0.4 gradi ogni 25 secondi da ferma
+ if(s_tetaz < - 0.05 || s_tetaz > 0.05)
+ tetaz += s_tetaz; //Nonostante la taratura (fatta su una media di 500 campioni) si nota un offset di -0.24 circa (in codice) che causa una deriva MAX di +-0.3 gradi ogni 25 secondi da ferma
+}
+*/
\ No newline at end of file