Raynaud Gilles
/
VITI_motor_angle_3
angle3
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?
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 | 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 | } |