STM32F0 Servo Tester

Sunday, everybody’s least favourite weekend day at the thought of having another 5 days until time is no longer scarce!

I’ve been getting down to a fair bit of VHDL programming, one program I wrote recently was an attempt at a brushless DC motor. I managed to get the outputs 120deg out of phase but gave up after realising the amount of effort required to actually monitor the motor! Until I get a brushless motor with a tach to actually try my VHDL program on, that can stay in the works!

In the meanwhile, I’ve been messing around with my STM32F0 discovery board, as per usual. I’ve finally decided to have a proper look into cool timer functionality. While some Arduino users may not consider PWM particularly interesting, due to the fact you merely have to write “analogWrite(X)”, its a little bit harder using an STM32F0 as you literally have to configure outputs, along with the timer and potential interrupts yourself.

For my servo controller, I wanted a minimum of 10bit control, a massive overkill but why not set a challenge! Looking through the manual, it seems that TIM2 is a 32bit timer allowing for periods of up to 2^32-1. The standard servo protocol states that a PWM period of 50Hz should be used and from 0 to 360deg, the pulse width should vary from 1ms to 2ms with 1.5ms being dead center. This is relatively easy to do with the STM32, merely set the timer to overflow every 50Hz, and set a capture compare to your required pulse width, voila!

I could have easily used one of the lesser capable timers and used a prescalar and set period but what is the fun of that when I can run TIM2 as maximum frequency and power, producing more resolution than I’ll ever need. For those wondering, I get 48000 steps of control. If your servo can respond to every one of them steps, I’ll be beyond pleasantly surprised! In my code, a PulseWidth value of 0 will give a pulse output length of 1ms where as a PulseWidth value of 48000 will give an pulse output length of 2ms. Every value in between will produce pulse lengths between 1ms and 2ms, with a PulseWidth value of 24000 being a 1.5ms output pulse.

The code is actually really simple and merely depends on setting up the timer. I’ve made it so the capture compare register is set when the timer updates to avoid the user potentially messing up the pulse width in the middle of a timer count. This adds a little bit of complexity by requiring an interrupt that is fired every 50Hz. The PulseWidth value is added to a constant 48000. By adding this 48000, it means that the minimum pulse width will always be 1ms. As a user precaution, I’ve also limited PulseWidth. The PulseWidth variable is a 16bit unsigned integer meaning if one tries to set it to a negative value, it will overflow and reflect back from the top by an amount defined by what negative value you put in. This has two good things, it means the capture compare register will never be <48000, directly translating to the pulse length always being atleast 1ms, it also means that I can easily limit the value of PulseWidth with one if statement. The PulseWidth value is limited to 48000 in the timer update interrupt meaning if the user did set it to an out of range value, say 60000, PulseWidth would keep that value until the next timer update interrupt. Once the timer update interrupt is triggered, PulseWidth will be limited to 48000.

I’ve also added “Triangle Mode”. I bought a really cheap servo tester off ebay a couple of years ago and one of the test modes was having the servo tester output a triangle wave, linearly moving the servo between minimum and maximum. I’ve included this in my version too!

LogicA1
As you can see, the output frequency is 50Hz, along with the pulse width being 2ms.

LogicA2
The pulse width is smaller here at 1.166ms. The code is running in triangle mode in the above two pictures.

LogicA3
Using the program in static mode, as you can see, the pulse width is constant!

The code is easily modify-able to use the other three capture compare channels on timer 2 to control up to 4 servos at once! If anybody likes the sound of this, I could easily do it and upload a revised copy.

The final code is:

#include <stm32f0xx_gpio.h>
#include <stm32f0xx_rcc.h>
#include <stm32f0xx_tim.h>
#include <stm32f0xx_misc.h>

//Create new structs
TIM_TimeBaseInitTypeDef T;
TIM_OCInitTypeDef TO;
GPIO_InitTypeDef G;
NVIC_InitTypeDef N;

#define ServoPWMOut GPIO_Pin_0
#define ServoPWMPS GPIO_PinSource0

//If triangle mode is defined, output will vary between
//1ms and 2ms over a ~2s period. To change to static
//mode, merely comment this out.
//#define TRIANGLE_MODE

//Variable to hold PulseWidth. In triangle mode,
//PulseWidth will temporarily be <0, to ensure this
//doesnt overflow and destroy the triangle wave,
//it will need to be a 32bit integer.
#ifdef TRIANGLE_MODE
 volatile int32_t PulseWidth = 0;
#else
 volatile uint16_t PulseWidth = 10000;
#endif


void TIM2_IRQHandler(void){
 //Rate of output variation defined by PulseAdd
 //If PulseAdd is larger, output will vary faster.

 static int16_t PulseAdd = 480;

 //WARNING: This value was only used to produce the waveform
 //in my logic analyzer image.
 //static int16_t PulseAdd = 10000;

 if(TIM_GetITStatus(TIM2, TIM_IT_Update) == SET){
 TIM_ClearITPendingBit(TIM2, TIM_IT_Update);

#ifdef TRIANGLE_MODE

 PulseWidth+=PulseAdd;

 //Made PulseWidth bounce between 0 and 48k
 //at a rate defined by the initial value of
 //PulseAdd.
 if(PulseWidth > 48000){
 PulseWidth = 48000;
 PulseAdd = -PulseAdd;
 }
 else if(PulseWidth < 0){
 PulseWidth = 0;
 PulseAdd = -PulseAdd;
 }

#else
 //Limit PulseWidth to 48k
 if(PulseWidth>48000){
 PulseWidth = 48000;
 }
#endif

 //Set capture compare register to 1ms+PulseWidth time
 //Each PulseWidth time is equal to 20.833ns
 //meaning 20.8333ns*48000 = 1ms
 TIM_SetCompare1(TIM2, 48000+PulseWidth);
 }
}

int main(void)
{
 //Enable the clock to GPIOA and TIM2. The updated
 //standard peripheral library doesn't require
 //SystemInit() anymore.
 RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

 //Setup timer 2 to overflow at 959999
 //with no clock division (running at 48MHz)
 //Period is calculated by:
 //(48e6/((Prescalar+1)*Freq) - 1
 //Where Freq = 50
 T.TIM_ClockDivision = TIM_CKD_DIV1;
 T.TIM_Prescaler = 0;
 T.TIM_Period = 960000 - 1;
 T.TIM_CounterMode = TIM_CounterMode_Up;
 TIM_TimeBaseInit(TIM2, &T);

 //Setup timer 2 output compare with the output being
 //reset in idle state and the output polarity being
 //high. Output is high if Cnt < CCR1.
 //Set initial pulse to 1ms (0% rotation)
 TO.TIM_OCIdleState = TIM_OCIdleState_Reset;
 TO.TIM_OCMode = TIM_OCMode_PWM1;
 TO.TIM_OCPolarity = TIM_OCPolarity_High;
 TO.TIM_OutputState = TIM_OutputState_Enable;
 TO.TIM_Pulse = 48000; //Initial pulse at 1ms

 //Initialize output compare for timer 2
 TIM_OC1Init(TIM2, &TO);

 //Disable OC preload
 TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Disable);

 //Set update interrupt
 TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
 TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);

 //Configure update interrupt IRQ
 N.NVIC_IRQChannel = TIM2_IRQn;
 N.NVIC_IRQChannelCmd = ENABLE;
 N.NVIC_IRQChannelPriority = 0;
 NVIC_Init(&N);

 //Configure Servo PWM output
 G.GPIO_Pin = ServoPWMOut;
 G.GPIO_Mode = GPIO_Mode_AF;
 G.GPIO_OType = GPIO_OType_PP;
 G.GPIO_Speed = GPIO_Speed_Level_1;
 GPIO_Init(GPIOA, &G);

 //Connect Servo PWM Output to timer 2 (GPIO_AF_2)
 GPIO_PinAFConfig(GPIOA, ServoPWMPS, GPIO_AF_2);

 //Start the timer!
 TIM_Cmd(TIM2, ENABLE);

 while(1)
 {
 }
}

As per, the code can be found on my github:

https://github.com/pyrohaz/STM32F0-ServoTester

Enjoy!

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s