Diff: main.cpp

Revision:

0:a2296270a125

Child:

2:952d41c26b43

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 21 17:56:46 2020 +0000
@@ -0,0 +1,133 @@
+#include "mbed.h"
+#include "LSM9DS1.h"
+
+
+
+#include "TB6549.h"
+
+#define dt 0.01     // pas d'integration
+
+DigitalOut Led0(LED1);
+
+Serial pc(SERIAL_TX, SERIAL_RX,115200);
+Ticker calc;
+Ticker affich;
+LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
+
+
+AnalogIn verin(PC_3);  // lecture pos verin
+AnalogOut ana (PA_5);   // pour debug analogique ISR
+Motor motor(PB_4,PA_1,PA_4,PC_7);
+int flag_affich=0;
+int flag_imu=0;
+double pi= 3.1415926535897932;
+double gx_off=0,gy_off=0,gz_off=0;
+double ang_off=0;
+
+
+//void gyro_zero(void);
+//void angle_zero(void);
+
+void calcule()
+{
+    flag_imu=1;
+}
+void affiche()
+{
+    flag_affich=1;
+}
+
+int main()
+{
+
+    double ang;
+    // fitres complémentaires
+    double Fc=0.05;
+    double RC=1./(Fc*2*pi);            //calcul de RC
+    double a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
+    double b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
+    double a1=a0*RC*1.0;           //calcul du coefficient a du filtre
+
+    double angle_acce_pred=0.0f;
+    double angle_acce=0.0f;
+    double angle_acce_f_pred=0.0f;
+    double angle_acce_f=0.0f;
+    //
+    double gyroy_pred=0.0f;
+    double gyroy=0.0f;
+    double angle_gyroy_f_pred=0.0f;
+    double angle_gyroy_f=0.0f;
+    //
+    double angle_final;
+    wait(1);
+    //DOF.calibration();
+    DOF.begin();
+    wait(1);
+    DOF.calibration();
+    wait(1);
+    calc.attach(&calcule,dt);
+    affich.attach(&affiche,0.1);
+    while(1) {
+        if(flag_imu) {
+            ana=0.6;
+            DOF.readAccel();
+            DOF.readGyro();
+            ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off);       // sur table
+            //ang=((180/pi)*atan2((double)DOF.ax,(double)DOF.ay)-ang_off);  // sur site
+
+            // filtres complémentaires
+            angle_acce_pred = angle_acce;
+            angle_acce=ang;
+            angle_acce_f_pred = angle_acce_f;
+            angle_acce_f=a0*angle_acce+a0*angle_acce_pred-b0*angle_acce_f_pred;      //filtrage accéléromètre
+
+            gyroy_pred = gyroy;
+            gyroy=-DOF.gx-gx_off;  //sur table
+            // gyroy=-DOF.gz-gz_off;  //sur site
+            angle_gyroy_f_pred = angle_gyroy_f;
+            angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred;
+            //
+            angle_final=angle_acce_f+angle_gyroy_f;
+            //
+            //
+            flag_imu=0;
+            ana=0.0;
+        }
+        if(flag_affich) {
+            ana=0.3;
+            //moteur
+            float x=verin.read();
+            float s=0.0;
+            if((angle_final>2)&&(x>0.1)) s=0.4;
+            else if((angle_final<-2)&&(x<0.9)) s=-0.4;
+            else motor.speed(0.0);
+            motor.speed(s);
+            pc.printf("$%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f;\r\n",ang,angle_acce_f,angle_gyroy_f,angle_final,gyroy,x,s);
+            ana=0.0;
+            flag_affich=0;
+        }
+    }
+}
+
+void gyro_zero(void)
+{
+    const int NN=1000;
+    //float GyroBuffer[3];
+    for(int i=0; i<NN; i++) {
+        DOF.readGyro();
+        gx_off=gx_off+DOF.gx/(NN);
+        gy_off=gy_off+DOF.gy/(NN);
+        gz_off=gz_off+DOF.gz/(NN);
+    }
+}
+void angle_zero(void)
+{
+    const int NN=1000;
+    //int16_t AccBuffer[3];
+    for(int i=0; i<NN; i++) {
+        DOF.readAccel();
+        double ang=(180/pi)*atan2((double)DOF.ay,(double)DOF.az);  // sur table
+        //double ang=(180/pi)*atan2((double)DOF.ax,(double)DOF.ay);  // sur site
+        ang_off=ang_off+ang/NN;
+    }
+}
\ No newline at end of file