position and speed dc motor regulator with real time software and trajectory generator
Dependencies: NVIC_set_all_priorities QEI_hw SoftPWM mbed
position/time graph:
un grand merci a Pierre Voiriot pour son aide .
rem: pour la modiffication hardware QEI voir ce lien
https://os.mbed.com/users/hexley/notebook/qei_hw-interface-implementation-notes/
main.cpp
- Committer:
- exarkun
- Date:
- 2017-11-13
- Revision:
- 0:6aa00967963c
File content as of revision 0:6aa00967963c:
#include "mbed.h" #include "qeihw.h" #include "NVIC_set_all_priorities.h" // déclaration du hardware Serial pc(USBTX, USBRX);//utilisation de la liaison usb QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE ); Serial device(p9, p10);// tx, rx.//definition de la liaison rs232 DigitalOut frein_moteur1(p15); //changement momentané pour le projet r2d de p5 a p15 DigitalOut sens_rotation_moteur1(p13); PwmOut PWM1(p21); // définition des timers. Timer timer1; Timer timer2; // définition des sortie leds DigitalOut led1(LED1); DigitalOut led3(LED3); char c; int t ; float PI=3.141592653589793; //position codeur en pulses 2000pulses/rotation int position_actuelle;//position du codeur int position_actuelle_n1;//position du codeur a n-1 //données mecanique & moteur float J=3.375*1e-5;//0.005;//inertie en kg.m² float K=0.05;//2;//constante de couple N.m/v float R=5;//K;//resistance du moteur en Ohms float Umax=24;//tension max en volts //parametres reglage float dt_vit(5*1e-4); float Ki=10;//default value=10; float Kp; float Kv; float K_sw(K); float tau; float taud=(10*dt_vit); float r1=3;//default value=2.5; float r2=r1;//default value=2.5; //variables generateur de consigne/////////////////////////////////////////////////////////////////////////////////////////// float Teta_debut, Teta_fin, Teta_diff; float delta_Teta; float consigne;//en pulse float slope_time(1.);//en seconde bool start_slope(false);//flag debut de montee int ticks_total; int count_ticks; int count_slope(0); int count_max(0); //variables calcul de l'asservissement en vitesse et position float Tetaco; //valeur de la consigne position en radians float Tetam;//valeur mesuree de la position en radians float dTetam; float integ; float A; float dTetam_n1; float Tetaco_n1; float Tetam_n1; float integ_n1; float cyclic; int signe_rot; //declaration des differantes taches void task1_switch(void); void task2_switch(void); void task3_switch(void); void task4_switch(void); void task5_switch(void); void task6_switch(void); void task7_switch(void); void task8_switch(void); //declaration des differantes interuption timer Ticker time_up1; //define a Ticker, with name “time_up1” Ticker time_up2; //define a Ticker, with name “time_up2” Ticker time_up3; //define a Ticker, with name “time_up3” Ticker time_up4; //define a Ticker, with name “time_up4” Ticker time_up5; //define a Ticker, with name “time_up5” Ticker time_up6; //define a Ticker, with name “time_up6” Ticker time_up7; //define a Ticker, with name “time_up7” Ticker time_up8; //define a Ticker, with name “time_up8” //declaration des leds visuelle utiliser pour mesurer le temps des taches DigitalOut myled1(LED1); DigitalOut myled3(LED3); //////////////////////////////////////// // Convertion pulses to radians // //////////////////////////////////////// //PI =3.141592653589793 =1000 pulses codeurs float pulsesToRadians(int pulses) { float radians_VAL; radians_VAL=(pulses*PI)/1000.; return radians_VAL; }; //////////////////////////////////////// // calcule de la vitesse angulaire // //////////////////////////////////////// // ici le code du calcule // Convert ASCII string to unsigned 32-bit decimal "string is null-terminated" unsigned long Str2UDec(unsigned char string[]){ unsigned long i = 0; // index unsigned long n = 0; // number while(string[i] != 0){ n = 10*n +(string[i]-0x30); i++; } return n; } //////////////////////////////////////// // TASKS1 // //////////////////////////////////////// void task1_switch() { myled1=1; //lecture valeur codeur et conversion en radians position_actuelle=qei.GetPosition();//lecture valeur codeur et affectation a la variable globale Tetam=pulsesToRadians(position_actuelle);//conversion valeur impulsionel en radians //Calcul de la nouvelle consigne: //Etape1:derivee filtree dTetam=1./(1.+taud*2./dt_vit)*(-dTetam_n1*(1.-taud*2./dt_vit)+2./dt_vit*(Tetam-Tetam_n1)); //Etape2:calcul de integ non saturee integ=integ_n1+dt_vit/2.*((Tetaco-Tetam)+(Tetaco_n1-Tetam_n1)); //Etape3:Calcul de A non saturee A=Kv*K_sw/Umax*(-dTetam+Kp*Ki*integ-Kp*Tetam); //Etape 4 et 5 : calcul de integ saturee if (A>1) { integ=1./Kp/Ki*(Umax/Kv/K_sw+dTetam+Kp*Tetam); A=1; } if(A<-1) { integ=1./Kp/Ki*(-Umax/Kv/K_sw+dTetam+Kp*Tetam); A=-1; } //Etape 6:affectation du signe de rotation a l'etage de puissance if (A>0) { signe_rot=1; sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1 cyclic=A; } else { signe_rot=0; sens_rotation_moteur1.write(signe_rot); //affectation du sens de rotation moteur1 cyclic=-A;//peut etre une erreur ici sy?? } PWM1.write(cyclic);// affectation de la valeur calculé en pwm // enregistrement des valeurs N-1 position_actuelle_n1=position_actuelle; dTetam_n1=dTetam; Tetaco_n1=Tetaco; Tetam_n1=Tetam; integ_n1=integ; myled1=0; //myled1=!myled1;//changement etat de la led1 } //////////////////////////////////////// // TASKS2 // //////////////////////////////////////// void task2_switch() { // pc.printf("%f\r\n", Tetaco);//valeur float consigne en radians le renvoyé par usb // pc.printf("%i\r\n", position_actuelle);//valeur int du codeur renvoyé par usb // pc.printf("%f\r\n", Tetam);//valeur float du codeur en radians renvoyé par usb // pc.printf("dTetam : %f\r\n", dTetam);//valeur float dTetam // pc.printf("integ : %f\r\n", integ);//valeur float integ // pc.printf("%f\r\n", A);//valeur float du codeur en radians le renvoyé par usb // pc.printf("%e\r\n", cyclic);//valeur float du codeur en radians le renvoyé par usb // pc.printf("\r\n");//retour chario // pc.printf("$%d %d %d;", position_actuelle,position_actuelle/2,position_actuelle/10 ); //utiliser avec logicielle serial port ploter pc.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter }; //////////////////////////////////////// // TASKS3 // //////////////////////////////////////// void task3_switch() { myled3=1; //generation de la tragectoire if(start_slope) { Teta_debut=Tetam;//affectation de la consigne a la variable globale Teta_fin=pulsesToRadians(consigne);//affectation de la consigne a la variable globale Teta_diff = Teta_fin - Teta_debut; delta_Teta=(Teta_diff/slope_time)*5*1e-4; //timer1.start();// déclenchement du timer1 count_slope = 0; count_max = slope_time/(5*1e-4); start_slope=false; } //float timeco = timer1.read(); count_slope++; if (count_slope<=count_max){Tetaco=Tetaco+delta_Teta;} //if (timeco<slope_time){Tetaco=Tetaco+delta_Teta;} //if (timeco==slope_time){Tetaco=Teta_fin;} //if (timeco>slope_time){timer1.reset(); // remise à zéro du timer1 //myled3=!myled3; myled3=0; } //////////////////////////////////////// // TASKS4 // //////////////////////////////////////// void task4_switch() { device.printf("$%d;", position_actuelle); //utiliser avec logicielle serial port ploter } //////////////////////////////////////// // TASKS5 // //////////////////////////////////////// void task5_switch() { // ici code de la tache 5 } //////////////////////////////////////// // TASKS6 // //////////////////////////////////////// void task6_switch() { // ici code de la tache 6 } //////////////////////////////////////// // TASKS7 // //////////////////////////////////////// void task7_switch() { // ici code de la tache 7 } //////////////////////////////////////// // TASKS8 // //////////////////////////////////////// void task8_switch() { // ici code de la tache 8 } ///////////////////////////////////////////////////////////////////////////// // declaration de la fonction moteur1 // ///////////////////////////////////////////////////////////////////////////// void motor1_mouve(int position_final,int T1) { //calcule nombres de ticks tache2 suite au temps T1 demander //calcule delta_Teta /////////////////////////////////////////////////// // debut du mouvement // /////////////////////////////////////////////////// timer1.start(); // déclenchement du timer1 timer2.start(); // déclenchement du timer2 //generation de consigne position for(int i=0;i<=consigne;i++) { } if (timer1.read_us()>T1) // lecture du temps du timer1 en us { device.printf("TIME out motor1 0\r\n"); } if (timer2.read_ms()>T1) // lecture du temps du timer1 en ms { device.printf("TIME out motor1 0\r\n"); } timer1.reset(); // remise à zéro du timer1 timer2.reset(); // remise à zéro du timer2 }; //////////////////////////////////////////////////////////////////////////// // PROGRAMME PRINCIPAL // //////////////////////////////////////////////////////////////////////////// int main() { int cycles; //NVIC_SetPriority(EINT3_IRQn, 252);//set interupt to highest priority 0. //NVIC_SetPriority(TIMER1_IRQn, 253);// set mbed tickers to lower priority than other things //NVIC_SetPriority(TIMER2_IRQn, 254);// set mbed tickers to lower priority than other things //NVIC_SetPriority(TIMER3_IRQn, 255);// set mbed tickers to lower priority than other things //NVIC_set_all_irq_priorities(0); qei.SetDigiFilter(480UL);//filtre qei.SetMaxPosition(0xFFFFFFFF);//position max 4294967295 pulses //initialisation de la com rs232 device.baud(9600);//rs232 28800 baud device.printf("serial rs232 ok \n"); //calculs de tau ,Kv,Kp tau=(J*R)/K; Kv=r1*r2*tau*Ki-1.; if(Kv<0){Kv = -Kv;} Kp=r1*Ki*(1.+Kv)/Kv; //initialisation moteur PWM1.period(0.00005);// initialisation du rapport cyclique fixe la période à 50us----f=20Khz frein_moteur1.write(0);//affectation de la valeur du frein moteur1 sens_rotation_moteur1.write(0); //affectation du sens de rotation moteur1 non utiliser puisque c'est la tache1 qui le fait non pas la generation de trajectoire //lancement des tasks time_up1.attach(&task1_switch, 0.0005); //initialises the ticker 2Khz "tous les 500us il y as une interruption task1" time_up2.attach(&task2_switch, 0.01); //initialises the ticker 100hz time_up3.attach(&task3_switch, 0.0005); //initialises the ticker 2khz time_up4.attach(&task4_switch, 0.01); //initialises the ticker 100hz time_up5.attach(&task5_switch, 0.0005); //initialises the ticker 2kh time_up6.attach(&task6_switch, 0.0005); //initialises the ticker 2kh time_up7.attach(&task7_switch, 0.0005); //initialises the ticker 2kh time_up8.attach(&task8_switch, 0.0005); //initialises the ticker 2kh count_ticks=0; while(1) { c='1'; //device.scanf("%c",&c);//capture du caract ascii switch(c) { case '1': consigne=70000; slope_time=0.001; start_slope=true; wait(3); consigne=0; slope_time=2.; start_slope=true; wait(3); break; case '2': device.printf("profile de mouvement 2\r\n"); break; case '3': device.printf("moving3 Robotic Axis 1\r\n"); break; case '4': device.printf("moving4 Robotic Axis 1\r\n"); break; case '5': device.printf("moving5 Robotic Axis 1\r\n"); break; case '6': device.printf("moving6 Robotic Axis 1\r\n"); break; case '7': device.printf("moving7 Robotic Axis 1\r\n"); break; case '8': device.printf("moving8 Robotic Axis 1\r\n"); break; case '9': device.printf("moving9 Robotic Axis 1\r\n"); break; case '0': device.printf("moving7 Robotic Axis 1\r\n"); break; } } }