Alejandro Giraldo Martinez
.
Dependencies: BNO055 SDFileSystem mbed
Diff: main.cpp
- Revision:
- 9:b5f2b626bff8
- Parent:
- 8:67c4e64bccb6
- Child:
- 11:4058bd1b41bb
--- a/main.cpp Thu Nov 08 03:22:44 2018 +0000
+++ b/main.cpp Tue Nov 13 02:40:50 2018 +0000
@@ -10,7 +10,8 @@
#define TS 0.01
using namespace std;
-int cont=0;
+int count=0;//cuenta hasta 4, si vel < 0.03 entonces hace vel = 0
+int count_interrupt = 0;
SDFileSystem sd(MOSI, MISO, SCK, CS, "sd");
DigitalOut led1(LED1);
@@ -25,11 +26,13 @@
Serial xbee(p28, p27);//p28, p27,
BNO055 bno055(p9, p10);
InterruptIn interrupt(p15);
+InterruptIn b1(p16);
-bool flagi = false, flags=false;
+bool flagi = false;
+int flag_calib = 0;
float acel_x, acel_y, acel_z,acel_x_ant,acel_y_ant,acel_z_ant, muestra_x[300], muestra_y[300], muestra_z[300];
float suma_x = 0, suma_y = 0, suma_z = 0, mean_x = 0, mean_y = 0, mean_z = 0;
-int conteo = 0, muestras = 0;
+int muestras = 0;
float vel_x=0, vel_y=0, vel_z=0;
float vel_x_act=0, vel_y_act=0, vel_z_act=0;
@@ -40,89 +43,92 @@
void event();
void tempo();
+void calibracion();
int main()
{
//float acel_x_act, acel_y_act, acel_z_act;
led4 = 0;
-
- mkdir("/sd/mydir", 0777);
- FILE *fp = fopen("/sd/datos.csv", "a+");
- if(fp == NULL) {
- //pc.printf("Could not open file for write\n");
- xbee.printf("Could not open file for write\n");
- }
led3=0;
green=0;
blue=0;
red=1;
bno055.reset();
interrupt.rise(&event);
+ b1.rise(&calibracion);
bno055.setmode(OPERATION_MODE_NDOF);
//bno055.write_calibration_data();
bno055.get_calib();
//bno055.write_calibration_data();
-
- while(((bno055.calib>>4)&3)!= 3) {
- red=1;
- blue=0;
- green=0;
- bno055.get_calib();
- wait_ms(50);
- red = 0;
- wait_ms(50);
- //pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
- xbee.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
- }
- //pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
- xbee.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
- wait(2);
+
+ xbee.printf("Para calibrar pulse B1, de lo contrario espere 5 segundos\n");
+ wait(7);
+ if (flag_calib == 1) {
+ while(((bno055.calib>>4)&3)!= 3) {
+ red=1;
+ blue=0;
+ green=0;
+ bno055.get_calib();
+ wait_ms(50);
+ red = 0;
+ wait_ms(50);
+ //pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
+ xbee.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
+ }
+ //pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
+ xbee.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
+ wait(2);
- red=0;
- //bno055.write_calibration_data();
- bno055.get_calib();
-
- while((bno055.calib & 3) != 3) {
red=0;
- blue=1;
- green=0;
//bno055.write_calibration_data();
bno055.get_calib();
- wait_ms(50);
- blue = 0;
- wait_ms(50);
- //pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
- xbee.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
- }
- //pc.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
- xbee.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
- wait(2);
+ while((bno055.calib & 3) != 3) {
+ red=0;
+ blue=1;
+ green=0;
+ //bno055.write_calibration_data();
+ bno055.get_calib();
+ wait_ms(50);
+ blue = 0;
+ wait_ms(50);
+ //pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
+ xbee.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
+ }
- blue=0;
- //bno055.write_calibration_data();
- bno055.get_calib();
- while(((bno055.calib>>2)& 3 )!= 3) {
- red=0;
+ //pc.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
+ xbee.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
+ wait(2);
+
blue=0;
- green=1;
//bno055.write_calibration_data();
bno055.get_calib();
- wait_ms(2000);
- green = 0;
- wait_ms(2000);
- //pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
- xbee.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
+ while(((bno055.calib>>2)& 3 )!= 3) {
+ red=0;
+ blue=0;
+ green=1;
+ //bno055.write_calibration_data();
+ bno055.get_calib();
+ wait_ms(2000);
+ green = 0;
+ wait_ms(2000);
+ //pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
+ xbee.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
+ }
+
+
+ //pc.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
+ xbee.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
+ //pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
+ xbee.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
+ wait(1.5);
+ green=1;
+
}
- //pc.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
- xbee.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
- //pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
- xbee.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
- wait(1.5);
- green=1;
+ flag_calib = 0;
if (bno055.check()) {
bno055.initIntr();
@@ -139,42 +145,43 @@
suma_y = suma_y + muestra_y[p];
suma_z = suma_z + muestra_z[p];
}
-
+
mean_x = suma_x/300.0;
mean_y = suma_y/300.0;
mean_z = suma_z/300.0;
-
- //pc.printf("PROMEDIO DE ACELERACION EN X = %f\n", mean_x);
- //pc.printf("PROMEDIO DE ACELERACION EN y = %f\n", mean_y);
- //pc.printf("PROMEDIO DE ACELERACION EN z = %f\n", mean_z);
-
+
xbee.printf("PROMEDIO DE ACELERACION EN X = %f\n", mean_x);
xbee.printf("PROMEDIO DE ACELERACION EN y = %f\n", mean_y);
xbee.printf("PROMEDIO DE ACELERACION EN z = %f\n", mean_z);
+
wait_ms(1500);
-
- while(1) {
+
+ mkdir("/sd/mydir", 0777);
+ FILE *fp = fopen("/sd/datos.csv", "a+");
+ if(fp == NULL) {
+ //pc.printf("Could not open file for write\n");
+ xbee.printf("Could not open file for write\n");
+ }
- /*
- bno055.get_angles(); //query the i2c device
- pc.printf("yaw:%6.2f pitch:%6.2f roll:%6.2f\r\n",bno055.euler.yaw, bno055.euler.pitch, bno055.euler.roll);
- bno055.get_lia(); //query the i2c device
- pc.printf("X: %3.2f, Y: %3.2f, Z: %3.2f\r\n",bno055.lia.x,bno055.lia.y,bno055.lia.z);
- wait_ms(TS);
- */
- if(flagi==true && flags==true) {
- led1 = 1;
- vel_x_act = vel_x + TS * ((acel_x-mean_x)+acel_x_ant)/2.0;
- vel_y_act = vel_y + TS * ((acel_y-mean_y)+acel_y_ant)/2.0;
- vel_z_act = vel_z + TS * ((acel_z-mean_z)+acel_y_ant)/2.0;
+ while(1) {
+ if (count_interrupt > 0) {
+ xbee.printf("FIN DE ESCRITURA EN SD\n");
+ fclose(fp);
+ count_interrupt = 0;
+ }
- acel_x_ant = acel_x-mean_x;
- acel_y_ant = acel_y-mean_y;
- acel_z_ant = acel_z-mean_z;
+ if(flagi==true) {
+ vel_x_act = vel_x + TS * ( ( acel_x + acel_x_ant ) ) / 2.0;
+ vel_y_act = vel_y + TS * ( ( acel_y + acel_y_ant ) ) / 2.0;
+ vel_z_act = vel_z + TS * ( ( acel_z + acel_y_ant ) ) / 2.0;
- pos_x_act = pos_x + TS * (vel_x_act+vel_x)/2.0;
- pos_y_act = pos_y + TS * (vel_y_act+vel_y)/2.0;
- pos_z_act = pos_z + TS * (vel_z_act+vel_z)/2.0;
+ acel_x_ant = acel_x;
+ acel_y_ant = acel_y;
+ acel_z_ant = acel_z;
+
+ pos_x_act = pos_x + TS * ( ( vel_x_act + vel_x ) ) / 2.0;
+ pos_y_act = pos_y + TS * ( ( vel_y_act + vel_y ) ) / 2.0;
+ pos_z_act = pos_z + TS * ( ( vel_z_act + vel_z ) ) / 2.0;
vel_x = vel_x_act;
vel_y = vel_y_act;
@@ -183,44 +190,43 @@
pos_x = pos_x_act;
pos_y = pos_y_act;
pos_z = pos_z_act;
- }
-
- if(flags == true) {
- cont++;
+ if (flag_calib == 1){
fprintf(fp,"%f %f %f\n",pos_x_act, pos_y_act, pos_z_act);
- if (cont == 6000) {
- //pc.printf("FIN DE ESCRITURA EN SD\n");
- xbee.printf("FIN DE ESCRITURA EN SD\n");
- fclose(fp);
-
+ wait_ms(5);
}
- flags=false;
+
}
- //bno055.resetIntr();
+
if(us_ticker_read()>(tant+1000000)) {
tant=us_ticker_read();
- //pc.printf("ACELERACION X=%2.4f m/s^2 Y=%2.4f m/s^2 Z=%2.4f m/s^2 \n", (acel_x-mean_x), (acel_y-mean_y), (acel_z-mean_z));
- //pc.printf("VELOCIDAD X=%2.4f m/s Y=%2.4f m/s Z=%2.4f m/s \n", vel_x_act, vel_y_act, vel_z_act);
- //pc.printf("POSICION X=%2.4f m Y=%2.4f m Z=%2.4f m \n\n\n\n",pos_x_act, pos_y_act, pos_z_act);
-
- xbee.printf("ACELERACION X=%2.4f m/s^2 Y=%2.4f m/s^2 Z=%2.4f m/s^2 \n", (acel_x-mean_x), (acel_y-mean_y), (acel_z-mean_z));
+ xbee.printf("ACELERACION X=%2.4f m/s^2 Y=%2.4f m/s^2 Z=%2.4f m/s^2 \n", (acel_x), (acel_y), (acel_z));
xbee.printf("VELOCIDAD X=%2.4f m/s Y=%2.4f m/s Z=%2.4f m/s \n", vel_x_act, vel_y_act, vel_z_act);
- xbee.printf("POSICION X=%2.4f m Y=%2.4f m Z=%2.4f m \n\n\n\n",pos_x_act, pos_y_act, pos_z_act);
-
+ xbee.printf("POSICION X=%2.4f m Y=%2.4f m Z=%2.4f m \n\n\n\n",pos_x_act, pos_y_act, pos_z_act);
}
+
+
+ }//end while(1)
+}//end main
+
+
+void calibracion(){
+ flag_calib++;
+ wait_ms(10);
}
-}
+
+
void event()
{
- led4 = 1;
-//Interrupcion de IMU
+ led4=!led4;;
+ count_interrupt++;
}
void tempo()
{
+ //1
bno055.get_lia(); //query the i2c device
acel_x = bno055.lia.x;
acel_y = bno055.lia.y;
@@ -230,35 +236,30 @@
muestra_y[muestras] = acel_y;
muestra_z[muestras] = acel_z;
muestras++;
+ flagi = false;
} else {
- if(fabs(acel_x)<0.01 && fabs(acel_y)<0.01 && fabs(acel_z)<0.01) {
- conteo++;
- if (conteo>=4) {
- flagi=false;
- led1=0;
- bno055.resetIntr();
- acel_x=0;
- acel_y=0;
- acel_z=0;
- vel_x=0;
- vel_y=0;
- vel_z=0;
- vel_x_act=0;
- vel_y_act=0;
- vel_z_act=0;
-
+ flagi = true;
+ if ( fabs(vel_x_act) < 1 && fabs(vel_y_act) < 1 && fabs(vel_z_act) < 1 && fabs(acel_x) < 0.03 && fabs(acel_y)<0.03 && fabs(acel_z)<0.03 ) {
+ count++;
+ if ( count >= 2) {
+ led1 = 0;
+ vel_x_act = 0;
+ vel_y_act = 0;
+ vel_z_act = 0;
+ vel_x = 0;
+ vel_y = 0;
+ vel_z = 0;
acel_x_ant = 0;
acel_y_ant = 0;
acel_z_ant = 0;
+ acel_x = 0;
+ acel_y = 0;
+ acel_z = 0;
}
} else {
- conteo=0;
- flagi=true;
+ led1=1;
+ count = 0;
}
+
}
- flags=true;
-}
-
-
-
-
+}
\ No newline at end of file
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 13 Nov 2018 |
| Imports: | 3 |
| Forks: | 0 |
| Commits: | 12 |
| Dependents: | 0 |
| Dependencies: | 3 |
| Followers: | 1 |
| Issues: | 0 |
The code in this repository is MIT licensed.