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