github
/
micropython
mirror of https://github.com/micropython/micropython.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
Browse Source

stmhal: Servo driver can move at a given speed.

pull/361/head
Damien George 11 years ago
parent
626f6b8133
commit
0119fc7532
6 changed files with 147 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 144
      stmhal/servo.c
  2. 3
      stmhal/servo.h
  3. 14
      stmhal/stm32f4xx_hal_msp.c
  4. 11
      stmhal/stm32f4xx_it.c
  5. 19
      stmhal/usbd_cdc_interface.c
  6. 7
      stmhal/usbd_cdc_interface.h

144
stmhal/servo.c
View File

@ -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);

3
stmhal/servo.h
View File

@ -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);

14
stmhal/stm32f4xx_hal_msp.c
View File

@ -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();
}
}
/**
* @}
*/

11
stmhal/stm32f4xx_it.c
View File

@ -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****/

19
stmhal/usbd_cdc_interface.c
View File

@ -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 */
}

7
stmhal/usbd_cdc_interface.h
View File

@ -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);

Write Preview
Loading…
Cancel
Save