#pragma once


static void setPwmValue(uint32_t value){
	htim2.Instance->CCR1 = value;
}

static void pwmStart(){
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
}
static void pwmStop(){
	HAL_TIM_PWM_Stop(&htim2, TIM_CHANNEL_1);
}

#define PERIOD_MS (20)
#define CLOCKS_PER_PERIOD (1000000 / 50)
#define CLOCKS_PER_MS (CLOCKS_PER_PERIOD / PERIOD_MS)
#define CLOCKS_PER_HALF_MS (CLOCKS_PER_MS / 2)

#define SERVO_POS_N_60 (CLOCKS_PER_MS)
#define SERVO_POS_0 (CLOCKS_PER_MS + CLOCKS_PER_HALF_MS)
#define SERVO_POS_P_60 (CLOCKS_PER_MS + CLOCKS_PER_MS)

setPwmValue(SERVO_POS_0);
pwmStart();
while(true){
	/*vTaskDelay(2000);
	HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
	setPwmValue(SERVO_POS_P_60);
*/


	vTaskDelay(3000);
	HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
	setPwmValue(CLOCKS_PER_HALF_MS  );

	vTaskDelay(3000);
			HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
			setPwmValue(SERVO_POS_P_60 + CLOCKS_PER_HALF_MS );

	/*vTaskDelay(2000);
	HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
	setPwmValue(SERVO_POS_N_60);

	vTaskDelay(2000);
	HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
	setPwmValue(SERVO_POS_0);*/
}