diff options
Diffstat (limited to 'stmhal/servo.c')
| -rw-r--r-- | stmhal/servo.c | 77 |
1 files changed, 38 insertions, 39 deletions
diff --git a/stmhal/servo.c b/stmhal/servo.c index 4c62044dd9..fa39587a8e 100644 --- a/stmhal/servo.c +++ b/stmhal/servo.c @@ -1,5 +1,5 @@ /* - * This file is part of the Micro Python project, http://micropython.org/ + * This file is part of the MicroPython project, http://micropython.org/ * * The MIT License (MIT) * @@ -26,28 +26,30 @@ #include <stdio.h> -#include STM32_HAL_H - -#include "py/nlr.h" #include "py/runtime.h" +#include "py/mphal.h" +#include "pin.h" +#include "genhdr/pins.h" #include "timer.h" #include "servo.h" -/// \moduleref pyb -/// \class Servo - 3-wire hobby servo driver -/// -/// Servo controls standard hobby servos with 3-wires (ground, power, signal). +#if MICROPY_HW_ENABLE_SERVO -// 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 TIM5 +// This file implements the pyb.Servo class which controls standard hobby servo +// motors that have 3-wires (ground, power, signal). +// +// The driver uses hardware PWM to drive servos on pins X1, X2, X3, X4 which are +// assumed to be on PA0, PA1, PA2, PA3 but not necessarily in that order (the +// pins PA0-PA3 are used directly if the X pins are not defined). +// +// TIM2 and TIM5 have CH1-CH4 on PA0-PA3 respectively. They are both 32-bit +// counters with 16-bit prescaler. TIM5 is used by this driver. #define PYB_SERVO_NUM (4) typedef struct _pyb_servo_obj_t { mp_obj_base_t base; - uint8_t servo_id; + const pin_obj_t *pin; uint8_t pulse_min; // units of 10us uint8_t pulse_max; // units of 10us uint8_t pulse_centre; // units of 10us @@ -67,7 +69,6 @@ void servo_init(void) { // reset servo objects for (int i = 0; i < PYB_SERVO_NUM; i++) { pyb_servo_obj[i].base.type = &pyb_servo_type; - pyb_servo_obj[i].servo_id = i + 1; pyb_servo_obj[i].pulse_min = 64; pyb_servo_obj[i].pulse_max = 242; pyb_servo_obj[i].pulse_centre = 150; @@ -77,6 +78,19 @@ void servo_init(void) { pyb_servo_obj[i].pulse_dest = 0; pyb_servo_obj[i].time_left = 0; } + + // assign servo objects to specific pins (must be some permutation of PA0-PA3) + #ifdef pyb_pin_X1 + pyb_servo_obj[0].pin = &pyb_pin_X1; + pyb_servo_obj[1].pin = &pyb_pin_X2; + pyb_servo_obj[2].pin = &pyb_pin_X3; + pyb_servo_obj[3].pin = &pyb_pin_X4; + #else + pyb_servo_obj[0].pin = &pin_A0; + pyb_servo_obj[1].pin = &pin_A1; + pyb_servo_obj[2].pin = &pin_A2; + pyb_servo_obj[3].pin = &pin_A3; + #endif } void servo_timer_irq_callback(void) { @@ -102,12 +116,7 @@ void servo_timer_irq_callback(void) { need_it = true; } // set the pulse width - switch (s->servo_id) { - case 1: TIM5->CCR1 = s->pulse_cur; break; - case 2: TIM5->CCR2 = s->pulse_cur; break; - case 3: TIM5->CCR3 = s->pulse_cur; break; - case 4: TIM5->CCR4 = s->pulse_cur; break; - } + *(&TIM5->CCR1 + s->pin->pin) = s->pulse_cur; } } if (need_it) { @@ -118,24 +127,12 @@ void servo_timer_irq_callback(void) { } STATIC void servo_init_channel(pyb_servo_obj_t *s) { - uint32_t pin; - uint32_t channel; - switch (s->servo_id) { - 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; - } + static const uint8_t channel_table[4] = + {TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4}; + uint32_t channel = channel_table[s->pin->pin]; // 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_AF2_TIM5; - HAL_GPIO_Init(GPIOA, &GPIO_InitStructure); + mp_hal_pin_config(s->pin, MP_HAL_PIN_MODE_ALT, MP_HAL_PIN_PULL_NONE, GPIO_AF2_TIM5); // PWM mode configuration TIM_OC_InitTypeDef oc_init; @@ -150,7 +147,7 @@ STATIC void servo_init_channel(pyb_servo_obj_t *s) { } /******************************************************************************/ -// Micro Python bindings +// MicroPython bindings STATIC mp_obj_t pyb_servo_set(mp_obj_t port, mp_obj_t value) { int p = mp_obj_get_int(port); @@ -180,7 +177,7 @@ MP_DEFINE_CONST_FUN_OBJ_2(pyb_pwm_set_obj, pyb_pwm_set); STATIC void pyb_servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { pyb_servo_obj_t *self = self_in; - mp_printf(print, "<Servo %lu at %luus>", self->servo_id, 10 * self->pulse_cur); + mp_printf(print, "<Servo %lu at %luus>", self - &pyb_servo_obj[0] + 1, 10 * self->pulse_cur); } /// \classmethod \constructor(id) @@ -194,7 +191,7 @@ STATIC mp_obj_t pyb_servo_make_new(const mp_obj_type_t *type, size_t n_args, siz // check servo number if (!(0 <= servo_id && servo_id < PYB_SERVO_NUM)) { - nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "Servo %d does not exist", servo_id + 1)); + nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "Servo(%d) doesn't exist", servo_id + 1)); } // get and init servo object @@ -333,3 +330,5 @@ const mp_obj_type_t pyb_servo_type = { .make_new = pyb_servo_make_new, .locals_dict = (mp_obj_dict_t*)&pyb_servo_locals_dict, }; + +#endif // MICROPY_HW_ENABLE_SERVO |