Raynaud Gilles / Mbed OS VITI_motor_angle_3

angle3

Dependencies:   LSM9DS1 TB5649

main.cpp@14:f4cbc5db2873, 2020-06-03 (annotated)

Committer:
gr66
Date:
Wed Jun 03 20:23:57 2020 +0000
Revision:
14:f4cbc5db2873
Parent:
13:0549a3e57bc4 
V1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gr66 14:f4cbc5db2873 1 // *******************************************************
gr66 14:f4cbc5db2873 2 // HUBLEX - Université Paris Saclay - IUT de Cachan
gr66 14:f4cbc5db2873 3 // Version 1.0 du 3 juin 2020
gr66 14:f4cbc5db2873 4 // Auteur : Gilles Raynaud
gr66 14:f4cbc5db2873 5 //
gr66 14:f4cbc5db2873 6 // SteadyTech
gr66 14:f4cbc5db2873 7 //*********************************************************
gr66 11:9d83bb9a611c 8 #define viti 1 // 1 montage viti ; 0 montage sur table
gr66 0:a2296270a125 9 #include "mbed.h"
gr66 0:a2296270a125 10 #include "LSM9DS1.h"
gr66 0:a2296270a125 11 #include "TB6549.h"
gr66 13:0549a3e57bc4 12 //
gr66 9:2a16e3930e1d 13 #define dt 0.01 // pas d'integration
gr66 5:d51ba8e93d10 14 #define FDC_PLUS 0.9 // fin de course +
gr66 5:d51ba8e93d10 15 #define FDC_MOINS 0.1 // din de course -
gr66 13:0549a3e57bc4 16 #define ZM 2.0 // zone morte +/- zm
gr66 13:0549a3e57bc4 17 #define SP 0.6 // pwm moteur en % (0-1)
gr66 13:0549a3e57bc4 18 //
gr66 8:41752698aa35 19 void calcul(void);
gr66 13:0549a3e57bc4 20 //
gr66 12:ab387ced85ab 21 DigitalOut LedVP(PC_8); // led Verin P
gr66 12:ab387ced85ab 22 DigitalOut LedVM(PC_5); // led Verin M
gr66 14:f4cbc5db2873 23 DigitalOut LedOK(PC_6); // led Système OK
gr66 13:0549a3e57bc4 24 //
gr66 0:a2296270a125 25 Serial pc(SERIAL_TX, SERIAL_RX,115200);
gr66 13:0549a3e57bc4 26 //
gr66 0:a2296270a125 27 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
gr66 4:b40027cb3012 28 Thread thread (osPriorityAboveNormal);
gr66 8:41752698aa35 29 EventQueue queue;
gr66 14:f4cbc5db2873 30 AnalogIn verin(PC_3); // lecture position verin
gr66 14:f4cbc5db2873 31 AnalogOut ana (PA_5); // pour debug analogique ISR et RTOS
gr66 13:0549a3e57bc4 32 Motor motor(PB_4,PA_1,PA_4,PC_7); // commande moteur vérin
gr66 8:41752698aa35 33 Ticker tic;
gr66 13:0549a3e57bc4 34 //
gr66 0:a2296270a125 35 double pi= 3.1415926535897932;
gr66 0:a2296270a125 36 double gx_off=0,gy_off=0,gz_off=0;
gr66 0:a2296270a125 37 double ang_off=0;
gr66 8:41752698aa35 38 //
gr66 3:d552b7419f51 39 double ang;
gr66 3:d552b7419f51 40 // fitres complémentaires
gr66 14:f4cbc5db2873 41 double Fc=0.05; // fréquence charnière du filtre complémentaire
gr66 14:f4cbc5db2873 42 double RC ; //constante de temps
gr66 14:f4cbc5db2873 43 double a0; //coefficient a0 du filtre
gr66 14:f4cbc5db2873 44 double b0; //coefficient b0 du filtre
gr66 14:f4cbc5db2873 45 double a1; //coefficient a1 du filtre
gr66 13:0549a3e57bc4 46 //
gr66 3:d552b7419f51 47 double angle_acce_pred=0.0f;
gr66 3:d552b7419f51 48 double angle_acce=0.0f;
gr66 3:d552b7419f51 49 double angle_acce_f_pred=0.0f;
gr66 3:d552b7419f51 50 double angle_acce_f=0.0f;
gr66 3:d552b7419f51 51 //
gr66 3:d552b7419f51 52 double gyroy_pred=0.0f;
gr66 3:d552b7419f51 53 double gyroy=0.0f;
gr66 3:d552b7419f51 54 double angle_gyroy_f_pred=0.0f;
gr66 3:d552b7419f51 55 double angle_gyroy_f=0.0f;
gr66 3:d552b7419f51 56 //
gr66 3:d552b7419f51 57 double angle_final;
gr66 6:65c511f6f1fc 58 //
gr66 3:d552b7419f51 59 void calcul(void)
gr66 0:a2296270a125 60 {
gr66 13:0549a3e57bc4 61 ana=0.3; // debug RTOS
gr66 13:0549a3e57bc4 62 Fc=0.05; // frequence de coupure des filtres
gr66 8:41752698aa35 63 RC=1./(Fc*2*pi); //calcul de RC
gr66 14:f4cbc5db2873 64 a0=1./(1+(2*RC/dt)); //calcul du coefficient a0 du filtre
gr66 14:f4cbc5db2873 65 b0=(1-(2.*RC/dt))*a0; //calcul du coefficient b0 du filtre
gr66 14:f4cbc5db2873 66 a1=a0*RC*1.0; //calcul du coefficient a1 du filtre
gr66 8:41752698aa35 67 DOF.readAccel();
gr66 8:41752698aa35 68 DOF.readGyro();
gr66 6:65c511f6f1fc 69 #if viti
gr66 12:ab387ced85ab 70 //ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off); // sur site USB dessous
gr66 12:ab387ced85ab 71 ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off); // sur site USB gauche
gr66 6:65c511f6f1fc 72 #else
gr66 8:41752698aa35 73 ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off); // sur table
gr66 6:65c511f6f1fc 74 #endif
gr66 8:41752698aa35 75 // filtres complémentaires
gr66 8:41752698aa35 76 angle_acce_pred = angle_acce;
gr66 8:41752698aa35 77 angle_acce=ang;
gr66 8:41752698aa35 78 angle_acce_f_pred = angle_acce_f;
gr66 8:41752698aa35 79 angle_acce_f=a0*angle_acce+a0*angle_acce_pred-b0*angle_acce_f_pred; //filtrage accéléromètre
gr66 10:2a1abbd2c84c 80 //
gr66 8:41752698aa35 81 gyroy_pred = gyroy;
gr66 6:65c511f6f1fc 82 #if viti
gr66 8:41752698aa35 83 gyroy=DOF.gz-gz_off; //sur site
gr66 6:65c511f6f1fc 84 #else
gr66 8:41752698aa35 85 gyroy=-DOF.gx-gx_off; //sur table
gr66 6:65c511f6f1fc 86 #endif
gr66 10:2a1abbd2c84c 87 //
gr66 8:41752698aa35 88 angle_gyroy_f_pred = angle_gyroy_f;
gr66 8:41752698aa35 89 angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred;
gr66 8:41752698aa35 90 //
gr66 8:41752698aa35 91 angle_final=angle_acce_f+angle_gyroy_f;
gr66 8:41752698aa35 92 ana=0.0;
gr66 0:a2296270a125 93 }
gr66 10:2a1abbd2c84c 94 //
gr66 0:a2296270a125 95 int main()
gr66 0:a2296270a125 96 {
gr66 12:ab387ced85ab 97 LedVP=1;
gr66 12:ab387ced85ab 98 LedVM=1;
gr66 12:ab387ced85ab 99 LedOK=1;
gr66 14:f4cbc5db2873 100 DOF.begin();
gr66 14:f4cbc5db2873 101 for(int ii=0; ii<4; ii++) {
gr66 14:f4cbc5db2873 102 DOF.calibration();
gr66 14:f4cbc5db2873 103 LedVP=!LedVP;
gr66 14:f4cbc5db2873 104 LedVM=!LedVM;
gr66 14:f4cbc5db2873 105 LedOK=!LedOK;
gr66 14:f4cbc5db2873 106 }
gr66 12:ab387ced85ab 107 LedVP=0;
gr66 12:ab387ced85ab 108 LedVM=0;
gr66 12:ab387ced85ab 109 LedOK=1;
gr66 14:f4cbc5db2873 110 //wait(0.5);
gr66 13:0549a3e57bc4 111 // init filtre accéléro
gr66 13:0549a3e57bc4 112 DOF.readAccel();
gr66 13:0549a3e57bc4 113 angle_acce_f_pred=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off); // sur site USB gauche
gr66 13:0549a3e57bc4 114 angle_acce_f=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off); // sur site USB gauche
gr66 13:0549a3e57bc4 115 //
gr66 8:41752698aa35 116 thread.start(callback(&queue,&EventQueue::dispatch_forever));
gr66 9:2a16e3930e1d 117 tic.attach(queue.event(&calcul),dt);
gr66 12:ab387ced85ab 118 LedOK=1;
gr66 0:a2296270a125 119 while(1) {
gr66 13:0549a3e57bc4 120 ana=0.6; // debug RTOS
gr66 3:d552b7419f51 121 //moteur
gr66 13:0549a3e57bc4 122 double x=verin.read();
gr66 13:0549a3e57bc4 123 double s=0.0;
gr66 12:ab387ced85ab 124 if((angle_final<-ZM)&&(x>FDC_MOINS)) {
gr66 12:ab387ced85ab 125 s=SP;
gr66 12:ab387ced85ab 126 LedVP=1;
gr66 12:ab387ced85ab 127 LedVM=0;
gr66 12:ab387ced85ab 128 } else if((angle_final>ZM)&&(x<FDC_PLUS)) {
gr66 12:ab387ced85ab 129 s=-SP;
gr66 13:0549a3e57bc4 130 LedVP=0;
gr66 13:0549a3e57bc4 131 LedVM=1;
gr66 12:ab387ced85ab 132 } else {
gr66 12:ab387ced85ab 133 s=0;
gr66 12:ab387ced85ab 134 LedVP=0;
gr66 12:ab387ced85ab 135 LedVM=0;
gr66 12:ab387ced85ab 136 }
gr66 3:d552b7419f51 137 motor.speed(s);
gr66 9:2a16e3930e1d 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 13:0549a3e57bc4 139 //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);
gr66 3:d552b7419f51 140 ana=0.0;
gr66 7:f5c5cf499311 141 wait(0.095);
gr66 0:a2296270a125 142 }
gr66 0:a2296270a125 143 }
gr66 0:a2296270a125 144