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,