embox

1
/**
2
 * @file
3
 * @brief Manage servo with STM32
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 <drivers/servo/servo.h>
14
#include <drivers/servo/stm32_servo_conf.h>
15
#include <util/log.h>
16
#include <kernel/printk.h>
17

18
#include <config/board_config.h>
19

20
static TIM_HandleTypeDef TimHandle;
21
static TIM_OC_InitTypeDef sConfig;
22

23
/*
24
 * @brief Initialize PWM parameters
25
 *
26
 * @return Zero. Code is too simple for considering any errors
27
 */
28
static int stm32_pwm_init(void) {
29
	CONF_PWM0_CLK_ENABLE_GPIO();
30

31
	GPIO_InitTypeDef PORT;
32
	memset(&PORT, 0, sizeof(PORT));
33
	PORT.Pin = CONF_PWM0_PIN_TIM_NR;
34
	PORT.Pull = GPIO_PULLUP;
35
	PORT.Mode = GPIO_MODE_AF_PP;
36
	PORT.Speed = GPIO_SPEED_FREQ_HIGH;
37
	PORT.Alternate = CONF_PWM0_PIN_TIM_AF;
38
	HAL_GPIO_Init(CONF_PWM0_PIN_TIM_PORT, &PORT);
39

40
	CONF_PWM0_CLK_ENABLE_TIM();
41
        memset(&TimHandle, 0, sizeof(TimHandle));
42

43
        TimHandle.Instance = CONF_PWM0_TIM_INSTANCE();
44
        TimHandle.Init.Prescaler         = 72;
45
        TimHandle.Init.Period            = 20000;
46
        TimHandle.Init.ClockDivision     = 0;
47
        TimHandle.Init.CounterMode       = TIM_COUNTERMODE_UP;
48
        TimHandle.Init.RepetitionCounter = 0;
49
        TimHandle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
50

51
        if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) {
52
          log_error("Failed to init TIM PWM\n");
53
          return -1;
54
        }
55

56
        memset(&sConfig, 0, sizeof(sConfig));
57
        /* Common configuration for all channels */
58
        sConfig.OCMode       = TIM_OCMODE_PWM1;
59
        sConfig.OCPolarity   = TIM_OCPOLARITY_HIGH;
60
        sConfig.OCFastMode   = TIM_OCFAST_DISABLE;
61
        sConfig.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
62
        sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET;
63

64
        sConfig.OCIdleState  = TIM_OCIDLESTATE_RESET;
65

66
        /* Set the pulse value for channel 1 */
67
        sConfig.Pulse = 10000;
68
        if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, CONF_PWM0_CHANNEL_CHANNEL_TIM()) != HAL_OK)
69
        {
70
          log_error("Failed to config TIM PWM channel\n");
71
          return -1;
72
        }
73

74
        if (HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_1) != HAL_OK) {
75
          log_error("Failed to start TIM PWM\n");
76
          return -1;
77
        }
78

79
	return 0;
80
}
81

82
/**
83
 * @brief Initialize timer and PWM output
84
 *
85
 * @return Always zero. In case of actual error handle_error()
86
 * is called
87
 */
88
static int stm32_servo_init(struct servo_dev *dev) {
89
	(void)dev;
90

91
	if (stm32_pwm_init()) {
92
		log_error("Failed to initialize stm32servo\n");
93
		return -1;
94
	}
95

96
	return 0;
97
}
98

99
#define SERVO_0    CONF_PWM0_SERVO_POS_LOW  /*   0 degrees */
100
#define SERVO_180  CONF_PWM0_SERVO_POS_HIGH /* 180 degrees */
101

102
#define MIN_POS 0
103
#define MAX_POS 100
104

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

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
	sConfig.Pulse = pos;
127

128
	if (HAL_TIM_PWM_ConfigChannel(&TimHandle,
129
	                              &sConfig,
130
	                              CONF_PWM0_CHANNEL_CHANNEL_TIM()) != HAL_OK) {
131
		return -1;
132
	}
133

134
	if (HAL_TIM_PWM_Start(&TimHandle, CONF_PWM0_CHANNEL_CHANNEL_TIM()) != HAL_OK) {
135
		return -1;
136
	}
137

138
	return 0;
139
}
140

141
const static struct servo_ops stm32_servo_ops = {
142
	.init = stm32_servo_init,
143
	.set_pos = stm32_servo_set,
144
};
145

146
SERVO_DEV_DEF(&stm32_servo_ops, NULL);
147