Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
TASTE
uPython-mirror
Commits
0119fc75
Commit
0119fc75
authored
Mar 22, 2014
by
Damien George
Browse files
stmhal: Servo driver can move at a given speed.
parent
626f6b81
Changes
6
Hide whitespace changes
Inline
Side-by-side
stmhal/servo.c
View file @
0119fc75
...
...
@@ -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
_i
n
)
{
switch
(
s
->
servo
_i
d
)
{
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_T
im
Handle
,
&
oc_init
,
channel
);
HAL_TIM_PWM_ConfigChannel
(
&
servo_T
IM2_
Handle
,
&
oc_init
,
channel
);
// start PWM
HAL_TIM_PWM_Start
(
&
servo_T
im
Handle
,
channel
);
HAL_TIM_PWM_Start
(
&
servo_T
IM2_
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
(
a
ngle
)
/
90
.
0
;
machine_int_t
v
=
152
+
85
.
0
*
mp_obj_get_float
(
a
rgs
[
1
]
)
/
90
.
0
;
#else
machine_int_t
v
=
152
+
85
*
mp_obj_get_int
(
a
ngle
)
/
90
;
machine_int_t
v
=
152
+
85
*
mp_obj_get_int
(
a
rgs
[
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
);
stmhal/servo.h
View file @
0119fc75
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
);
...
...
stmhal/stm32f4xx_hal_msp.c
View file @
0119fc75
...
...
@@ -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
();
}
}
/**
* @}
*/
...
...
stmhal/stm32f4xx_it.c
View file @
0119fc75
...
...
@@ -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_T
im
Handle
);
HAL_TIM_IRQHandler
(
&
USBD_CDC_T
IM3_
Handle
);
}
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
stmhal/usbd_cdc_interface.c
View file @
0119fc75
...
...
@@ -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_T
im
Handle
;
TIM_HandleTypeDef
USBD_CDC_T
IM3_
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_T
im
Handle
)
!=
HAL_OK
)
if
(
HAL_TIM_Base_Start_IT
(
&
USBD_CDC_T
IM3_
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_T
im
Handle
.
Instance
=
USBD_CDC_TIMx
;
USBD_CDC_T
IM3_
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_T
im
Handle
.
Init
.
Period
=
(
USBD_CDC_POLLING_INTERVAL
*
1000
)
-
1
;
USBD_CDC_T
im
Handle
.
Init
.
Prescaler
=
84
-
1
;
USBD_CDC_T
im
Handle
.
Init
.
ClockDivision
=
0
;
USBD_CDC_T
im
Handle
.
Init
.
CounterMode
=
TIM_COUNTERMODE_UP
;
if
(
HAL_TIM_Base_Init
(
&
USBD_CDC_T
im
Handle
)
!=
HAL_OK
)
USBD_CDC_T
IM3_
Handle
.
Init
.
Period
=
(
USBD_CDC_POLLING_INTERVAL
*
1000
)
-
1
;
USBD_CDC_T
IM3_
Handle
.
Init
.
Prescaler
=
84
-
1
;
USBD_CDC_T
IM3_
Handle
.
Init
.
ClockDivision
=
0
;
USBD_CDC_T
IM3_
Handle
.
Init
.
CounterMode
=
TIM_COUNTERMODE_UP
;
if
(
HAL_TIM_Base_Init
(
&
USBD_CDC_T
IM3_
Handle
)
!=
HAL_OK
)
{
/* Initialization Error */
}
...
...
stmhal/usbd_cdc_interface.h
View file @
0119fc75
...
...
@@ -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() __
USART
3_FORCE_RESET()
#define USBD_CDC_TIMx_RELEASE_RESET() __
USART
3_RELEASE_RESET()
#define USBD_CDC_TIMx_FORCE_RESET() __
TIM
3_FORCE_RESET()
#define USBD_CDC_TIMx_RELEASE_RESET() __
TIM
3_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
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment