diff options
Diffstat (limited to 'stmhal/servo.c')
| -rw-r--r-- | stmhal/servo.c | 50 |
1 files changed, 18 insertions, 32 deletions
diff --git a/stmhal/servo.c b/stmhal/servo.c index 15ba35165c..9c757c5659 100644 --- a/stmhal/servo.c +++ b/stmhal/servo.c @@ -8,12 +8,13 @@ #include "qstr.h" #include "obj.h" #include "runtime.h" +#include "timer.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 +// we use TIM5 #define PYB_SERVO_NUM (4) @@ -30,23 +31,8 @@ STATIC const mp_obj_type_t servo_obj_type; STATIC pyb_servo_obj_t pyb_servo_obj[PYB_SERVO_NUM]; -TIM_HandleTypeDef TIM2_Handle; - void servo_init(void) { - // TIM2 clock enable - __TIM2_CLK_ENABLE(); - - // set up and enable interrupt - HAL_NVIC_SetPriority(TIM2_IRQn, 6, 0); - HAL_NVIC_EnableIRQ(TIM2_IRQn); - - // PWM clock configuration - TIM2_Handle.Instance = TIM2; - TIM2_Handle.Init.Period = 2000; // timer cycles at 50Hz - TIM2_Handle.Init.Prescaler = ((SystemCoreClock / 2) / 100000) - 1; // timer runs at 100kHz - TIM2_Handle.Init.ClockDivision = 0; - TIM2_Handle.Init.CounterMode = TIM_COUNTERMODE_UP; - HAL_TIM_PWM_Init(&TIM2_Handle); + timer_tim5_init(); // reset servo objects for (int i = 0; i < PYB_SERVO_NUM; i++) { @@ -74,17 +60,17 @@ void servo_timer_irq_callback(void) { need_it = true; } switch (s->servo_id) { - case 1: TIM2->CCR1 = s->pulse_cur; break; - case 2: TIM2->CCR2 = s->pulse_cur; break; - case 3: TIM2->CCR3 = s->pulse_cur; break; - case 4: TIM2->CCR4 = s->pulse_cur; break; + 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; } } } if (need_it) { - __HAL_TIM_ENABLE_IT(&TIM2_Handle, TIM_IT_UPDATE); + __HAL_TIM_ENABLE_IT(&TIM5_Handle, TIM_IT_UPDATE); } else { - __HAL_TIM_DISABLE_IT(&TIM2_Handle, TIM_IT_UPDATE); + __HAL_TIM_DISABLE_IT(&TIM5_Handle, TIM_IT_UPDATE); } } @@ -105,7 +91,7 @@ STATIC void servo_init_channel(pyb_servo_obj_t *s) { GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; GPIO_InitStructure.Speed = GPIO_SPEED_FAST; GPIO_InitStructure.Pull = GPIO_NOPULL; - GPIO_InitStructure.Alternate = GPIO_AF1_TIM2; + GPIO_InitStructure.Alternate = GPIO_AF2_TIM5; HAL_GPIO_Init(GPIOA, &GPIO_InitStructure); // PWM mode configuration @@ -114,10 +100,10 @@ STATIC void servo_init_channel(pyb_servo_obj_t *s) { oc_init.Pulse = s->pulse_cur; // units of 10us oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; oc_init.OCFastMode = TIM_OCFAST_DISABLE; - HAL_TIM_PWM_ConfigChannel(&TIM2_Handle, &oc_init, channel); + HAL_TIM_PWM_ConfigChannel(&TIM5_Handle, &oc_init, channel); // start PWM - HAL_TIM_PWM_Start(&TIM2_Handle, channel); + HAL_TIM_PWM_Start(&TIM5_Handle, channel); } /******************************************************************************/ @@ -129,10 +115,10 @@ STATIC mp_obj_t pyb_servo_set(mp_obj_t port, mp_obj_t 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; + case 1: TIM5->CCR1 = v; break; + case 2: TIM5->CCR2 = v; break; + case 3: TIM5->CCR3 = v; break; + case 4: TIM5->CCR4 = v; break; } return mp_const_none; } @@ -142,8 +128,8 @@ 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; + TIM5->ARR = pe; + TIM5->CCR3 = pu; return mp_const_none; } |