embox

1
/**
2
 * @file
3
 * @brief Manage servo with STM32F3Discovery
4
 * @author Denis Deryugin <deryugin.denis@gmail.com>
5
 * @version 0.1
6
 * @date 2016-04-04
7
 */
8

9
/* Read "servo.h" for details of how servo actually works */
10

11
#include <string.h>
12
#include <hal/reg.h>
13
#include <stm32f3_discovery.h>
14
#include <stm32f3xx_hal.h>
15
#include <stm32f3xx_hal_tim.h>
16

17
static TIM_HandleTypeDef servo_thandle;
18
static TIM_OC_InitTypeDef servo_output_config;
19

20
/**
21
 * @brief Turn on red led on board and do nothing
22
 */
23
static void handle_error(void) {
24
	BSP_LED_On(LED3);
25
	while(1);
26
}
27

28
/**
29
 * @brief  Timer setup
30
 *
31
 * @return Zero. If fails, handle_error() is called
32
 */
33
static int _timer_init(void) {
34
	__HAL_RCC_TIM2_CLK_ENABLE();
35

36
	servo_thandle.Instance = TIM2;
37

38
	servo_thandle.Init.Prescaler         = 72;
39
	servo_thandle.Init.Period            = 20000;
40
	servo_thandle.Init.ClockDivision     = 0;
41
	servo_thandle.Init.CounterMode       = TIM_COUNTERMODE_UP;
42
	servo_thandle.Init.RepetitionCounter = 0;
43

44
	if (HAL_TIM_PWM_Init(&servo_thandle) != HAL_OK) {
45
		handle_error();
46
	}
47

48
	return 0;
49
}
50

51
/*
52
 * @brief Initialize PWM parameters
53
 *
54
 * @return Zero. Code is too simple for considering any errors
55
 */
56
static int _pwm_init() {
57
	__HAL_RCC_GPIOD_CLK_ENABLE();
58

59
	GPIO_InitTypeDef PORT;
60
	memset(&PORT, 0, sizeof(PORT));
61
	PORT.Pin = GPIO_PIN_4;
62
	PORT.Pull = GPIO_PULLUP;
63
	PORT.Mode = GPIO_MODE_AF_PP;
64
	PORT.Speed = GPIO_SPEED_FREQ_HIGH;
65
	PORT.Alternate = GPIO_AF2_TIM2;
66
	HAL_GPIO_Init(GPIOD, &PORT);
67

68
	servo_output_config.OCMode       = TIM_OCMODE_PWM1;
69
	servo_output_config.OCPolarity   = TIM_OCPOLARITY_HIGH;
70
	servo_output_config.OCFastMode   = TIM_OCFAST_DISABLE;
71
	servo_output_config.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
72
	servo_output_config.OCNIdleState = TIM_OCNIDLESTATE_RESET;
73

74
	servo_output_config.OCIdleState  = TIM_OCIDLESTATE_RESET;
75

76
	return 0;
77
}
78

79
/**
80
 * @brief Initialize timer and PWM output
81
 *
82
 * @return Always zero. In case of actual error handle_error()
83
 * is called
84
 */
85
int servo_init(void) {
86
	/* Required for handle_error() */
87
	BSP_LED_Init(LED3);
88

89
	if (HAL_Init() != HAL_OK)
90
		handle_error();
91

92
	if (_timer_init())
93
		handle_error();
94

95
	if (_pwm_init())
96
		handle_error();
97

98
	return 0;
99
}
100

101
#define SERVO_0    430  /* 180 degrees */
102
#define SERVO_180  2175 /* 0   degrees */
103

104
#define MIN_POS 0
105
#define MAX_POS 100
106

107
/**
108
 * @brief Set servo position
109
 *
110
 * @param pos  Should  be  between MIN_POS and MAX_POS,  but if it's
111
 * out of range, it will be adjusted. Without adjustment servo gears
112
 * can be damaged.
113
 *
114
 * @return Always zero.  In case of error board will turn on red LED
115
 * and stall. Look handle_error() for details.
116
 */
117
int servo_set(int pos) {
118
	if (pos < MIN_POS)
119
		pos = MIN_POS;
120

121
	if (pos > MAX_POS)
122
		pos = MAX_POS;
123

124
	pos = SERVO_0 + pos * (SERVO_180 - SERVO_0) / (MAX_POS - MIN_POS);
125

126
	servo_output_config.Pulse = pos;
127

128
	if (HAL_TIM_PWM_ConfigChannel(&servo_thandle,
129
	                              &servo_output_config,
130
	                              TIM_CHANNEL_2) != HAL_OK) {
131
		handle_error();
132
	}
133

134
	if (HAL_TIM_PWM_Start(&servo_thandle, TIM_CHANNEL_2) != HAL_OK) {
135
		handle_error();
136
	}
137

138
	return 0;
139
}
140