(转)正交编码器在stm32中的应用 Quadrature encoders with the STM32F4

Most commonly, the speed and direction of the motors in a micromouse or other small robot will be read from quadrature encoders connected to the drive train. One of the great things about the STM32F4 processor is that it can read these directly.
 
Quadrature Encoder Signals
 
A quadrature encoder generates a series of pulses on two channels. the pulses are likely to be square waves, out of phase by 90 degrees. Thus, the signals are said to be in quadrature.

quadrature1.gif

To get the greatest resolution out of this pulse train, the edges can be counted. You get four edges per complete cycle. The order and relative arrangement of the edges tell you what direction the motor is turning and, clearly, the frequency of the edges, tells you the speed. I have a page elsewhere that describes the whole business in a bit more detail:
 
There exist a number of ready-to-go chips that can convert the output of incremental (quadrature) encoders into counts, count and direction or count-up and count-down signals. This projects uses an 8-pin ATtiny processor to flexibly perform this conversion for a fraction of the cost of commercial devices.

Several applications make use of incremental shaft encoders. Many of the ‘soft’ front panel controls on electronic equipment use them. The4se are the kind that spin without stops – a spin clockwise increases a quantity, a spin anticlockwise decreases it. Motor speed/position sensors also produce quadrature signals as the shaft is rotated. A mechanical mouse has a slotted wheel and a pair of sensors that generate quadrature signals when the mouse is moved. The chip inside the mouse is a simple processor which can detect this quadrature signal and keep count of distance and direction, communicating the results periodically with the main computer.

A typical, ideal two-channel quadrature signal looks like this:

View this as a time-dependant graph as if on an oscilloscope. The sequence begins at the left.
1X decoding
The simplest way to use the output would be to take channel A as the input to a counter. However, that would tell us nothing about the direction of the count. Suppose however that the counter increments on the rising edge of A. You will be able to see that if we are scanning from left to right, immediately after a rising edge on channel A, channels A and B have different values. Equally, if we reverse the direction and scan from right to left, after a rising edge on channel A, both channels have the same value. Thus, we can test for equality of channels A and B to determine the direction while channel A generates the count. This scheme works very well. even if your encoder dithers back and forward around a channel A edge, the count simply increments and decrements alternately.

2X decoding
A shortcoming of the previous method is that the count frequency is the same as the frequency of channel A. Thus, an encoder said to have a resolution of 500 counts per revolution (cpr) does exactly that. We can do better by using both edges of Channel A . This is not too hard to arrange in hardware but this uses up valuable board space. The equality test described just now works just as well if we are detecting falling edges. Thus we can use the same routine for both rising and falling edges and detect twice as many transitions. With 2X decoding our 500 cpr encoder can generate 1000 counts per revolution.

4x decoding
It is possible to do even better if we examine the edges of both channel A and channel B. There are four edges for each phase of channel A and it is possible to get 2000 counts per revolution from our 500 cpr encoder.

If you have a processor with the ability to generate an interrupt on pin change it is pretty simple to get the count we want. I had a number of Atmel AVR ATtiny13 processors available which are 8 pin devices with an internal 9.6MHz clock. Channel A is connected to the INT0 pin and channel B is connected to the INT1 pin. Only the INT0 interrupt is used here as I needed only 1X or 2X decoding. The ATtiny can generate an interrupt on either a rising or a trailing edge. For 2X decoding, the sense of the interrupt is changed after each interrupt so that the routine responds alternately to rising and falling edges. On each interrupt, the direction of the count is determined and a pulse generated on either the count-up or the count-down pin as appropriate. On the processor used, pulse rates as high as 150kHz should be readily detectable. The interrupt routine takes less than 4.5us to execute and rates as high as 220kHz may be possible with inputs that are in perfect quadrature. That is unlikely in practice.
Here, I just want to look at how such an encoder can be read with the STM32F4 processor. Several of the timers on this device can be used to decode quadrature encoder signals. A key feature for micromouse and robot builders is that the STM32F4 can handle several encoders at once. Many other processors can only do one encoder.

The Code

The first job is to identify a timer that can be used with the quadrature encoder and work out which pins are to be connected to the encoder. The pin assignment on the STM32F4 is very flexible. You need channel 1 and channel 2 for a given timer to allow decoding of the pulses. For my convenience, I use macros in a hardware description header file to describe which timers, channels and pins are to be used. Here is part of that file for Decimus 4:
 /*
* definitions for the quadrature encoder pins
*/
// Left Motor Channels
#define ENCLA_PIN GPIO_Pin_15
#define ENCLA_GPIO_PORT GPIOA
#define ENCLA_GPIO_CLK RCC_AHB1Periph_GPIOA
#define ENCLA_SOURCE GPIO_PinSource15
#define ENCLA_AF GPIO_AF_TIM2

#define ENCLB_PIN GPIO_Pin_3
#define ENCLB_GPIO_PORT GPIOB
#define ENCLB_GPIO_CLK RCC_AHB1Periph_GPIOB
#define ENCLB_SOURCE GPIO_PinSource3
#define ENCLB_AF GPIO_AF_TIM2

// Right Motor Channels
#define ENCRA_PIN GPIO_Pin_6
#define ENCRA_GPIO_PORT GPIOB
#define ENCRA_GPIO_CLK RCC_AHB1Periph_GPIOB
#define ENCRA_SOURCE GPIO_PinSource6
#define ENCRA_AF GPIO_AF_TIM4

#define ENCRB_PIN GPIO_Pin_7
#define ENCRB_GPIO_PORT GPIOB
#define ENCRB_GPIO_CLK RCC_AHB1Periph_GPIOB
#define ENCRB_SOURCE GPIO_PinSource7
#define ENCRB_AF GPIO_AF_TIM4

// determine the timers to use
#define ENCL_TIMER TIM2
#define ENCL_TIMER_CLK RCC_APB1Periph_TIM2
#define ENCR_TIMER TIM4
#define ENCR_TIMER_CLK RCC_APB1Periph_TIM4

#define LEFT_COUNT() ENCL_TIMER->CNT
#define RIGHT_COUNT() ENCR_TIMER->CNT
It may all seem a bit wordy but changing the quadrature encoder hardware is easy without touching the actual code.

Now the peripherals need to be set up on the processor. With the STM32F4, this is a relatively complex process. You can do the setup directly or use the Standard Peripheral Library. I use the library as it saves me from making some simple mistakes. Code efficiency here is not really important as the setup is done only once. The intent should be clear enough from the comments so I won’t labour the details here:
/*
* Configure two timers as quadrature encoder counters.
* Details of which timers should be used are
* in the project hardware header file.
* Most timers can be used if channels 1 and 2 are available on pins.
* The timers are mostly 16 bit. Timers can be set to 32 bit but they are
* not very convenient for IO pins so the counters are simply set to to
* 16 bit counting regardless.
* A mouse needs 32 bits of positional data and, since it also needs the
* current speed, distance is not maintained by the encoder code but will
* be looked after by the motion control code.
* The counters are set to X4 mode. The only alternative is X2 counting.
*/
void encodersInit (void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// turn on the clocks for each of the ports needed
RCC_AHB1PeriphClockCmd (ENCLA_GPIO_CLK, ENABLE);
RCC_AHB1PeriphClockCmd (ENCLB_GPIO_CLK, ENABLE);
RCC_AHB1PeriphClockCmd (ENCRA_GPIO_CLK, ENABLE);
RCC_AHB1PeriphClockCmd (ENCRB_GPIO_CLK, ENABLE);

// now configure the pins themselves
// they are all going to be inputs with pullups
GPIO_StructInit (&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_InitStructure.GPIO_Pin = ENCLA_PIN;
GPIO_Init (ENCLA_GPIO_PORT, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = ENCLB_PIN;
GPIO_Init (ENCLB_GPIO_PORT, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = ENCRA_PIN;
GPIO_Init (ENCRA_GPIO_PORT, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = ENCRB_PIN;
GPIO_Init (ENCRB_GPIO_PORT, &GPIO_InitStructure);

// Connect the pins to their Alternate Functions
GPIO_PinAFConfig (ENCLA_GPIO_PORT, ENCLA_SOURCE, ENCLA_AF);
GPIO_PinAFConfig (ENCLB_GPIO_PORT, ENCLB_SOURCE, ENCLB_AF);
GPIO_PinAFConfig (ENCRA_GPIO_PORT, ENCRA_SOURCE, ENCRA_AF);
GPIO_PinAFConfig (ENCRB_GPIO_PORT, ENCRB_SOURCE, ENCRB_AF);

// Timer peripheral clock enable
RCC_APB1PeriphClockCmd (ENCL_TIMER_CLK, ENABLE);
RCC_APB1PeriphClockCmd (ENCR_TIMER_CLK, ENABLE);

// set them up as encoder inputs
// set both inputs to rising polarity to let it use both edges
TIM_EncoderInterfaceConfig (ENCL_TIMER, TIM_EncoderMode_TI12,
TIM_ICPolarity_Rising,
TIM_ICPolarity_Rising);
TIM_SetAutoreload (ENCL_TIMER, 0xffff);
TIM_EncoderInterfaceConfig (ENCR_TIMER, TIM_EncoderMode_TI12,
TIM_ICPolarity_Rising,
TIM_ICPolarity_Rising);
TIM_SetAutoreload (ENCR_TIMER, 0xffff);

// turn on the timer/counters
TIM_Cmd (ENCL_TIMER, ENABLE);
TIM_Cmd (ENCR_TIMER, ENABLE);
encodersReset();
}
Once this has run, the quadrature encoders will be working away and all you need to do is read the appropriate registers. On the micromouse, I want to also keep track of speed and distance for each wheel as well as for the overall forward and rotational motion. Further, there needs to be a way to set everything to zero:
void encodersReset (void)
{
__disable_irq();
oldLeftEncoder = 0;
oldRightEncoder = 0;
leftTotal = 0;
rightTotal = 0;
fwdTotal = 0;
rotTotal = 0;
TIM_SetCounter (ENCL_TIMER, 0);
TIM_SetCounter (ENCR_TIMER, 0);
encodersRead();
__enable_irq();
}
Notice that interrupts are turned off while resetting the encoder variables. This is needed here because the mouse software normally reads the encoders every millisecond in the systick event and we can do without getting that confused. There is actually no need to set the counters to zero. Possibly, it might be best to disable the timers while resetting the counters but it does not seem to cause trouble.

Using the counters is now quite simple. A number of variables are used to keep track of the important variables:
/available to the rest of the code
//speeds
volatile int16_t leftCount;
volatile int16_t rightCount;
volatile int16_t fwdCount;
volatile int16_t rotCount;
//distances
volatile int32_t leftTotal;
volatile int32_t rightTotal;
volatile int32_t fwdTotal;
volatile int32_t rotTotal;

// local variables
static volatile int16_t oldLeftEncoder;
static volatile int16_t oldRightEncoder;
static volatile int16_t leftEncoder;
static volatile int16_t rightEncoder;
static volatile int16_t encoderSum;
static volatile int16_t encoderDiff;
At each systick, the following function is called to update everything:
void encodersRead (void)
{
oldLeftEncoder = leftEncoder;
leftEncoder = TIM_GetCounter (ENCL_TIMER) ;
oldRightEncoder = rightEncoder;
rightEncoder = -TIM_GetCounter (ENCR_TIMER) ;
leftCount = leftEncoder - oldLeftEncoder;
rightCount = rightEncoder - oldRightEncoder;
fwdCount = leftCount + rightCount;
rotCount = - (leftCount - rightCount);
fwdTotal += fwdCount;
rotTotal += rotCount;
leftTotal += leftCount;
rightTotal += rightCount;
}
From the declarations above, you will see that the count and speed variables are 16 bit signed values. The counters themselves are set up as 16 bit but they don’t care about the subtleties of signed vs unsigned variables. So long as you are consistent, the arithmetic will work out. The only time you might get into trouble is if you read the counters too infrequently. For my mouse, I am getting about 200 counts per mm which is a bit high. REading them every millisecond means that I would have to go at about 160 m/s (half the speed of sound) before they counters overflowed so that should not be a problem.

Distances are 32 bit because a 16 bit variable would only be able to keep track of distances up to about 160mm.

It is entirely possible that I have overlooked something important here, or that I could have done it better. Suggestions are welcome. For now though, it all seems to work quite nicely.
 
 
已邀请:

admin

赞同来自:

楼下复制一些歪果仁的相关提问和回复!
Thanks for sharing Pete!
Why your encoder only goes up to 160mm where mine can go mo than that? is that because of your gear ratio? I use 12:40 gear ratio and it only takes about 24000/65535 counts per 180mm and my wheel diameter is 24.5mm.
I’ve noticed that the TIMx->CNT in STM32F4 will reload when exceeding max encoder count(ex, if encoder turning backwards and make encoder TIMx->CNT smaller than 0, then it will starts to count down from 65535 again) when STM32F3 can’t do this and will will just freeze my printf output to indicate the actual encoder outputs.
If your mouse can only go 160mm per 65535 counts, you may need to consider to use both TIM2 and TIM5 for their native 32bit resolution :), even though the TIM2 and TIM5 pins are separated on 2 sides of the chip:)

admin

赞同来自:

yeah, it’s weird, I copy the exactly same code from STM32F103 to F4 for encoder and F4 TIM->CNT doesn’t freeze the MCU when F103 does. So far I am safe as long as I reset the TIM->CNT before it overflows, but I really want to know if then TIM->CNT does freeze the MCU for other ppl :). I also fount a very interesting thing on my code: if I call encoder_config() before motor_pwm_config, the pwm won’t output unless I swap the order of these setup functions. My encoder use TIM3, TIM2_remap, motor use TIM8, how does this possible that calling the setup code for one timer will disable other timer LOL.

admin

赞同来自:

hi, nice work and many thanks to you for sharing this with us. If you can, please post the code of the motor controlling part (PID).
thanks again
老外的伸手党也挺多的....哈哈

admin

赞同来自:

 think timer for STM32F4 will auto reload, it will freeze the MCU for STM32F3 but it will be fine for F4 since I tested. the one I tested for STM32 F4 is 32bit timer, I dont know the results for 16bit timer, but they should be same.

admin

赞同来自:

Sorry Peter for pointing this out, but don’t you think your encoder reading code is inefficient? First you make the substraction to get the count and then some more additions, instead of just overwriting the encoder count with a 0 every time you read the encoders? Pseudocode to show you what I mean:

init_encoders();
TIM3->CNT=0;
TIM4->CNT=0;

each 1ms:
Read encoders{
leftCount=(s16)TIM3->CNT;
rightCount=(s16)TIM4->CNT;
TIM3->CNT=0;
TIM4->CNT=0;
leftTotal+=leftCount
rightTotal+=rightCount
etc…
}
Isn’t this a faster solution?

admin

赞同来自: ZJZTO

A have a problem my encoder can not take me a true results plz help me
this is my program with keil
#include “stm32f4_discovery.h”

//***********************************************
// Configuation générale
//***********************************************
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
//***********************************************
// Appelle des fontions de configuration
//***********************************************
void GPIO_Config(void);
void PWM_Config(int period);
void TIM_Config(void);
void Delay(__IO uint32_t nCount);
void servo_1(int deg1);
void servo_2(int deg2);
void servo_3(int deg3);
void servo_4(int deg4);
void moteur_PWM(uint16_t mdav,uint16_t mdar, uint16_t mgav, uint16_t mgar);
void configureEncoder(void);
uint32_t offsetencoder=0x7fffffff;
long long encoder1=0;
long long encoder2=0;
/*****************************************/
void encodersReset (void) ;
#define LEFT_COUNT() TIM2->CNT
#define RIGHT_COUNT() TIM5->CNT
volatile int16_t leftCount;
volatile int16_t rightCount;
volatile int16_t fwdCount;
volatile int16_t rotCount;
//distances
volatile int32_t leftTotal;
volatile int32_t rightTotal;
volatile int32_t fwdTotal;
volatile int32_t rotTotal;
// local variables static
volatile int16_t oldLeftEncoder;
static volatile int16_t oldRightEncoder;
static volatile int16_t leftEncoder;
static volatile int16_t rightEncoder;
static volatile int16_t encoderSum;
static volatile int16_t encoderDiff;

/*******************************************/
uint16_t CCR1_Val = 0; //pc6
uint16_t CCR2_Val = 0; //PC7
uint16_t CCR3_Val = 0; //PB0
uint16_t CCR4_Val = 0; //PB1
uint16_t PrescalerValue = 0;
int deg;
//***********************************************
// Programme principale
//***********************************************
int main(void)
{

GPIO_Config();
TIM_Config();
PWM_Config(2000); // Il faut saisir la période

while (1) //*Equivalent a void(loop) a arduino*//
{ oldLeftEncoder = leftEncoder;
leftEncoder = TIM_GetCounter (TIM2) ;
oldRightEncoder = rightEncoder;
rightEncoder = -TIM_GetCounter (TIM5) ;
leftCount = leftEncoder – oldLeftEncoder;
rightCount = rightEncoder – oldRightEncoder;
fwdCount = leftCount + rightCount;
rotCount = – (leftCount – rightCount);
fwdTotal += fwdCount;
rotTotal += rotCount;
leftTotal += leftCount;
rightTotal += rightCount;

}

}

//***********************************************
// Fonction pour configuration GPIO
//***********************************************
void GPIO_Config(void)
{
/* GPIOC and GPIOB clock enable */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOB| RCC_AHB1Periph_GPIOD |RCC_AHB1Periph_GPIOA, ENABLE);

/****************TIM3*********************/

/* GPIOC Configuration: TIM3 CH1 (PC6) and TIM3 CH2 (PC7) */ /* ===>Moteur 1 */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
GPIO_Init(GPIOA, &GPIO_InitStructure);

/* GPIOB Configuration: TIM3 CH3 (PB0) and TIM3 CH4 (PB1) */ /* ===>Moteur 2 */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
GPIO_Init(GPIOB, &GPIO_InitStructure);

/****************TIM4*********************/

/* GPIOB Configuration: TIM4 CH1 (PD12) and TIM4 CH2 (PD13) and TIM4 CH3 (PD14) and TIM4 CH4 (PD15) */ /* ===>ServoMoteur */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13| GPIO_Pin_14| GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
GPIO_Init(GPIOD, &GPIO_InitStructure);

/****************TIM2*********************/

/* GPIOB Configuration: TIM2 CH1 (PA15) and TIM2 CH3(PA2) and TIM2 CH4 (PA3)*/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2| GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/******encoder 1*********/
/* GPIOB Configuration: TIM2 CH1 (PB3)*/
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_3;
GPIO_Init(GPIOB, &GPIO_InitStructure);

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_15;
GPIO_Init(GPIOA, &GPIO_InitStructure);

/****************TIM8*********************/

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
GPIO_Init(GPIOC, &GPIO_InitStructure);

/****************TIM1*********************/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
GPIO_Init(GPIOE, &GPIO_InitStructure);
/****************TIM5*********************/
/******encoder 2*********/
/* GPIOB Configuration: TIM5*/
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_0 | GPIO_Pin_1;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}

//***********************************************
// Fonction pour configuration TIMER
//***********************************************
void TIM_Config(void)
{
/* TIM3 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
/* TIM4 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
/* TIM2 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
/* TIM8 clock enable */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
/* TIM1 clock enable */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
/* TIM5 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);

/* Connect TIM3 pins to AF2 */
GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3); // Pin Moteur 1 Sens 1
GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_TIM3); // Pin Moteur 1 Sens 2
GPIO_PinAFConfig(GPIOB, GPIO_PinSource0, GPIO_AF_TIM3); // Pin Moteur 2 Sens 1
GPIO_PinAFConfig(GPIOB, GPIO_PinSource1, GPIO_AF_TIM3); // Pin Moteur 2 Sens 2

/* Connect TIM4 pins to AF2 */
GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4); //Pin du servo 1
GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4); //Pin du servo 2
GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_TIM4); //Pin du servo 3
GPIO_PinAFConfig(GPIOD, GPIO_PinSource15, GPIO_AF_TIM4); //Pin du servo 4

/* Connect TIM2 pins to AF2 */
GPIO_PinAFConfig(GPIOA, GPIO_PinSource15, GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource3, GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM2);

/* Connect TIM8 pins to AF2 */
GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_TIM8);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource7, GPIO_AF_TIM8);

/* Connect TIM1 pins to AF2 */
GPIO_PinAFConfig(GPIOC, GPIO_PinSource9, GPIO_AF_TIM1);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource11, GPIO_AF_TIM1);
/* Connect TIM5 pins to AF2 */
GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM5);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_TIM5);
}

//***********************************************
// Fonction pour configuration PWM
//***********************************************
void PWM_Config(int period)
{
/********************************/
/*******Prametrage du PWM********/
/*******************************/

/* Compute the prescaler value */
PrescalerValue = (uint16_t) ((SystemCoreClock /2) / 16000000) – 1;

/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = period ;
TIM_TimeBaseStructure.TIM_Prescaler = 500;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

/*******Prametrage des Timeurs********/

TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
/*****************************************************************/
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
/* Configure the timer */
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_SetAutoreload (TIM2, 0xffff);
TIM_EncoderInterfaceConfig(TIM5, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_SetAutoreload (TIM5, 0xffff);
/* TIM2 and TIM5 counter enable */
TIM_Cmd(TIM2, ENABLE);
TIM_Cmd(TIM5, ENABLE);
encodersReset ();
/*******Prametrage des Chanels********/

/*******TIM3******/
/* PWM1 Mode configuration: Channel1 TIM3*/
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR1_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC1Init(TIM3, &TIM_OCInitStructure);
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);

/* PWM1 Mode configuration: Channel2 TIM3*/
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR2_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC2Init(TIM3, &TIM_OCInitStructure);
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel3 TIM3*/
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR3_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC3Init(TIM3, &TIM_OCInitStructure);
TIM_OC3Init(TIM4, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);

/* PWM1 Mode configuration: Channel4 TIM3*/
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR4_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC4Init(TIM3, &TIM_OCInitStructure);
TIM_OC4Init(TIM4, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);

/*Actication des TIM*/
TIM_ARRPreloadConfig(TIM3, ENABLE);
TIM_ARRPreloadConfig(TIM4, ENABLE);

/***********TIM4*********/
/* PWM1 Mode configuration: Channel1 TIM4 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR1_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC1Init(TIM3, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel2 TIM4 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR2_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC2Init(TIM3, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel3 TIM4 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR3_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC3Init(TIM4, &TIM_OCInitStructure);
TIM_OC3Init(TIM3, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel4 TIM4 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = CCR4_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC4Init(TIM4, &TIM_OCInitStructure);
TIM_OC4Init(TIM3, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);

/*Actication des TIM*/
TIM_ARRPreloadConfig(TIM3, ENABLE);
TIM_ARRPreloadConfig(TIM4, ENABLE);
TIM_ARRPreloadConfig(TIM8, ENABLE);

/* TIM3 enable counter */
TIM_Cmd(TIM3, ENABLE);
TIM_Cmd(TIM4, ENABLE);
TIM_Cmd(TIM8, ENABLE);
TIM_Cmd(TIM1, ENABLE);

}
//***********************************************
// Fonction pour configuration du delay
//***********************************************
void Delay(__IO uint32_t nCount)
{
while(nCount–)
{
}
}
//***********************************************
// Fonction pour controler les servo moteur
//***********************************************
void servo_1(int deg1)
{
TIM4->CCR1 =(uint16_t)(deg1*1000)/180; // Servo pinD12 TIM4
}
void servo_2(int deg2)
{
TIM4->CCR2 =(uint16_t)(deg2*1000)/180; // Servo pinD13 TIM4
}
void servo_3(int deg3)
{
TIM4->CCR1 =(uint16_t)(deg3*1000)/180; // Servo pinD14 TIM4
}
void servo_4(int deg4)
{
TIM4->CCR4 =(uint16_t)(deg4*1000)/180; // Servo pinD15 TIM4
}
//***********************************************
// Fonction pour controler deux moteur
//***********************************************
void moteur_PWM(uint16_t mdav,uint16_t mdar, uint16_t mgav, uint16_t mgar)
{
TIM3->CCR1 =(uint16_t)(mdav); // PA6 TIM3
TIM3->CCR2 =(uint16_t)(mdar); //PA7 TIM3
TIM3->CCR3 = (uint16_t)(mgav); //PB0 TIM3
TIM3->CCR4 = (uint16_t)(mgar); //PB1 TIM3
}

void encodersReset (void) {
oldLeftEncoder = 0;
oldRightEncoder = 0;
leftTotal = 0;
rightTotal = 0;
fwdTotal = 0;
rotTotal = 0;
TIM_SetCounter (TIM2, 0);
TIM_SetCounter (TIM3, 0);
oldLeftEncoder = leftEncoder;
leftEncoder = TIM_GetCounter (TIM2) ;
oldRightEncoder = rightEncoder;
rightEncoder = -TIM_GetCounter (TIM5) ;
leftCount = leftEncoder – oldLeftEncoder;
rightCount = rightEncoder – oldRightEncoder;
fwdCount = leftCount + rightCount;
rotCount = – (leftCount – rightCount);
fwdTotal += fwdCount;
rotTotal += rotCount;
leftTotal += leftCount;
rightTotal += rightCount;
}

要回复问题请先登录注册