Raynaud Gilles / Mbed OS VITI_motor_angle_3

angle3

Dependencies:   LSM9DS1 TB5649

main.cpp@0:a2296270a125, 2020-05-21 (annotated)

Committer:
gr66
Date:
Thu May 21 17:56:46 2020 +0000
Revision:
0:a2296270a125
Child:
2:952d41c26b43
v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gr66 0:a2296270a125 1 #include "mbed.h"
gr66 0:a2296270a125 2 #include "LSM9DS1.h"
gr66 0:a2296270a125 3
gr66 0:a2296270a125 4
gr66 0:a2296270a125 5
gr66 0:a2296270a125 6 #include "TB6549.h"
gr66 0:a2296270a125 7
gr66 0:a2296270a125 8 #define dt 0.01 // pas d'integration
gr66 0:a2296270a125 9
gr66 0:a2296270a125 10 DigitalOut Led0(LED1);
gr66 0:a2296270a125 11
gr66 0:a2296270a125 12 Serial pc(SERIAL_TX, SERIAL_RX,115200);
gr66 0:a2296270a125 13 Ticker calc;
gr66 0:a2296270a125 14 Ticker affich;
gr66 0:a2296270a125 15 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
gr66 0:a2296270a125 16
gr66 0:a2296270a125 17
gr66 0:a2296270a125 18 AnalogIn verin(PC_3); // lecture pos verin
gr66 0:a2296270a125 19 AnalogOut ana (PA_5); // pour debug analogique ISR
gr66 0:a2296270a125 20 Motor motor(PB_4,PA_1,PA_4,PC_7);
gr66 0:a2296270a125 21 int flag_affich=0;
gr66 0:a2296270a125 22 int flag_imu=0;
gr66 0:a2296270a125 23 double pi= 3.1415926535897932;
gr66 0:a2296270a125 24 double gx_off=0,gy_off=0,gz_off=0;
gr66 0:a2296270a125 25 double ang_off=0;
gr66 0:a2296270a125 26
gr66 0:a2296270a125 27
gr66 0:a2296270a125 28 //void gyro_zero(void);
gr66 0:a2296270a125 29 //void angle_zero(void);
gr66 0:a2296270a125 30
gr66 0:a2296270a125 31 void calcule()
gr66 0:a2296270a125 32 {
gr66 0:a2296270a125 33 flag_imu=1;
gr66 0:a2296270a125 34 }
gr66 0:a2296270a125 35 void affiche()
gr66 0:a2296270a125 36 {
gr66 0:a2296270a125 37 flag_affich=1;
gr66 0:a2296270a125 38 }
gr66 0:a2296270a125 39
gr66 0:a2296270a125 40 int main()
gr66 0:a2296270a125 41 {
gr66 0:a2296270a125 42
gr66 0:a2296270a125 43 double ang;
gr66 0:a2296270a125 44 // fitres complémentaires
gr66 0:a2296270a125 45 double Fc=0.05;
gr66 0:a2296270a125 46 double RC=1./(Fc*2*pi); //calcul de RC
gr66 0:a2296270a125 47 double a0=1./(1+(2*RC/dt)); //calcul du coefficient a du filtre
gr66 0:a2296270a125 48 double b0=(1-(2.*RC/dt))*a0; //calcul du coefficient b du filtre
gr66 0:a2296270a125 49 double a1=a0*RC*1.0; //calcul du coefficient a du filtre
gr66 0:a2296270a125 50
gr66 0:a2296270a125 51 double angle_acce_pred=0.0f;
gr66 0:a2296270a125 52 double angle_acce=0.0f;
gr66 0:a2296270a125 53 double angle_acce_f_pred=0.0f;
gr66 0:a2296270a125 54 double angle_acce_f=0.0f;
gr66 0:a2296270a125 55 //
gr66 0:a2296270a125 56 double gyroy_pred=0.0f;
gr66 0:a2296270a125 57 double gyroy=0.0f;
gr66 0:a2296270a125 58 double angle_gyroy_f_pred=0.0f;
gr66 0:a2296270a125 59 double angle_gyroy_f=0.0f;
gr66 0:a2296270a125 60 //
gr66 0:a2296270a125 61 double angle_final;
gr66 0:a2296270a125 62 wait(1);
gr66 0:a2296270a125 63 //DOF.calibration();
gr66 0:a2296270a125 64 DOF.begin();
gr66 0:a2296270a125 65 wait(1);
gr66 0:a2296270a125 66 DOF.calibration();
gr66 0:a2296270a125 67 wait(1);
gr66 0:a2296270a125 68 calc.attach(&calcule,dt);
gr66 0:a2296270a125 69 affich.attach(&affiche,0.1);
gr66 0:a2296270a125 70 while(1) {
gr66 0:a2296270a125 71 if(flag_imu) {
gr66 0:a2296270a125 72 ana=0.6;
gr66 0:a2296270a125 73 DOF.readAccel();
gr66 0:a2296270a125 74 DOF.readGyro();
gr66 0:a2296270a125 75 ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off); // sur table
gr66 0:a2296270a125 76 //ang=((180/pi)*atan2((double)DOF.ax,(double)DOF.ay)-ang_off); // sur site
gr66 0:a2296270a125 77
gr66 0:a2296270a125 78 // filtres complémentaires
gr66 0:a2296270a125 79 angle_acce_pred = angle_acce;
gr66 0:a2296270a125 80 angle_acce=ang;
gr66 0:a2296270a125 81 angle_acce_f_pred = angle_acce_f;
gr66 0:a2296270a125 82 angle_acce_f=a0*angle_acce+a0*angle_acce_pred-b0*angle_acce_f_pred; //filtrage accéléromètre
gr66 0:a2296270a125 83
gr66 0:a2296270a125 84 gyroy_pred = gyroy;
gr66 0:a2296270a125 85 gyroy=-DOF.gx-gx_off; //sur table
gr66 0:a2296270a125 86 // gyroy=-DOF.gz-gz_off; //sur site
gr66 0:a2296270a125 87 angle_gyroy_f_pred = angle_gyroy_f;
gr66 0:a2296270a125 88 angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred;
gr66 0:a2296270a125 89 //
gr66 0:a2296270a125 90 angle_final=angle_acce_f+angle_gyroy_f;
gr66 0:a2296270a125 91 //
gr66 0:a2296270a125 92 //
gr66 0:a2296270a125 93 flag_imu=0;
gr66 0:a2296270a125 94 ana=0.0;
gr66 0:a2296270a125 95 }
gr66 0:a2296270a125 96 if(flag_affich) {
gr66 0:a2296270a125 97 ana=0.3;
gr66 0:a2296270a125 98 //moteur
gr66 0:a2296270a125 99 float x=verin.read();
gr66 0:a2296270a125 100 float s=0.0;
gr66 0:a2296270a125 101 if((angle_final>2)&&(x>0.1)) s=0.4;
gr66 0:a2296270a125 102 else if((angle_final<-2)&&(x<0.9)) s=-0.4;
gr66 0:a2296270a125 103 else motor.speed(0.0);
gr66 0:a2296270a125 104 motor.speed(s);
gr66 0:a2296270a125 105 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);
gr66 0:a2296270a125 106 ana=0.0;
gr66 0:a2296270a125 107 flag_affich=0;
gr66 0:a2296270a125 108 }
gr66 0:a2296270a125 109 }
gr66 0:a2296270a125 110 }
gr66 0:a2296270a125 111
gr66 0:a2296270a125 112 void gyro_zero(void)
gr66 0:a2296270a125 113 {
gr66 0:a2296270a125 114 const int NN=1000;
gr66 0:a2296270a125 115 //float GyroBuffer[3];
gr66 0:a2296270a125 116 for(int i=0; i<NN; i++) {
gr66 0:a2296270a125 117 DOF.readGyro();
gr66 0:a2296270a125 118 gx_off=gx_off+DOF.gx/(NN);
gr66 0:a2296270a125 119 gy_off=gy_off+DOF.gy/(NN);
gr66 0:a2296270a125 120 gz_off=gz_off+DOF.gz/(NN);
gr66 0:a2296270a125 121 }
gr66 0:a2296270a125 122 }
gr66 0:a2296270a125 123 void angle_zero(void)
gr66 0:a2296270a125 124 {
gr66 0:a2296270a125 125 const int NN=1000;
gr66 0:a2296270a125 126 //int16_t AccBuffer[3];
gr66 0:a2296270a125 127 for(int i=0; i<NN; i++) {
gr66 0:a2296270a125 128 DOF.readAccel();
gr66 0:a2296270a125 129 double ang=(180/pi)*atan2((double)DOF.ay,(double)DOF.az); // sur table
gr66 0:a2296270a125 130 //double ang=(180/pi)*atan2((double)DOF.ax,(double)DOF.ay); // sur site
gr66 0:a2296270a125 131 ang_off=ang_off+ang/NN;
gr66 0:a2296270a125 132 }
gr66 0:a2296270a125 133 }