Raynaud Gilles / Mbed OS VITI_motor_angle_3

angle3

Dependencies:   LSM9DS1 TB5649

main.cpp@6:65c511f6f1fc, 2020-05-23 (annotated)

Committer:
gr66
Date:
Sat May 23 10:30:02 2020 +0000
Revision:
6:65c511f6f1fc
Parent:
5:d51ba8e93d10
Child:
7:f5c5cf499311
12:29

Who changed what in which revision?

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