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.

Quadrature Encoder Pulse Train

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:

http://www.helicron.net/avr/quadrature-decoders/

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.

This entry was posted in Hardware, Micromouse, STM32 and tagged , . Bookmark the permalink.

23 Responses to Quadrature encoders with the STM32F4

  1. Green says:

    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:)

  2. Peter Harrison says:

    I was mixing up numbers a bit but –

    Each wheel gives about 103 counts per mm on the ground. I add the two together for forward motion. So, for every mm of forward travel, I have about 206 counts. Using signed 16 bit values to hold that would allow a range of +/- 32768 or so which would be +/- 157.7 mm.

    Now this is just for the variables used for obtaining counts in each systick event because the counters can only be relied upon to give 16 bit values and I don’t want to be restricted to those counters that have 32 bit counters. Those numbers only apply in the systick event.

    In the overall controller code, 32 bit variables are used with 24.8 fixed point. Thus I would have a range of distances of about +/- 40000 mm.

    Actually, in this mouse, they will be converted into real values as floats because it makes everything easier to keep track of when the code always refers to real values like mm and degrees.

    Your counter register should wrap around just fine so I am not sure why you have trouble with that.

  3. Green says:

    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.

  4. Peter Harrison says:

    Hard to say without seeing your code but –

    If you are using a single, global TIM_TimeBaseInitStruct, you may be carrying values over into a different peripheral. Try using TIM_TimeBaseStructInit ( ) and TIM_DeInit() before setting up the timers.

    Also, remember that, if you declare an InitStructure inside a function, its contents are undefined whereas, if it is declared with file or program scope, it will be initialised to zero by the startup code.

  5. Kasun says:

    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

  6. Peter Harrison says:

    I have previous posts about motor controllers. This one describes a PD controller.
    This one describes a more complex but better performing phase lead controller.

  7. Kasun says:

    Yes i red that.. you did a tutorial about a possition controller.. actually how do you control the speed of the robot ? with possition.? i m a little confused on that.. can you upload a sample code?

  8. Kasun says:

    timer counters will get overflowed when it comes to possition control. how do you manage that?

  9. Peter Harrison says:

    I may do a post on motion control sometime son but this is not the place for It I am afraid.

    Basically though, you generate a series of speeds ramping up to some maximum and back down to zero. At each control cycle, the speed is added to the setpoint of the motors so that the set point moves on. The controller then tries to get the wheels caught up with that set point.

    Don’t worry about the counters overflowing. The arithmetic still works if you just follow the rules. Try and work it out on paper with smaller numbers. Try a four bit counter, do the arithmetic by hand and look at the results. So long as each cycle does not do more than half the counter range, it works just fine.

  10. Green says:

    I 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.

  11. Kasun says:

    Thanks for the heads up. I did it n work fine :). Thanks again

  12. Marcel says:

    Anybody have an idea if you can also check on encoder phase errors on the stm32F4. We want to detect if encoder signal is good or not.

  13. mog123 says:

    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?

  14. Peter Harrison says:

    I expect you are right. Certainly, many others do this apparently without problems. However, I can’t help but think there is a possibility that edges migt be lost this way so I accept the slight inefficiency.

  15. sami says:

    PLS describe the connctions between the encoder and STM32F4.
    PLS give number example to understand how you calc the speed and disance and direcation.

  16. Peter Harrison says:

    I think all the answers to your question are in the code listings above.

  17. MrLinh says:

    your code is verry good. if we use mog123’Code, we will count not enoup pull or wrong . Becaul if you erase timer->counter will be loss.

  18. Ken Boak says:

    Peter

    Thanks for useful guide to quadrature decoding.

    I’ve got TIM2 and TIM5 working as 32 bit QE channels. Is it possible to chain together TIM 3 and TIM 4 to give another 32 bit channel, or is is best to just use them as 16 bit quadrature encoders and extend them to 32 bit counts in software. Is this the approach you take?

    Ken Boak

  19. Peter Harrison says:

    I have not looked into timer chaining. For my purposes, I only need to grab speeds. So long as there is no overflow during the period between grabbing those speeds, I don’t worry. the distances are accumulated in software using other variables which can be as big as you like.

  20. jonatan says:

    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;
    }

  21. Martin says:

    @Green
    A counter overflow can generate an update event which generates a pending interrupt.
    Maybe your MCU runs into the ISR which is not defined and then the HardFaultHandler continues indefinitely.

  22. serkan says:

    can you share encoder application or project? thanks…

  23. Green Ye says:

    @Martin
    That could be the reason. May be I should define an empty handler.
    I got used to 32bit timer on STM32F4 already. I just can’t imaging the day that I was using 16 bit one on STM32F103 :) Thanks ST to make their library a lot friendly than before.

Leave a Reply