Raynaud Gilles / Mbed OS VITI_motor_angle_2

V2

Dependencies:   LSM9DS1 TB5649

main.cpp@2:952d41c26b43, 2020-05-21 (annotated)

Committer:
gr66
Date:
Thu May 21 18:23:34 2020 +0000
Revision:
2:952d41c26b43
Parent:
0:a2296270a125
Child:
3:d552b7419f51
RTOS1

Who changed what in which revision?

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