Observing Jitters and incorrect rotation angle of the servo with STM32F401 timers

[Lalit26]

Hi,

I am using Tower Pro SG90 servo motor interfaced with STM32F401. Timer 2 is used for generating PWM and timer 5 is used for generating the delay. I tried multiple code changes by myself and also took help from internet, but the servo jitters a lot, gets heated and gives the incorrect angle. Can anybody check my code and help me out?

#include <stdlib.h>
#include <stdint.h>
#include <stm32f401xe.h>

/*************This structure defines the registers for general purpose timers 2 and 5*************/
typedef struct{
	volatile uint32_t REG_CR1;
	uint32_t RESERVED0[2];
	volatile uint32_t REG_DIER;
	volatile uint32_t REG_SR;
	volatile uint32_t REG_EGR;
	volatile uint32_t REG_CCMR1;
	uint32_t RESERVED1[1];
	volatile uint32_t REG_CCER;
	volatile uint32_t REG_CNT;
	volatile uint32_t REG_PSC;
	volatile uint32_t REG_ARR;
	uint32_t RESERVED2[1];
	volatile uint32_t REG_CCR1;
	uint32_t RESERVED3[8]; 
}TIM_REG_DEFINES;

/*************This structure defines the registers for GPIO*************/
typedef struct{
	volatile uint32_t REG_MODER;
	volatile uint32_t REG_OTYPER;
	volatile uint32_t REG_OSPEEDR;
	volatile uint32_t REG_PUPDR;
	volatile uint32_t REG_IDR;
	volatile uint32_t REG_ODR;
	volatile uint32_t REG_BSRR;
	volatile uint32_t REG_LCKR;
	volatile uint32_t REG_AFR[2];
	volatile uint32_t REG_BRR;
}GPIO_REG_DEFINES;

/*************This structure defines registers for RCC*************/
typedef struct{
	uint32_t RESERVED0[12];
	volatile uint32_t REG_AHB1_ENR;
	volatile uint32_t REG_AHB2_ENR;
	uint32_t RESERVED1[2];
	volatile uint32_t REG_APB1_ENR;
	volatile uint32_t REG_APB2_ENR;
	uint32_t RESERVED2[18];
}RCC_REG_DEFINES;

/*************Important MACROS*************/
#define SERVO_PIN		0
#define REG_RCC			((RCC_REG_DEFINES *)0x40023800)
#define GPIO_A			((GPIO_REG_DEFINES *)0x40020000)
#define TIM_2				((TIM_REG_DEFINES *)0x40000000)
#define TIM_5				((TIM_REG_DEFINES *)0x40000C00)

/*************This function enables the clock to GPIOA, TIM2 AND TIM5*************/
void GPIOA_TIM2_TIM5_Enable(){
	REG_RCC->REG_AHB1_ENR |= 1;
	REG_RCC->REG_APB1_ENR |= 1;
	REG_RCC->REG_APB1_ENR |= (1<<3);
}

/*************This function sets up GPIO Port A pin 0 in alternate function 1 mode*************/
void GPIO_Setup(){
	GPIO_A->REG_MODER &= ~(0x3 << (2*SERVO_PIN));
	GPIO_A->REG_MODER |= (0x2 << (2*SERVO_PIN));
	GPIO_A->REG_AFR[0] &= ~(0xF << (4*SERVO_PIN));
	GPIO_A->REG_AFR[0] |= (0x1 << (4*SERVO_PIN));
	GPIO_A->REG_OTYPER &= ~(0x1);
}

void TIM5_Setup(){
	TIM_5->REG_CR1 &= ~0x1;					//Disable the timer
	TIM_5->REG_PSC = 15;
	TIM_5->REG_ARR = 0xFFFFFFFF;
	TIM_5->REG_EGR |= 0x1;					//Load the PSC, ARR value immediately 
	TIM_5->REG_CR1 |= 1;
}

void delay_ms(){
	uint32_t start = TIM_5->REG_CNT;
	uint32_t ticks = 300000;
	while((uint32_t)(TIM_5->REG_CNT - start)<ticks);
}


/*************This function sets up timer 2 for output compare on channel 1 and PWM mode 1*************/
void TIM2_Setup(){
	TIM_2->REG_CR1 &= ~(0x1);						//Disable the timer
	TIM_2->REG_CCER &= ~(0x1);											//Output compare disabled
	
	TIM_2->REG_PSC = 15;														//Use 15 for 1us ticks
	TIM_2->REG_ARR = 19999;													//For 20ms period
	TIM_2->REG_CNT = 0;															//Timer count zero (safe)
	TIM_2->REG_CCR1 = 999;													//For -90 degree (initial)
	
	TIM_2->REG_CR1 |= (0x1<<7);											//ARP preload enable
	TIM_2->REG_CCMR1 &= ~((0x7<<4)|(0x3));					//Configure channel 1 for output compare and clear OC1M[1:0] and OC
	TIM_2->REG_CCMR1 |= (0x6<<4);										//set the pwm mode to 1
	TIM_2->REG_CCMR1 |= (0x1<<3);										//Output compare preload enable
	
	TIM_2->REG_EGR |= 1;														//Load the PSC, ARR and CCR value immediately
	TIM_2->REG_SR &= ~0x1;													//Clear update flag
	
	TIM_2->REG_CR1 |= 1;														//start the timer
	
	while(!(TIM_2->REG_SR & 1));
	
	TIM_2->REG_SR &= ~0x1;
	
	TIM_2->REG_CCER |= 0x1;													//Output compare enabled
}

void set_ccr_ticks(uint32_t ccr_ticks){
	while(!(TIM_2->REG_SR & 1));
	TIM_2->REG_SR &= ~0x1;
	TIM_2->REG_CCR1 = ccr_ticks;
}

int main(){
	GPIOA_TIM2_TIM5_Enable();					//Enable the clock to GPIOA, TIM2 and TIM5
	GPIO_Setup();											//Setup the GPIO port A Pin 0 in AF mode
	TIM2_Setup();											//Timer 2 in OC mode, pwm mode 1 on channel 1
	TIM5_Setup();
	//delay_ms();												//200 ms delay for initial stability at 0 degree
	while(1){
	  set_ccr_ticks(999);					//CCR value corresponding to 90 degree
		delay_ms();											//200 ms delay for proper rotation
		set_ccr_ticks(1499);					//CCR value corresponding to 0 degree
		delay_ms();											//200 ms delay for proper rotation
	}
}

Thanks in advance,

[mƎALLEm]

Hello,

First, try using HAL instead of bare metal at the first step.

Second check, your PWM signal and frequency using an oscilloscope and compare to what is recommended in the servo motor datasheet.