diff options
| -rw-r--r-- | stmhal/servo.c | 144 | ||||
| -rw-r--r-- | stmhal/servo.h | 3 | ||||
| -rw-r--r-- | stmhal/stm32f4xx_hal_msp.c | 14 | ||||
| -rw-r--r-- | stmhal/stm32f4xx_it.c | 11 | ||||
| -rw-r--r-- | stmhal/usbd_cdc_interface.c | 19 | ||||
| -rw-r--r-- | stmhal/usbd_cdc_interface.h | 7 |
6 files changed, 147 insertions, 51 deletions
diff --git a/stmhal/servo.c b/stmhal/servo.c index 1be18b8a4e..57f1eb5d18 100644 --- a/stmhal/servo.c +++ b/stmhal/servo.c @@ -2,6 +2,7 @@ #include "stm32f4xx_hal.h" +#include "nlr.h" #include "misc.h" #include "mpconfig.h" #include "qstr.h" @@ -13,25 +14,85 @@ // they are both 32-bit counters with 16-bit prescaler // we use TIM2 -STATIC TIM_HandleTypeDef servo_TimHandle; +#define PYB_SERVO_NUM (4) + +typedef struct _pyb_servo_obj_t { + mp_obj_base_t base; + uint16_t servo_id; + uint16_t time_left; + int16_t pulse_accum; + uint16_t pulse_cur; + uint16_t pulse_dest; +} pyb_servo_obj_t; + +STATIC const mp_obj_type_t servo_obj_type; + +STATIC pyb_servo_obj_t pyb_servo_obj[PYB_SERVO_NUM]; + +TIM_HandleTypeDef servo_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 - 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); + servo_TIM2_Handle.Instance = TIM2; + servo_TIM2_Handle.Init.Period = 2000; // timer cycles at 50Hz + servo_TIM2_Handle.Init.Prescaler = ((SystemCoreClock / 2) / 100000) - 1; // timer runs at 100kHz + servo_TIM2_Handle.Init.ClockDivision = 0; + servo_TIM2_Handle.Init.CounterMode = TIM_COUNTERMODE_UP; + HAL_TIM_PWM_Init(&servo_TIM2_Handle); + + // reset servo objects + for (int i = 0; i < PYB_SERVO_NUM; i++) { + pyb_servo_obj[i].base.type = &servo_obj_type; + pyb_servo_obj[i].servo_id = i + 1; + pyb_servo_obj[i].time_left = 0; + pyb_servo_obj[i].pulse_cur = 150; // units of 10us + pyb_servo_obj[i].pulse_dest = 0; + } +} + +#include "led.h" +void servo_timer_irq_callback(void) { + led_toggle(1); + bool need_it = false; + for (int i = 0; i < PYB_SERVO_NUM; i++) { + pyb_servo_obj_t *s = &pyb_servo_obj[i]; + if (s->pulse_cur != s->pulse_dest) { + if (s->time_left <= 1) { + s->pulse_cur = s->pulse_dest; + s->time_left = 0; + } else { + s->pulse_accum += s->pulse_dest - s->pulse_cur; + s->pulse_cur += s->pulse_accum / s->time_left; + s->pulse_accum %= s->time_left; + s->time_left--; + 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; + } + } + } + if (need_it) { + __HAL_TIM_ENABLE_IT(&servo_TIM2_Handle, TIM_IT_UPDATE); + } else { + __HAL_TIM_DISABLE_IT(&servo_TIM2_Handle, TIM_IT_UPDATE); + } } -STATIC void servo_init_channel(int channel_in) { +STATIC void servo_init_channel(pyb_servo_obj_t *s) { uint32_t pin; uint32_t channel; - switch (channel_in) { + 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; @@ -51,13 +112,13 @@ STATIC void servo_init_channel(int channel_in) { // PWM mode configuration TIM_OC_InitTypeDef oc_init; oc_init.OCMode = TIM_OCMODE_PWM1; - oc_init.Pulse = 150; // units of 10us + 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(&servo_TimHandle, &oc_init, channel); + HAL_TIM_PWM_ConfigChannel(&servo_TIM2_Handle, &oc_init, channel); // start PWM - HAL_TIM_PWM_Start(&servo_TimHandle, channel); + HAL_TIM_PWM_Start(&servo_TIM2_Handle, channel); } /******************************************************************************/ @@ -89,35 +150,39 @@ STATIC mp_obj_t pyb_pwm_set(mp_obj_t period, mp_obj_t pulse) { 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, "<Servo %lu>", self->servo_id); + print(env, "<Servo %lu at %lu>", self->servo_id, self->pulse_cur); } -STATIC mp_obj_t servo_obj_angle(mp_obj_t self_in, mp_obj_t angle) { - pyb_servo_obj_t *self = self_in; +STATIC mp_obj_t servo_obj_angle(uint n_args, const mp_obj_t *args) { + pyb_servo_obj_t *self = args[0]; + if (n_args == 1) { + // get angle + return mp_obj_new_int((self->pulse_cur - 152) * 90 / 85); + } else { #if MICROPY_ENABLE_FLOAT - machine_int_t v = 152 + 85.0 * mp_obj_get_float(angle) / 90.0; + machine_int_t v = 152 + 85.0 * mp_obj_get_float(args[1]) / 90.0; #else - machine_int_t v = 152 + 85 * mp_obj_get_int(angle) / 90; + machine_int_t v = 152 + 85 * mp_obj_get_int(args[1]) / 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; + if (v < 65) { v = 65; } + if (v > 210) { v = 210; } + self->pulse_dest = v; + if (n_args == 2) { + // set angle immediately + self->time_left = 0; + } else { + // set angle over a given time (given in milli seconds) + self->time_left = mp_obj_get_int(args[2]) / 20; + self->pulse_accum = 0; + } + servo_timer_irq_callback(); + return mp_const_none; } - return mp_const_none; } -STATIC MP_DEFINE_CONST_FUN_OBJ_2(servo_obj_angle_obj, servo_obj_angle); +STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_angle_obj, 1, 3, servo_obj_angle); STATIC const mp_method_t servo_methods[] = { { "angle", &servo_obj_angle_obj }, @@ -131,12 +196,17 @@ STATIC const mp_obj_type_t servo_obj_type = { .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; +STATIC mp_obj_t pyb_Servo(mp_obj_t servo_id_o) { + machine_int_t servo_id = mp_obj_get_int(servo_id_o) - 1; + if (0 <= servo_id && servo_id < PYB_SERVO_NUM) { + pyb_servo_obj_t *s = &pyb_servo_obj[servo_id]; + s->pulse_dest = s->pulse_cur; + s->time_left = 0; + servo_init_channel(s); + return s; + } else { + nlr_jump(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "Servo %d does not exist", servo_id)); + } } MP_DEFINE_CONST_FUN_OBJ_1(pyb_Servo_obj, pyb_Servo); diff --git a/stmhal/servo.h b/stmhal/servo.h index bb3edd97a2..277ccdd1c3 100644 --- a/stmhal/servo.h +++ b/stmhal/servo.h @@ -1,4 +1,7 @@ +extern TIM_HandleTypeDef servo_TIM2_Handle; + void servo_init(void); +void servo_timer_irq_callback(void); MP_DECLARE_CONST_FUN_OBJ(pyb_servo_set_obj); MP_DECLARE_CONST_FUN_OBJ(pyb_pwm_set_obj); diff --git a/stmhal/stm32f4xx_hal_msp.c b/stmhal/stm32f4xx_hal_msp.c index 816b1427e9..c64f377959 100644 --- a/stmhal/stm32f4xx_hal_msp.c +++ b/stmhal/stm32f4xx_hal_msp.c @@ -50,6 +50,12 @@ #include "usbd_cdc_msc.h"
#include "usbd_cdc_interface.h"
+#include "misc.h"
+#include "mpconfig.h"
+#include "qstr.h"
+#include "obj.h"
+#include "servo.h"
+
/** @addtogroup STM32F4xx_HAL_Driver
* @{
*/
@@ -155,6 +161,14 @@ void HAL_RTC_MspDeInit(RTC_HandleTypeDef *hrtc) __HAL_RCC_RTC_DISABLE();
}
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
+ if (htim == &USBD_CDC_TIM3_Handle) {
+ USBD_CDC_HAL_TIM_PeriodElapsedCallback();
+ } else if (htim == &servo_TIM2_Handle) {
+ servo_timer_irq_callback();
+ }
+}
+
/**
* @}
*/
diff --git a/stmhal/stm32f4xx_it.c b/stmhal/stm32f4xx_it.c index 9100d63a2a..30ded5f7b5 100644 --- a/stmhal/stm32f4xx_it.c +++ b/stmhal/stm32f4xx_it.c @@ -42,12 +42,15 @@ #include "stm32f4xx_it.h"
#include "stm32f4xx_hal.h"
+#include "usbd_cdc_msc.h"
+#include "usbd_cdc_interface.h"
#include "misc.h"
#include "mpconfig.h"
#include "qstr.h"
#include "obj.h"
#include "exti.h"
+#include "servo.h"
/** @addtogroup STM32F4xx_HAL_Examples
* @{
@@ -64,7 +67,6 @@ extern void fatality();
extern PCD_HandleTypeDef hpcd;
-extern TIM_HandleTypeDef USBD_CDC_TimHandle;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
@@ -349,9 +351,14 @@ void RTC_WKUP_IRQHandler(void) { Handle_EXTI_Irq(EXTI_RTC_WAKEUP);
}
+void TIM2_IRQHandler(void) {
+ // servo timer is TIM2
+ HAL_TIM_IRQHandler(&servo_TIM2_Handle);
+}
+
void TIM3_IRQHandler(void) {
// USBD CDC timer is TIM3
- HAL_TIM_IRQHandler(&USBD_CDC_TimHandle);
+ HAL_TIM_IRQHandler(&USBD_CDC_TIM3_Handle);
}
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/stmhal/usbd_cdc_interface.c b/stmhal/usbd_cdc_interface.c index 5116ce860e..e190e34793 100644 --- a/stmhal/usbd_cdc_interface.c +++ b/stmhal/usbd_cdc_interface.c @@ -66,7 +66,7 @@ static int user_interrupt_char = VCP_CHAR_NONE; static void *user_interrupt_data = NULL; /* TIM handler declaration */ -TIM_HandleTypeDef USBD_CDC_TimHandle; +TIM_HandleTypeDef USBD_CDC_TIM3_Handle; /* USB handler declaration */ extern USBD_HandleTypeDef hUSBDDevice; @@ -132,7 +132,7 @@ static int8_t CDC_Itf_Init(void) /*##-4- Start the TIM Base generation in interrupt mode ####################*/ /* Start Channel1 */ - if(HAL_TIM_Base_Start_IT(&USBD_CDC_TimHandle) != HAL_OK) + if(HAL_TIM_Base_Start_IT(&USBD_CDC_TIM3_Handle) != HAL_OK) { /* Starting Error */ } @@ -250,8 +250,7 @@ static int8_t CDC_Itf_Control(uint8_t cmd, uint8_t* pbuf, uint16_t length) { * @param htim: TIM handle * @retval None */ -void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) -{ +void USBD_CDC_HAL_TIM_PeriodElapsedCallback(void) { uint32_t buffptr; uint32_t buffsize; @@ -388,7 +387,7 @@ int USBD_CDC_RxGet(void) { static void TIM_Config(void) { /* Set TIMx instance */ - USBD_CDC_TimHandle.Instance = USBD_CDC_TIMx; + USBD_CDC_TIM3_Handle.Instance = USBD_CDC_TIMx; /* Initialize TIM3 peripheral as follow: + Period = 10000 - 1 @@ -396,11 +395,11 @@ static void TIM_Config(void) + ClockDivision = 0 + Counter direction = Up */ - USBD_CDC_TimHandle.Init.Period = (USBD_CDC_POLLING_INTERVAL*1000) - 1; - USBD_CDC_TimHandle.Init.Prescaler = 84-1; - USBD_CDC_TimHandle.Init.ClockDivision = 0; - USBD_CDC_TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - if(HAL_TIM_Base_Init(&USBD_CDC_TimHandle) != HAL_OK) + USBD_CDC_TIM3_Handle.Init.Period = (USBD_CDC_POLLING_INTERVAL*1000) - 1; + USBD_CDC_TIM3_Handle.Init.Prescaler = 84-1; + USBD_CDC_TIM3_Handle.Init.ClockDivision = 0; + USBD_CDC_TIM3_Handle.Init.CounterMode = TIM_COUNTERMODE_UP; + if(HAL_TIM_Base_Init(&USBD_CDC_TIM3_Handle) != HAL_OK) { /* Initialization Error */ } diff --git a/stmhal/usbd_cdc_interface.h b/stmhal/usbd_cdc_interface.h index bcb6333a6a..f0c8404ff5 100644 --- a/stmhal/usbd_cdc_interface.h +++ b/stmhal/usbd_cdc_interface.h @@ -36,8 +36,8 @@ /* Definition for TIMx clock resources */
#define USBD_CDC_TIMx TIM3
#define USBD_CDC_TIMx_CLK_ENABLE __TIM3_CLK_ENABLE
-#define USBD_CDC_TIMx_FORCE_RESET() __USART3_FORCE_RESET()
-#define USBD_CDC_TIMx_RELEASE_RESET() __USART3_RELEASE_RESET()
+#define USBD_CDC_TIMx_FORCE_RESET() __TIM3_FORCE_RESET()
+#define USBD_CDC_TIMx_RELEASE_RESET() __TIM3_RELEASE_RESET()
/* Definition for TIMx's NVIC */
#define USBD_CDC_TIMx_IRQn TIM3_IRQn
@@ -47,8 +47,11 @@ The period depends on USBD_CDC_POLLING_INTERVAL */
#define USBD_CDC_POLLING_INTERVAL 10 /* in ms. The max is 65 and the min is 1 */
+extern TIM_HandleTypeDef USBD_CDC_TIM3_Handle;
extern const USBD_CDC_ItfTypeDef USBD_CDC_fops;
+void USBD_CDC_HAL_TIM_PeriodElapsedCallback(void);
+
int USBD_CDC_IsConnected(void);
void USBD_CDC_SetInterrupt(int chr, void *data);
void USBD_CDC_Tx(const char *str, uint32_t len);
|