Skip to content

Commit 626f6b8

Browse files
dpgeorgedpgeorge
committed
stmhal: Add servo driver.
1 parent d311655 commit 626f6b8

5 files changed

Lines changed: 151 additions & 6 deletions

File tree

‎stmhal/Makefile‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -89,8 +89,8 @@ SRC_C = \
8989
diskio.c \
9090
lcd.c \
9191
accel.c \
92+
servo.c \
9293

93-
# servo.c \
9494
# timer.c \
9595
# audio.c \
9696
# i2c.c \

‎stmhal/main.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,8 +36,8 @@
3636
#include "ff.h"
3737
#include "lcd.h"
3838
#include "accel.h"
39-
#if 0
4039
#include "servo.h"
40+
#if 0
4141
#include "timer.h"
4242
#include "pybwlan.h"
4343
#include "pin.h"
@@ -254,12 +254,12 @@ int main(void) {
254254
rng_init();
255255
#endif
256256

257-
#if 0
258257
#if MICROPY_HW_ENABLE_SERVO
259258
// servo
260259
servo_init();
261260
#endif
262261

262+
#if 0
263263
#if MICROPY_HW_ENABLE_TIMER
264264
// timer
265265
timer_init();

‎stmhal/pybmodule.c‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,8 @@
2323
#include "storage.h"
2424
#include "sdcard.h"
2525
#include "accel.h"
26-
#if 0
2726
#include "servo.h"
27+
#if 0
2828
#include "usb.h"
2929
#include "i2c.h"
3030
#include "adc.h"
@@ -231,13 +231,11 @@ STATIC const mp_map_elem_t pyb_module_globals_table[] = {
231231
{ MP_OBJ_NEW_QSTR(MP_QSTR_rtc_info), (mp_obj_t)&pyb_rtc_info_obj },
232232
#endif
233233

234-
#if 0
235234
#if MICROPY_HW_ENABLE_SERVO
236235
{ MP_OBJ_NEW_QSTR(MP_QSTR_pwm), (mp_obj_t)&pyb_pwm_set_obj },
237236
{ MP_OBJ_NEW_QSTR(MP_QSTR_servo), (mp_obj_t)&pyb_servo_set_obj },
238237
{ MP_OBJ_NEW_QSTR(MP_QSTR_Servo), (mp_obj_t)&pyb_Servo_obj },
239238
#endif
240-
#endif
241239

242240
#if MICROPY_HW_HAS_SWITCH
243241
{ MP_OBJ_NEW_QSTR(MP_QSTR_switch), (mp_obj_t)&pyb_switch_obj },

‎stmhal/servo.c‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
#include <stdio.h>
2+
3+
#include "stm32f4xx_hal.h"
4+
5+
#include "misc.h"
6+
#include "mpconfig.h"
7+
#include "qstr.h"
8+
#include "obj.h"
9+
#include "servo.h"
10+
11+
// this servo driver uses hardware PWM to drive servos on PA0, PA1, PA2, PA3 = X1, X2, X3, X4
12+
// TIM2 and TIM5 have CH1, CH2, CH3, CH4 on PA0-PA3 respectively
13+
// they are both 32-bit counters with 16-bit prescaler
14+
// we use TIM2
15+
16+
STATIC TIM_HandleTypeDef servo_TimHandle;
17+
18+
void servo_init(void) {
19+
// TIM2 clock enable
20+
__TIM2_CLK_ENABLE();
21+
22+
// PWM clock configuration
23+
servo_TimHandle.Instance = TIM2;
24+
servo_TimHandle.Init.Period = 2000; // timer cycles at 50Hz
25+
servo_TimHandle.Init.Prescaler = ((SystemCoreClock / 2) / 100000) - 1; // timer runs at 100kHz
26+
servo_TimHandle.Init.ClockDivision = 0;
27+
servo_TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
28+
HAL_TIM_PWM_Init(&servo_TimHandle);
29+
}
30+
31+
STATIC void servo_init_channel(int channel_in) {
32+
uint32_t pin;
33+
uint32_t channel;
34+
switch (channel_in) {
35+
case 1: pin = GPIO_PIN_0; channel = TIM_CHANNEL_1; break;
36+
case 2: pin = GPIO_PIN_1; channel = TIM_CHANNEL_2; break;
37+
case 3: pin = GPIO_PIN_2; channel = TIM_CHANNEL_3; break;
38+
case 4: pin = GPIO_PIN_3; channel = TIM_CHANNEL_4; break;
39+
default: return;
40+
}
41+
42+
// GPIO configuration
43+
GPIO_InitTypeDef GPIO_InitStructure;
44+
GPIO_InitStructure.Pin = pin;
45+
GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
46+
GPIO_InitStructure.Speed = GPIO_SPEED_FAST;
47+
GPIO_InitStructure.Pull = GPIO_NOPULL;
48+
GPIO_InitStructure.Alternate = GPIO_AF1_TIM2;
49+
HAL_GPIO_Init(GPIOA, &GPIO_InitStructure);
50+
51+
// PWM mode configuration
52+
TIM_OC_InitTypeDef oc_init;
53+
oc_init.OCMode = TIM_OCMODE_PWM1;
54+
oc_init.Pulse = 150; // units of 10us
55+
oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
56+
oc_init.OCFastMode = TIM_OCFAST_DISABLE;
57+
HAL_TIM_PWM_ConfigChannel(&servo_TimHandle, &oc_init, channel);
58+
59+
// start PWM
60+
HAL_TIM_PWM_Start(&servo_TimHandle, channel);
61+
}
62+
63+
/******************************************************************************/
64+
// Micro Python bindings
65+
66+
STATIC mp_obj_t pyb_servo_set(mp_obj_t port, mp_obj_t value) {
67+
int p = mp_obj_get_int(port);
68+
int v = mp_obj_get_int(value);
69+
if (v < 50) { v = 50; }
70+
if (v > 250) { v = 250; }
71+
switch (p) {
72+
case 1: TIM2->CCR1 = v; break;
73+
case 2: TIM2->CCR2 = v; break;
74+
case 3: TIM2->CCR3 = v; break;
75+
case 4: TIM2->CCR4 = v; break;
76+
}
77+
return mp_const_none;
78+
}
79+
80+
MP_DEFINE_CONST_FUN_OBJ_2(pyb_servo_set_obj, pyb_servo_set);
81+
82+
STATIC mp_obj_t pyb_pwm_set(mp_obj_t period, mp_obj_t pulse) {
83+
int pe = mp_obj_get_int(period);
84+
int pu = mp_obj_get_int(pulse);
85+
TIM2->ARR = pe;
86+
TIM2->CCR3 = pu;
87+
return mp_const_none;
88+
}
89+
90+
MP_DEFINE_CONST_FUN_OBJ_2(pyb_pwm_set_obj, pyb_pwm_set);
91+
92+
typedef struct _pyb_servo_obj_t {
93+
mp_obj_base_t base;
94+
uint servo_id;
95+
} pyb_servo_obj_t;
96+
97+
STATIC void servo_obj_print(void (*print)(void *env, const char *fmt, ...), void *env, mp_obj_t self_in, mp_print_kind_t kind) {
98+
pyb_servo_obj_t *self = self_in;
99+
print(env, "<Servo %lu>", self->servo_id);
100+
}
101+
102+
STATIC mp_obj_t servo_obj_angle(mp_obj_t self_in, mp_obj_t angle) {
103+
pyb_servo_obj_t *self = self_in;
104+
#if MICROPY_ENABLE_FLOAT
105+
machine_int_t v = 152 + 85.0 * mp_obj_get_float(angle) / 90.0;
106+
#else
107+
machine_int_t v = 152 + 85 * mp_obj_get_int(angle) / 90;
108+
#endif
109+
if (v < 65) { v = 65; }
110+
if (v > 210) { v = 210; }
111+
switch (self->servo_id) {
112+
case 1: TIM2->CCR1 = v; break;
113+
case 2: TIM2->CCR2 = v; break;
114+
case 3: TIM2->CCR3 = v; break;
115+
case 4: TIM2->CCR4 = v; break;
116+
}
117+
return mp_const_none;
118+
}
119+
120+
STATIC MP_DEFINE_CONST_FUN_OBJ_2(servo_obj_angle_obj, servo_obj_angle);
121+
122+
STATIC const mp_method_t servo_methods[] = {
123+
{ "angle", &servo_obj_angle_obj },
124+
{ NULL, NULL },
125+
};
126+
127+
STATIC const mp_obj_type_t servo_obj_type = {
128+
{ &mp_type_type },
129+
.name = MP_QSTR_Servo,
130+
.print = servo_obj_print,
131+
.methods = servo_methods,
132+
};
133+
134+
STATIC mp_obj_t pyb_Servo(mp_obj_t servo_id) {
135+
pyb_servo_obj_t *o = m_new_obj(pyb_servo_obj_t);
136+
o->base.type = &servo_obj_type;
137+
o->servo_id = mp_obj_get_int(servo_id);
138+
servo_init_channel(o->servo_id);
139+
return o;
140+
}
141+
142+
MP_DEFINE_CONST_FUN_OBJ_1(pyb_Servo_obj, pyb_Servo);

‎stmhal/servo.h‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
void servo_init(void);
2+
3+
MP_DECLARE_CONST_FUN_OBJ(pyb_servo_set_obj);
4+
MP_DECLARE_CONST_FUN_OBJ(pyb_pwm_set_obj);
5+
MP_DECLARE_CONST_FUN_OBJ(pyb_Servo_obj);

0 commit comments

Comments
 (0)