diff options
| -rw-r--r-- | stmhal/servo.c | 67 |
1 files changed, 32 insertions, 35 deletions
diff --git a/stmhal/servo.c b/stmhal/servo.c index 6ea6938ada..2916ca2807 100644 --- a/stmhal/servo.c +++ b/stmhal/servo.c @@ -26,26 +26,28 @@ #include <stdio.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). - -// 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 @@ -65,7 +67,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; @@ -75,6 +76,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) { @@ -100,12 +114,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) { @@ -116,24 +125,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; @@ -178,7 +175,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) |