From 626f6b813375a2a67d110cf8f240fe829d5887e4 Mon Sep 17 00:00:00 2001 From: Damien George Date: Sat, 22 Mar 2014 15:52:33 +0000 Subject: [PATCH] stmhal: Add servo driver. --- stmhal/Makefile | 2 +- stmhal/main.c | 4 +- stmhal/pybmodule.c | 4 +- stmhal/servo.c | 142 +++++++++++++++++++++++++++++++++++++++++++++ stmhal/servo.h | 5 ++ 5 files changed, 151 insertions(+), 6 deletions(-) create mode 100644 stmhal/servo.c create mode 100644 stmhal/servo.h diff --git a/stmhal/Makefile b/stmhal/Makefile index 60e5fde3d3..938871aefb 100644 --- a/stmhal/Makefile +++ b/stmhal/Makefile @@ -89,8 +89,8 @@ SRC_C = \ diskio.c \ lcd.c \ accel.c \ + servo.c \ -# servo.c \ # timer.c \ # audio.c \ # i2c.c \ diff --git a/stmhal/main.c b/stmhal/main.c index 2b4daa3705..6511891a1b 100644 --- a/stmhal/main.c +++ b/stmhal/main.c @@ -36,8 +36,8 @@ #include "ff.h" #include "lcd.h" #include "accel.h" -#if 0 #include "servo.h" +#if 0 #include "timer.h" #include "pybwlan.h" #include "pin.h" @@ -254,12 +254,12 @@ soft_reset: rng_init(); #endif -#if 0 #if MICROPY_HW_ENABLE_SERVO // servo servo_init(); #endif +#if 0 #if MICROPY_HW_ENABLE_TIMER // timer timer_init(); diff --git a/stmhal/pybmodule.c b/stmhal/pybmodule.c index afb3b2e60e..09cdf83b8a 100644 --- a/stmhal/pybmodule.c +++ b/stmhal/pybmodule.c @@ -23,8 +23,8 @@ #include "storage.h" #include "sdcard.h" #include "accel.h" -#if 0 #include "servo.h" +#if 0 #include "usb.h" #include "i2c.h" #include "adc.h" @@ -231,13 +231,11 @@ STATIC const mp_map_elem_t pyb_module_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR_rtc_info), (mp_obj_t)&pyb_rtc_info_obj }, #endif -#if 0 #if MICROPY_HW_ENABLE_SERVO { MP_OBJ_NEW_QSTR(MP_QSTR_pwm), (mp_obj_t)&pyb_pwm_set_obj }, { MP_OBJ_NEW_QSTR(MP_QSTR_servo), (mp_obj_t)&pyb_servo_set_obj }, { MP_OBJ_NEW_QSTR(MP_QSTR_Servo), (mp_obj_t)&pyb_Servo_obj }, #endif -#endif #if MICROPY_HW_HAS_SWITCH { MP_OBJ_NEW_QSTR(MP_QSTR_switch), (mp_obj_t)&pyb_switch_obj }, diff --git a/stmhal/servo.c b/stmhal/servo.c new file mode 100644 index 0000000000..1be18b8a4e --- /dev/null +++ b/stmhal/servo.c @@ -0,0 +1,142 @@ +#include + +#include "stm32f4xx_hal.h" + +#include "misc.h" +#include "mpconfig.h" +#include "qstr.h" +#include "obj.h" +#include "servo.h" + +// this servo driver uses hardware PWM to drive servos on PA0, PA1, PA2, PA3 = X1, X2, X3, X4 +// TIM2 and TIM5 have CH1, CH2, CH3, CH4 on PA0-PA3 respectively +// they are both 32-bit counters with 16-bit prescaler +// we use TIM2 + +STATIC TIM_HandleTypeDef servo_TimHandle; + +void servo_init(void) { + // TIM2 clock enable + __TIM2_CLK_ENABLE(); + + // PWM clock configuration + servo_TimHandle.Instance = TIM2; + servo_TimHandle.Init.Period = 2000; // timer cycles at 50Hz + servo_TimHandle.Init.Prescaler = ((SystemCoreClock / 2) / 100000) - 1; // timer runs at 100kHz + servo_TimHandle.Init.ClockDivision = 0; + servo_TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; + HAL_TIM_PWM_Init(&servo_TimHandle); +} + +STATIC void servo_init_channel(int channel_in) { + uint32_t pin; + uint32_t channel; + switch (channel_in) { + case 1: pin = GPIO_PIN_0; channel = TIM_CHANNEL_1; break; + case 2: pin = GPIO_PIN_1; channel = TIM_CHANNEL_2; break; + case 3: pin = GPIO_PIN_2; channel = TIM_CHANNEL_3; break; + case 4: pin = GPIO_PIN_3; channel = TIM_CHANNEL_4; break; + default: return; + } + + // GPIO configuration + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.Pin = pin; + GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; + GPIO_InitStructure.Speed = GPIO_SPEED_FAST; + GPIO_InitStructure.Pull = GPIO_NOPULL; + GPIO_InitStructure.Alternate = GPIO_AF1_TIM2; + HAL_GPIO_Init(GPIOA, &GPIO_InitStructure); + + // PWM mode configuration + TIM_OC_InitTypeDef oc_init; + oc_init.OCMode = TIM_OCMODE_PWM1; + oc_init.Pulse = 150; // units of 10us + oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; + oc_init.OCFastMode = TIM_OCFAST_DISABLE; + HAL_TIM_PWM_ConfigChannel(&servo_TimHandle, &oc_init, channel); + + // start PWM + HAL_TIM_PWM_Start(&servo_TimHandle, channel); +} + +/******************************************************************************/ +// Micro Python bindings + +STATIC mp_obj_t pyb_servo_set(mp_obj_t port, mp_obj_t value) { + int p = mp_obj_get_int(port); + int v = mp_obj_get_int(value); + if (v < 50) { v = 50; } + if (v > 250) { v = 250; } + switch (p) { + case 1: TIM2->CCR1 = v; break; + case 2: TIM2->CCR2 = v; break; + case 3: TIM2->CCR3 = v; break; + case 4: TIM2->CCR4 = v; break; + } + return mp_const_none; +} + +MP_DEFINE_CONST_FUN_OBJ_2(pyb_servo_set_obj, pyb_servo_set); + +STATIC mp_obj_t pyb_pwm_set(mp_obj_t period, mp_obj_t pulse) { + int pe = mp_obj_get_int(period); + int pu = mp_obj_get_int(pulse); + TIM2->ARR = pe; + TIM2->CCR3 = pu; + return mp_const_none; +} + +MP_DEFINE_CONST_FUN_OBJ_2(pyb_pwm_set_obj, pyb_pwm_set); + +typedef struct _pyb_servo_obj_t { + mp_obj_base_t base; + uint servo_id; +} pyb_servo_obj_t; + +STATIC void servo_obj_print(void (*print)(void *env, const char *fmt, ...), void *env, mp_obj_t self_in, mp_print_kind_t kind) { + pyb_servo_obj_t *self = self_in; + print(env, "", self->servo_id); +} + +STATIC mp_obj_t servo_obj_angle(mp_obj_t self_in, mp_obj_t angle) { + pyb_servo_obj_t *self = self_in; +#if MICROPY_ENABLE_FLOAT + machine_int_t v = 152 + 85.0 * mp_obj_get_float(angle) / 90.0; +#else + machine_int_t v = 152 + 85 * mp_obj_get_int(angle) / 90; +#endif + if (v < 65) { v = 65; } + if (v > 210) { v = 210; } + switch (self->servo_id) { + case 1: TIM2->CCR1 = v; break; + case 2: TIM2->CCR2 = v; break; + case 3: TIM2->CCR3 = v; break; + case 4: TIM2->CCR4 = v; break; + } + return mp_const_none; +} + +STATIC MP_DEFINE_CONST_FUN_OBJ_2(servo_obj_angle_obj, servo_obj_angle); + +STATIC const mp_method_t servo_methods[] = { + { "angle", &servo_obj_angle_obj }, + { NULL, NULL }, +}; + +STATIC const mp_obj_type_t servo_obj_type = { + { &mp_type_type }, + .name = MP_QSTR_Servo, + .print = servo_obj_print, + .methods = servo_methods, +}; + +STATIC mp_obj_t pyb_Servo(mp_obj_t servo_id) { + pyb_servo_obj_t *o = m_new_obj(pyb_servo_obj_t); + o->base.type = &servo_obj_type; + o->servo_id = mp_obj_get_int(servo_id); + servo_init_channel(o->servo_id); + return o; +} + +MP_DEFINE_CONST_FUN_OBJ_1(pyb_Servo_obj, pyb_Servo); diff --git a/stmhal/servo.h b/stmhal/servo.h new file mode 100644 index 0000000000..bb3edd97a2 --- /dev/null +++ b/stmhal/servo.h @@ -0,0 +1,5 @@ +void servo_init(void); + +MP_DECLARE_CONST_FUN_OBJ(pyb_servo_set_obj); +MP_DECLARE_CONST_FUN_OBJ(pyb_pwm_set_obj); +MP_DECLARE_CONST_FUN_OBJ(pyb_Servo_obj);