[STM32] Asservissement

Automatique : Rappels

L’ automatique est une technique proposant des méthodes pour asservir un système.
Dans tout système asservi, il y a :

Une Consigne : La vitesse à laquelle je souhaite rouler / la température que je souhaite avoir dans ma pièce.
Une Mesure : Ce que renvoie le capteur de vitesse / la température observée par la sonde
Une Erreur : La différence entre ce que je souhaite et ce que j’ai réellement
Une Commande : Signal envoyé au convertisseur pour réaliser une action afin que l’erreur soit minimale.

Afin d’optimiser les performances du système ( stabilité, rapidité, précision ), le commande peut être corrigée.

Périphériques du Microcontrôleur mis en jeu dans un asservissement

Fonctionnement d’un Timer

Un Timer est un compteur qui s’incrémente à chaque front montant d’une horloge.
Cette horloge peut éventuellement être ralentie avec un diviseur de fréquence (Prescaler).
Le Compteur est remis périodiquement à zéro lorsque sa valeur courante de comptage atteint celle d’un registre de comparaison.

T=[(Presc_buff+1) * (Comp_Reg+1)]*T_CLK

Timer PWM (Pulse Width Modulation)

Pour générer un signal MLI (Modulation de Largeur d’Impulsion), il suffit de comparer la valeur courant de comptage à celle d’un registre (Duty), et de forcer à 0 ou à 1 une broche de sortie du microcontrôleur en fonction du résultat de cette comparaison.

Commande des Moteurs ( motoCommand.c )

void motorCommand_Init(void)
void motorLeft_SetDuty(int duty)
void motorRight_SetDuty(int duty)
- duty rapport cyclique
- 0 < duty < 200
- moteurs au repos pour duty=100

code motorCommand

motorCommand.h

/*
 * MotorCommand.h
 */

#ifndef INC_MOTORCOMMAND_H_
#define INC_MOTORCOMMAND_H_

#include "main.h"


void motorCommand_Init(void);
void motorLeft_SetDuty(int);
void motorRight_SetDuty(int);



#endif /* INC_MOTORCOMMAND_H_ */

motorCommand.c

/*
 * MotorCommand.c
 */

#include "motorCommand.h"

static TIM_HandleTypeDef    TimHandle;
static TIM_OC_InitTypeDef   sConfigOC;

//=================================================================
//			PWM INIT
// TIMER 4 (PWM)  : CH1 et CH2
// ENABLE : Sortie Logique (GPIO) PA15
//=================================================================

void motorCommand_Init(void)
{
	unsigned int uwPrescalerValue = 0;

	/* Compute the prescaler value to have TIM4 counter clock equal to 10MHz */
	  uwPrescalerValue = (unsigned int) ((SystemCoreClock / 10000000) - 1);
	  TimHandle.Instance = TIM4;
	  TimHandle.Init.Period = 200 - 1; // 100MHz/200=50kHz
	  TimHandle.Init.Prescaler = uwPrescalerValue;
	  TimHandle.Init.ClockDivision = 0;
	  TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;

	  HAL_TIM_Base_Init(&TimHandle);

	  sConfigOC.OCMode = TIM_OCMODE_PWM1;
	  sConfigOC.Pulse = 0x5;// Specifies the pulse value to be loaded into the Capture Compare Register. This parameter can be a number between Min_Data = 0x0000 and Max_Data = 0xFFFF */

	  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
	  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;

	 HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfigOC, TIM_CHANNEL_1);
	 HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfigOC, TIM_CHANNEL_2);

	 // CHANGEMENT DU RAPPORT CYCLIQUE
	 __HAL_TIM_SetCompare(&TimHandle, TIM_CHANNEL_1, 100);	// 100 : moteurs au repos
	 __HAL_TIM_SetCompare(&TimHandle, TIM_CHANNEL_2, 100);

	  HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_1);	// MOTOR RIGHT
	  HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_2); // MOTOR LEFT

	  // ENABLE MOTEUR (SI INVERSEUR)
	  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, 0);
}

//=================================================================
//			SET DUTY CYCLE LEFT
//=================================================================
void motorLeft_SetDuty(int duty)
{
	__HAL_TIM_SetCompare(&TimHandle, TIM_CHANNEL_1, duty);
}
//=================================================================
//			SET DUTY CYCLE RIGHT
//=================================================================
void motorRight_SetDuty(int duty)
{
	__HAL_TIM_SetCompare(&TimHandle, TIM_CHANNEL_2, duty);
}
//=================================================================

Timer Interface Codeur

Afin d’asservir les moteurs en vitesse ou en position, il faut déjà connaître les mesures correspondantes.

Le Codeur incrémental

Mesure le la Position

Le comptage des fronts montants des signaux A et B issus du codeur incrémental permet de connaître la position du rotor du moteur.
La précision des codeurs étant de 1250 impulsions/tours, le OU EXCLUSIF entre A et B permet d’obtenir une précision de 2500 impulsions par tours.
le signal index ( mis à 1 à chaque tour ) déclenche une interruption de type EXTI permettant d’incrémenter une variable ( nombre de tours ) et de remettre à zéro le compteur dans le cas d’une mesure de position.

Mesure de la Vitesse

La vitesse est la dérivée de la position.
Numériquement cela se traduit par SPEED=(POS_act-POS_prec)/Te
Ce résultat est faux au moment de la remise à zéro du timer.
L’algorithme suivant est donc nécessaire :

SENS DIRECT

Speed=10000-5000=5000 –> Speed>0 –> Speed<16384 –> Speed=5000
Speed=25000-20000=5000 –> Speed>0 –> Speed<16384 –> Speed=5000
Speed=2232-30000=-27768 –> Speed<0 –> Speed<-16384 –> Speed=-27768+32768=5000 –> Speed=5000

SENS INVERSE

Speed=5000-10000=-5000 –> Speed<0 –> Speed>-16384 –> Speed=-5000
Speed=20000-25000=-5000 –> Speed<0 –> Speed>-16384 –> Speed=-5000
Speed=30000-2232=27768 –> Speed>0 –> Speed>16384 –> Speed=27768-32768=-5000 –> Speed=-5000
void quadEncoder_Init(void)
int16_t quadEncoder_GetPos16L(void)
- Retourne une position sur 16 bits (moins d’un tour)
int16_t quadEncoder_GetPos16R(void)
int32_t quadEncoder_GetPos32L(void)
- Retourne une position sur 32 bits ; à utiliser en cas d’asservissement de position
int32_t quadEncoder_GetPos32R(void)
int16_t quadEncoder_GetSpeedL(void)
- Retourne l’image de la vitesse ; à appeler périodiquement
int16_t quadEncoder_GetSpeedR(void)
void quadEncoder_CallbackIndexL(void)
- Fonction de Callback appelée depuis l’interruption EXTI générée par l’index
- mise à jour de la variable indexL
void quadEncoder_CallbackIndexR(void)

Pour une période d’échantillonage de 5ms, la vitesse mesurée est en tr/min ( rpm ).

code quadEncoder

quadEncoder.h

/*
 * QuadEncoder.h
 */

#ifndef INC_QUADENCODER_H_
#define INC_QUADENCODER_H_

#include "main.h"

void quadEncoder_Init(void);
int16_t quadEncoder_GetPos16L(void);
int16_t quadEncoder_GetPos16R(void);
int32_t quadEncoder_GetPos32L(void);
int32_t quadEncoder_GetPos32R(void);
int16_t quadEncoder_GetSpeedL(void);
int16_t quadEncoder_GetSpeedR(void);
void quadEncoder_CallbackIndexL(void);
void quadEncoder_CallbackIndexR(void);
void quadEncoder_PosCalcL(int*);
void quadEncoder_PosCalcR(int*);

#endif /* INC_QUADENCODER_H_ */

quadEncoder.c

/*
 * QuadEncoder.c
 */
#include "quadEncoder.h"

#define TE_ms	SAMPLING_PERIOD_ms
#define USE_QUAD_ENCODER_1250_CPR 1

#if USE_QUAD_ENCODER_1250_CPR
#define COUNT_PER_ROUND 1250
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*6555)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF	6555
#endif

#if USE_QUAD_ENCODER_1000_CPR
#define COUNT_PER_ROUND 1000
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*8192)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF 8192
#endif

#if USE_QUAD_ENCODER_500_CPR
#define COUNT_PER_ROUND 500
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*16392)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF 16392
#endif

#if USE_QUAD_ENCODER_250_CPR
#define COUNT_PER_ROUND 250
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*32768)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF 32768
#endif



TIM_HandleTypeDef    TimEncoderHandleLeft;
TIM_HandleTypeDef    TimEncoderHandleRight;

/*******************************************************************************
 * TIMER 1, CHANNEL 1 et 2  --> RIGHT
 * TIMER 2, CHANNEL 1 et 2	--> LEFT
****************************************************************************** */
int indexL=0;
static int indexR=0;

//================================================================
//		TIMER INIT
//================================================================

void quadEncoder_Init(void)
{
	TIM_Encoder_InitTypeDef sConfig;
	//--------------------------------------------------
	// TIMER 1
	//--------------------------------------------------
	TimEncoderHandleLeft.Instance = TIM1;
	TimEncoderHandleLeft.Init.Prescaler = 0;
	TimEncoderHandleLeft.Init.CounterMode = TIM_COUNTERMODE_UP;
	TimEncoderHandleLeft.Init.Period = COUNT_PER_ROUND*4;
	TimEncoderHandleLeft.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;

	sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
	sConfig.IC1Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
	sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
	sConfig.IC1Prescaler = TIM_ICPSC_DIV4;
	sConfig.IC1Filter = 0x0F;
	sConfig.IC2Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
	sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;//TIM_ICSELECTION_DIRECTTI; //TIM_TI1SELECTION_XORCOMBINATION
	sConfig.IC2Prescaler = TIM_ICPSC_DIV4;
	sConfig.IC2Filter = 0x0F;

	HAL_TIM_Encoder_Init(&TimEncoderHandleLeft, &sConfig);

	__HAL_TIM_SetCounter(&TimEncoderHandleLeft, 0);

	HAL_TIM_Encoder_Start(&TimEncoderHandleLeft,TIM_CHANNEL_1);
	HAL_TIM_Encoder_Start(&TimEncoderHandleLeft,TIM_CHANNEL_2);

	//--------------------------------------------------
	// TIMER 2
	//--------------------------------------------------
	TimEncoderHandleRight.Instance = TIM2;
	TimEncoderHandleRight.Init.Prescaler = 0;
	TimEncoderHandleRight.Init.CounterMode = TIM_COUNTERMODE_UP;
	TimEncoderHandleRight.Init.Period = COUNT_PER_ROUND*4;
	TimEncoderHandleRight.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;

	sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
	sConfig.IC1Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
	sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
	sConfig.IC1Prescaler = TIM_ICPSC_DIV4;
	sConfig.IC1Filter = 0x0F;
	sConfig.IC2Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
	sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;//TIM_ICSELECTION_DIRECTTI; //TIM_TI1SELECTION_XORCOMBINATION
	sConfig.IC2Prescaler = TIM_ICPSC_DIV4;
	sConfig.IC2Filter = 0x0F;

	HAL_TIM_Encoder_Init(&TimEncoderHandleRight, &sConfig);

	__HAL_TIM_SetCounter(&TimEncoderHandleRight, 0);

	HAL_TIM_Encoder_Start(&TimEncoderHandleRight,TIM_CHANNEL_1);
	HAL_TIM_Encoder_Start(&TimEncoderHandleRight,TIM_CHANNEL_2);
}

//================================================================
//		POSITION LEFT CALC
//================================================================

void quadEncoder_PosCalcL(int* AngPos)
{

int POSCNTcopy = 0;
POSCNTcopy = (int)TIM1->CNT;
AngPos[1] = AngPos[0];
AngPos[0] = (unsigned int)(((unsigned long)POSCNTcopy * COEFF)/1000); // 0 <= POSCNT <= 4999 to 0 <= AngPos <= 32767
}

//================================================================
//		POSITION RIGHT CALC
//================================================================

void quadEncoder_PosCalcR(int* AngPos)
{

int POSCNTcopy = 0;
POSCNTcopy = (int)TIM2->CNT;
AngPos[1] = AngPos[0];
AngPos[0] = (unsigned int)(((unsigned long)POSCNTcopy * COEFF)/1000); // 0 <= POSCNT <= 4999 to 0 <= AngPos <= 32767
}

//================================================================
//		POSITION LEFT 16 BITS
//================================================================

int16_t quadEncoder_GetPos16L(void)
{
	uint16_t PosL = 0;
	PosL=TIM1->CNT;
	return (int16_t)PosL;

	}

//================================================================
//		POSITION RIGHT 16 BITS
//================================================================

int16_t quadEncoder_GetPos16R(void)
{
	uint16_t PosR = 0;
	PosR=TIM2->CNT;
    return (int16_t)PosR;
}
//================================================================
//		POSITION LEFT 32 BITS	(pos 16 bits + nombre de tours)
//================================================================

int32_t quadEncoder_GetPos32L(void)
{
	int32_t  PosL = 0;
	PosL=indexL*4*COUNT_PER_ROUND + (int32_t) quadEncoder_GetPos16L();
	return PosL;
}

//================================================================
//		POSITION RIGHT 32 BITS	(pos 16 bits + nombre de tours)
//================================================================

int32_t quadEncoder_GetPos32R(void)
{
	int32_t PosR = 0;
	PosR=indexR*4*COUNT_PER_ROUND + (int32_t) quadEncoder_GetPos16R();
	return PosR;
}

//================================================================
//		SPEED LEFT
//--> must be called every Te
//================================================================

int16_t quadEncoder_GetSpeedL(void)
{
	static int AngPos[2] = {0,0};
	static int16_t SpeedL=0;

	quadEncoder_PosCalcL(AngPos);
	SpeedL = AngPos[0] - AngPos[1];
	if (SpeedL >= 0)
	{
		if (SpeedL >= HALF_MAX_COUNT)
			{
			SpeedL = SpeedL - MAX_COUNT;
			}
	}
	else
	{
		if (SpeedL < -HALF_MAX_COUNT)
			{
			SpeedL = SpeedL + MAX_COUNT;
			}
	}
	
	//***********************************
	// CONVERT RPM
	// 1 tour = 32767
	// Nbre de Tours pendant Te: DELTA_pos/32767
	// Nbre de Tours pendant 1s (Te en ms) : (DELTA_pos/32767)*(1000/Te)
	// Nbre de Tours par minute : : (DELTA_pos/32767)*((60*1000)/Te)

	SpeedL=(SpeedL*60*1000)/(32767*TE_ms);
	return SpeedL;
}

//================================================================
//		SPEED RIGHT
//-->  must be called every Te
//================================================================

int16_t quadEncoder_GetSpeedR(void)
{
	static int AngPos[2] = {0,0};
	static int16_t SpeedR=0;


	quadEncoder_PosCalcR(AngPos);
	SpeedR = AngPos[0] - AngPos[1];
	if (SpeedR >= 0)
	{
		if (SpeedR >= HALF_MAX_COUNT)
			{
			SpeedR = SpeedR - MAX_COUNT;
			}
	}
	else
	{
		if (SpeedR < -HALF_MAX_COUNT)
			{
			SpeedR = SpeedR + MAX_COUNT;
			}
	}
	//***********************************
	// CONVERT RPM
	// 1 tour = 32767
	// Nbre de Tours pendant Te: DELTA_pos/32767
	// Nbre de Tours pendant 1s (Te en ms) : (DELTA_pos/32767)*(1000/Te)
	// Nbre de Tours par minute : : (DELTA_pos/32767)*((60*1000)/Te)

	SpeedR=(SpeedR*60*1000)/(32767*TE_ms);
	return SpeedR;
}

//================================================================
//		MAJ index Left
//================================================================

void quadEncoder_CallbackIndexL()
{
				if (__HAL_TIM_DIRECTION_STATUS(&TimEncoderHandleLeft)==1)
				{
					indexL--;
				}
				else
				{
					indexL++;
				}


				__HAL_TIM_SetCounter(&TimEncoderHandleLeft, 0);		// RAZ Counter
				HAL_TIM_Encoder_Start(&TimEncoderHandleLeft,TIM_CHANNEL_1);
				HAL_TIM_Encoder_Start(&TimEncoderHandleLeft,TIM_CHANNEL_2);

}
//================================================================
//		MAJ index Right
//================================================================

void quadEncoder_CallbackIndexR()
{
				if (__HAL_TIM_DIRECTION_STATUS(&TimEncoderHandleRight)==1)
				{
					indexR--;

				}
				else
				{
					indexR++;
				}

				__HAL_TIM_SetCounter(&TimEncoderHandleRight, 0);			// RAZ Counter
				HAL_TIM_Encoder_Start(&TimEncoderHandleRight,TIM_CHANNEL_1);
				HAL_TIM_Encoder_Start(&TimEncoderHandleRight,TIM_CHANNEL_2);

}
//================================================================

Mise en Oeuvre d’un Correcteur PI

Comment commander ? –> motoCommand.c :
- motorLeft_SetDuty(int duty)
- motorRight_SetDuty(int duty)
Comment Mesurer ? –> quadEncoder.c :
- int16_t quadEncoder_GetSpeedL(void)
- int16_t quadEncoder_GetSpeedR(void)

La mise en oeuvre d’un correcteur pour le robot est présenté dans : Réglage (rapide) d’un correcteur PI

[STM32] Carte Nucleo F411 [STM32] Real Time Operating System (RTOS)