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
22934713
Commit
22934713
authored
Apr 09, 2014
by
Damien George
Browse files
stmhal: Add functionality to Servo object.
Can now calibrate, set pulse width, angle and speed.
parent
d9ec625d
Changes
2
Hide whitespace changes
Inline
Side-by-side
stmhal/qstrdefsport.h
View file @
22934713
...
...
@@ -108,7 +108,10 @@ Q(dma)
// for Servo object
Q
(
Servo
)
Q
(
pulse_width
)
Q
(
calibrate
)
Q
(
angle
)
Q
(
speed
)
// for os module
Q
(
os
)
...
...
stmhal/servo.c
View file @
22934713
...
...
@@ -20,11 +20,16 @@
typedef
struct
_pyb_servo_obj_t
{
mp_obj_base_t
base
;
uint16_t
servo_id
;
uint16_t
time_left
;
uint8_t
servo_id
;
uint8_t
pulse_min
;
// units of 10us
uint8_t
pulse_max
;
// units of 10us
uint8_t
pulse_centre
;
// units of 10us
uint8_t
pulse_angle_90
;
// units of 10us; pulse at 90 degrees, minus pulse_centre
uint8_t
pulse_speed_100
;
// units of 10us; pulse at 100% forward speed, minus pulse_centre
uint16_t
pulse_cur
;
// units of 10us
uint16_t
pulse_dest
;
// units of 10us
int16_t
pulse_accum
;
uint16_t
pulse_cur
;
uint16_t
pulse_dest
;
uint16_t
time_left
;
}
pyb_servo_obj_t
;
STATIC
pyb_servo_obj_t
pyb_servo_obj
[
PYB_SERVO_NUM
];
...
...
@@ -36,9 +41,14 @@ void servo_init(void) {
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
].
time_left
=
0
;
pyb_servo_obj
[
i
].
pulse_cur
=
150
;
// units of 10us
pyb_servo_obj
[
i
].
pulse_min
=
64
;
pyb_servo_obj
[
i
].
pulse_max
=
242
;
pyb_servo_obj
[
i
].
pulse_centre
=
150
;
pyb_servo_obj
[
i
].
pulse_angle_90
=
97
;
pyb_servo_obj
[
i
].
pulse_speed_100
=
70
;
pyb_servo_obj
[
i
].
pulse_cur
=
150
;
pyb_servo_obj
[
i
].
pulse_dest
=
0
;
pyb_servo_obj
[
i
].
time_left
=
0
;
}
}
...
...
@@ -47,6 +57,13 @@ void servo_timer_irq_callback(void) {
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
)
{
// clamp pulse to within min/max
if
(
s
->
pulse_dest
<
s
->
pulse_min
)
{
s
->
pulse_dest
=
s
->
pulse_min
;
}
else
if
(
s
->
pulse_dest
>
s
->
pulse_max
)
{
s
->
pulse_dest
=
s
->
pulse_max
;
}
// adjust cur to get closer to dest
if
(
s
->
time_left
<=
1
)
{
s
->
pulse_cur
=
s
->
pulse_dest
;
s
->
time_left
=
0
;
...
...
@@ -57,6 +74,7 @@ void servo_timer_irq_callback(void) {
s
->
time_left
--
;
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
;
...
...
@@ -135,7 +153,7 @@ MP_DEFINE_CONST_FUN_OBJ_2(pyb_pwm_set_obj, pyb_pwm_set);
STATIC
void
pyb_servo_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 at %lu>"
,
self
->
servo_id
,
self
->
pulse_cur
);
print
(
env
,
"<Servo %lu at %lu
us
>"
,
self
->
servo_id
,
10
*
self
->
pulse_cur
);
}
STATIC
mp_obj_t
pyb_servo_make_new
(
mp_obj_t
type_in
,
uint
n_args
,
uint
n_kw
,
const
mp_obj_t
*
args
)
{
...
...
@@ -159,20 +177,64 @@ STATIC mp_obj_t pyb_servo_make_new(mp_obj_t type_in, uint n_args, uint n_kw, con
return
s
;
}
STATIC
mp_obj_t
pyb_servo_pulse_width
(
uint
n_args
,
const
mp_obj_t
*
args
)
{
pyb_servo_obj_t
*
self
=
args
[
0
];
if
(
n_args
==
1
)
{
// get pulse width, in us
return
mp_obj_new_int
(
10
*
self
->
pulse_cur
);
}
else
{
// set pulse width, in us
self
->
pulse_dest
=
mp_obj_get_int
(
args
[
1
])
/
10
;
self
->
time_left
=
0
;
servo_timer_irq_callback
();
return
mp_const_none
;
}
}
STATIC
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN
(
pyb_servo_pulse_width_obj
,
1
,
2
,
pyb_servo_pulse_width
);
STATIC
mp_obj_t
pyb_servo_calibrate
(
uint
n_args
,
const
mp_obj_t
*
args
)
{
pyb_servo_obj_t
*
self
=
args
[
0
];
if
(
n_args
==
1
)
{
// get calibration values
mp_obj_t
tuple
[
5
];
tuple
[
0
]
=
mp_obj_new_int
(
10
*
self
->
pulse_min
);
tuple
[
1
]
=
mp_obj_new_int
(
10
*
self
->
pulse_max
);
tuple
[
2
]
=
mp_obj_new_int
(
10
*
self
->
pulse_centre
);
tuple
[
3
]
=
mp_obj_new_int
(
10
*
(
self
->
pulse_angle_90
+
self
->
pulse_centre
));
tuple
[
4
]
=
mp_obj_new_int
(
10
*
(
self
->
pulse_speed_100
+
self
->
pulse_centre
));
return
mp_obj_new_tuple
(
5
,
tuple
);
}
else
if
(
n_args
>=
4
)
{
// set min, max, centre
self
->
pulse_min
=
mp_obj_get_int
(
args
[
1
])
/
10
;
self
->
pulse_max
=
mp_obj_get_int
(
args
[
2
])
/
10
;
self
->
pulse_centre
=
mp_obj_get_int
(
args
[
3
])
/
10
;
if
(
n_args
==
4
)
{
return
mp_const_none
;
}
else
if
(
n_args
==
6
)
{
self
->
pulse_angle_90
=
mp_obj_get_int
(
args
[
4
])
/
10
-
self
->
pulse_centre
;
self
->
pulse_speed_100
=
mp_obj_get_int
(
args
[
5
])
/
10
-
self
->
pulse_centre
;
return
mp_const_none
;
}
}
// bad number of arguments
nlr_raise
(
mp_obj_new_exception_msg_varg
(
&
mp_type_TypeError
,
"calibrate expecting 1, 4 or 6 arguments, got %d"
,
n_args
));
}
STATIC
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN
(
pyb_servo_calibrate_obj
,
1
,
6
,
pyb_servo_calibrate
);
STATIC
mp_obj_t
pyb_servo_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
);
return
mp_obj_new_int
((
self
->
pulse_cur
-
self
->
pulse_centre
)
*
90
/
self
->
pulse_angle_90
);
}
else
{
#if MICROPY_ENABLE_FLOAT
machine_int_t
v
=
152
+
85
.
0
*
mp_obj_get_float
(
args
[
1
])
/
90
.
0
;
self
->
pulse_dest
=
self
->
pulse_centre
+
self
->
pulse_angle_9
0
*
mp_obj_get_float
(
args
[
1
])
/
90
.
0
;
#else
machine_int_t
v
=
152
+
85
*
mp_obj_get_int
(
args
[
1
])
/
90
;
self
->
pulse_dest
=
self
->
pulse_centre
+
self
->
pulse_angle_90
*
mp_obj_get_int
(
args
[
1
])
/
90
;
#endif
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
;
...
...
@@ -188,8 +250,37 @@ STATIC mp_obj_t pyb_servo_angle(uint n_args, const mp_obj_t *args) {
STATIC
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN
(
pyb_servo_angle_obj
,
1
,
3
,
pyb_servo_angle
);
STATIC
mp_obj_t
pyb_servo_speed
(
uint
n_args
,
const
mp_obj_t
*
args
)
{
pyb_servo_obj_t
*
self
=
args
[
0
];
if
(
n_args
==
1
)
{
// get speed
return
mp_obj_new_int
((
self
->
pulse_cur
-
self
->
pulse_centre
)
*
100
/
self
->
pulse_speed_100
);
}
else
{
#if MICROPY_ENABLE_FLOAT
self
->
pulse_dest
=
self
->
pulse_centre
+
self
->
pulse_speed_100
*
mp_obj_get_float
(
args
[
1
])
/
100
.
0
;
#else
self
->
pulse_dest
=
self
->
pulse_centre
+
self
->
pulse_speed_100
*
mp_obj_get_int
(
args
[
1
])
/
100
;
#endif
if
(
n_args
==
2
)
{
// set speed immediately
self
->
time_left
=
0
;
}
else
{
// set speed 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
;
}
}
STATIC
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN
(
pyb_servo_speed_obj
,
1
,
3
,
pyb_servo_speed
);
STATIC
const
mp_map_elem_t
pyb_servo_locals_dict_table
[]
=
{
{
MP_OBJ_NEW_QSTR
(
MP_QSTR_pulse_width
),
(
mp_obj_t
)
&
pyb_servo_pulse_width_obj
},
{
MP_OBJ_NEW_QSTR
(
MP_QSTR_calibrate
),
(
mp_obj_t
)
&
pyb_servo_calibrate_obj
},
{
MP_OBJ_NEW_QSTR
(
MP_QSTR_angle
),
(
mp_obj_t
)
&
pyb_servo_angle_obj
},
{
MP_OBJ_NEW_QSTR
(
MP_QSTR_speed
),
(
mp_obj_t
)
&
pyb_servo_speed_obj
},
};
STATIC
MP_DEFINE_CONST_DICT
(
pyb_servo_locals_dict
,
pyb_servo_locals_dict_table
);
...
...
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