#include "servo.h"
#include "timer.h"

void MyServo_Init()
{	
	MyTimer_Struct_Typedef Timer;
	
	//Période de 20ms -> F de 50Hz
	Timer.Timer = TIM2;
	Timer.ARR = 3599;		//20*180 (angle) = 3600
	Timer.PSC = 399;		//72Mhz / 50Hz = 1.44Mhz et 3600*400 = 1.44M
	
	MyTimer_Base_Init(&Timer);
	
	//Pin Timer 2 Channel 1 PA0
	MyTimer_PWM(TIM2, 1);
	MyTimer_DutyCycle(TIM2, 1, 750);

	MyTimer_Base_Start(Timer);
}

void MyServo_ChangeAngle(uint8_t Angle)
{
	if (Angle > 180)
		Angle = 180;
	
	int DC = 500 + (Angle * 500 / 180);
	
	MyTimer_DutyCycle(TIM2, 1, DC);
}