Raynaud Gilles
/
VITI_motor_angle_1
V1
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?
User | Revision | Line number | New 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 | } |