Raynaud Gilles / Mbed OS VITI_motor_angle_1

V1

Dependencies:   LSM9DS1 TB5649

main.cpp@1:a524fa7b2606, 2020-05-21 (annotated)

Committer:
gr66
Date:
Thu May 21 17:50:59 2020 +0000
Revision:
1:a524fa7b2606
Parent:
0:e5a92da6a6fe 
V1

Who changed what in which revision?

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