Working with STM32 and Timers part 4: Servo Motor Control

#include "stm32f4xx.h"                  // Device header
void GPIO_Init(void);
void Timer2_init(void);
void delay(int ms );
void servo_write(uint8_t angle);

int pos;
int main(void)
{

GPIO_Init();
Timer2_init();
while(1)
{
for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servo_write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    servo_write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }

}
}

void GPIO_Init(void)
{
RCC->AHB1ENR|=RCC_AHB1ENR_GPIOAEN;
GPIOA->AFR[0]|=(1<<0);
GPIOA->MODER|=(1<<1);

}
void  Timer2_init(void){
RCC->APB1ENR|=RCC_APB1ENR_TIM2EN; //enable clock access tto tim2
TIM2->PSC=4; //set prescaller to 4 (16000000Hz/4=4000000Hz)
TIM2->ARR=63999; //set the maximum count value 
TIM2->CNT=0; //seset the current count
TIM2->CCMR1=(1<<5)|(1<<6); //configure the pins as PWM
TIM2->CCER|=0x1; //enbale channel1 and channel2
TIM2->CR1=1; //enable timer
}


void delay(int ms)

{

int i;

for(; ms>0 ;ms--)

{

for(i =0; i<3195;i++);

}

}

long map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


void servo_write(uint8_t angle)
	{
	
	if(angle<0){angle=0;}
	if(angle>180){angle=180;}
	
	
TIM2->CCR1=map (angle,0,180,2500,7000);
	
	
	
	}