Raynaud Gilles / Mbed OS VITI_motor_angle_3

angle3

Dependencies:   LSM9DS1 TB5649

main.cpp@4:b40027cb3012, 2020-05-22 (annotated)

Committer:
gr66
Date:
Fri May 22 14:19:42 2020 +0000
Revision:
4:b40027cb3012
Parent:
3:d552b7419f51
Child:
5:d51ba8e93d10
V22/05

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 3:d552b7419f51 14 //Ticker calc;
gr66 3:d552b7419f51 15 //Ticker affich;
gr66 0:a2296270a125 16 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
gr66 4:b40027cb3012 17 Thread thread (osPriorityAboveNormal);
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 3:d552b7419f51 31 double ang;
gr66 3:d552b7419f51 32 // fitres complémentaires
gr66 3:d552b7419f51 33 double Fc=0.05;
gr66 3:d552b7419f51 34 double RC=1./(Fc*2*pi); //calcul de RC
gr66 3:d552b7419f51 35 double a0=1./(1+(2*RC/dt)); //calcul du coefficient a du filtre
gr66 3:d552b7419f51 36 double b0=(1-(2.*RC/dt))*a0; //calcul du coefficient b du filtre
gr66 3:d552b7419f51 37 double a1=a0*RC*1.0; //calcul du coefficient a du filtre
gr66 0:a2296270a125 38
gr66 3:d552b7419f51 39 double angle_acce_pred=0.0f;
gr66 3:d552b7419f51 40 double angle_acce=0.0f;
gr66 3:d552b7419f51 41 double angle_acce_f_pred=0.0f;
gr66 3:d552b7419f51 42 double angle_acce_f=0.0f;
gr66 3:d552b7419f51 43 //
gr66 3:d552b7419f51 44 double gyroy_pred=0.0f;
gr66 3:d552b7419f51 45 double gyroy=0.0f;
gr66 3:d552b7419f51 46 double angle_gyroy_f_pred=0.0f;
gr66 3:d552b7419f51 47 double angle_gyroy_f=0.0f;
gr66 3:d552b7419f51 48 //
gr66 3:d552b7419f51 49 double angle_final;
gr66 3:d552b7419f51 50 void calcul(void)
gr66 0:a2296270a125 51 {
gr66 3:d552b7419f51 52 while(1) {
gr66 3:d552b7419f51 53 ana=0.6;
gr66 3:d552b7419f51 54 DOF.readAccel();
gr66 3:d552b7419f51 55 DOF.readGyro();
gr66 3:d552b7419f51 56 ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off); // sur table
gr66 3:d552b7419f51 57 //ang=((180/pi)*atan2((double)DOF.ax,(double)DOF.ay)-ang_off); // sur site
gr66 3:d552b7419f51 58
gr66 3:d552b7419f51 59 // filtres complémentaires
gr66 3:d552b7419f51 60 angle_acce_pred = angle_acce;
gr66 3:d552b7419f51 61 angle_acce=ang;
gr66 3:d552b7419f51 62 angle_acce_f_pred = angle_acce_f;
gr66 3:d552b7419f51 63 angle_acce_f=a0*angle_acce+a0*angle_acce_pred-b0*angle_acce_f_pred; //filtrage accéléromètre
gr66 3:d552b7419f51 64
gr66 3:d552b7419f51 65 gyroy_pred = gyroy;
gr66 3:d552b7419f51 66 gyroy=-DOF.gx-gx_off; //sur table
gr66 3:d552b7419f51 67 // gyroy=-DOF.gz-gz_off; //sur site
gr66 3:d552b7419f51 68 angle_gyroy_f_pred = angle_gyroy_f;
gr66 3:d552b7419f51 69 angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred;
gr66 3:d552b7419f51 70 //
gr66 3:d552b7419f51 71 angle_final=angle_acce_f+angle_gyroy_f;
gr66 3:d552b7419f51 72
gr66 3:d552b7419f51 73 //
gr66 3:d552b7419f51 74 //
gr66 3:d552b7419f51 75
gr66 3:d552b7419f51 76 ana=0.0;
gr66 3:d552b7419f51 77 wait(0.0082);
gr66 3:d552b7419f51 78 }
gr66 0:a2296270a125 79 }
gr66 0:a2296270a125 80
gr66 0:a2296270a125 81 int main()
gr66 0:a2296270a125 82 {
gr66 0:a2296270a125 83
gr66 0:a2296270a125 84
gr66 0:a2296270a125 85 wait(1);
gr66 0:a2296270a125 86 //DOF.calibration();
gr66 0:a2296270a125 87 DOF.begin();
gr66 0:a2296270a125 88 wait(1);
gr66 0:a2296270a125 89 DOF.calibration();
gr66 0:a2296270a125 90 wait(1);
gr66 3:d552b7419f51 91 thread.start(calcul);
gr66 3:d552b7419f51 92
gr66 0:a2296270a125 93 while(1) {
gr66 0:a2296270a125 94
gr66 0:a2296270a125 95
gr66 3:d552b7419f51 96 ana=0.3;
gr66 3:d552b7419f51 97 //moteur
gr66 3:d552b7419f51 98 float x=verin.read();
gr66 3:d552b7419f51 99 float s=0.0;
gr66 3:d552b7419f51 100 if((angle_final>2)&&(x>0.1)) s=0.4;
gr66 3:d552b7419f51 101 else if((angle_final<-2)&&(x<0.9)) s=-0.4;
gr66 3:d552b7419f51 102 else motor.speed(0.0);
gr66 3:d552b7419f51 103 motor.speed(s);
gr66 3:d552b7419f51 104 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 3:d552b7419f51 105 ana=0.0;
gr66 3:d552b7419f51 106 wait(0.1);
gr66 3:d552b7419f51 107
gr66 0:a2296270a125 108 }
gr66 0:a2296270a125 109 }
gr66 0:a2296270a125 110
gr66 0:a2296270a125 111 void gyro_zero(void)
gr66 0:a2296270a125 112 {
gr66 0:a2296270a125 113 const int NN=1000;
gr66 0:a2296270a125 114 //float GyroBuffer[3];
gr66 0:a2296270a125 115 for(int i=0; i<NN; i++) {
gr66 0:a2296270a125 116 DOF.readGyro();
gr66 0:a2296270a125 117 gx_off=gx_off+DOF.gx/(NN);
gr66 0:a2296270a125 118 gy_off=gy_off+DOF.gy/(NN);
gr66 0:a2296270a125 119 gz_off=gz_off+DOF.gz/(NN);
gr66 0:a2296270a125 120 }
gr66 0:a2296270a125 121 }
gr66 0:a2296270a125 122 void angle_zero(void)
gr66 0:a2296270a125 123 {
gr66 0:a2296270a125 124 const int NN=1000;
gr66 0:a2296270a125 125 //int16_t AccBuffer[3];
gr66 0:a2296270a125 126 for(int i=0; i<NN; i++) {
gr66 0:a2296270a125 127 DOF.readAccel();
gr66 0:a2296270a125 128 double ang=(180/pi)*atan2((double)DOF.ay,(double)DOF.az); // sur table
gr66 0:a2296270a125 129 //double ang=(180/pi)*atan2((double)DOF.ax,(double)DOF.ay); // sur site
gr66 0:a2296270a125 130 ang_off=ang_off+ang/NN;
gr66 0:a2296270a125 131 }
gr66 0:a2296270a125 132 }