mirrors/micropython
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
69dee59ce4
micropython/teensy/servo.c
Damien George c5966128c7 Implement proper exception type hierarchy. 
Each built-in exception is now a type, with base type BaseException.
C exceptions are created by passing a pointer to the exception type to
make an instance of.  When raising an exception from the VM, an
instance is created automatically if an exception type is raised (as
opposed to an exception instance).

Exception matching (RT_BINARY_OP_EXCEPTION_MATCH) is now proper.

Handling of parse error changed to match new exceptions.

mp_const_type renamed to mp_type_type for consistency.
2014-02-15 16:10:44 +00:00

266 lines
8.4 KiB
C
Raw Blame History

#include <stdio.h>
#include "misc.h"
#include "mpconfig.h"
#include "qstr.h"
#include "nlr.h"
#include "obj.h"
#include "servo.h"
#include "Arduino.h"
#define MAX_SERVOS 12
#define INVALID_SERVO -1
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached
#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds
#define PDB_CONFIG (PDB_SC_TRGSEL(15) | PDB_SC_PDBEN | PDB_SC_PDBIE \
| PDB_SC_CONT | PDB_SC_PRESCALER(2) | PDB_SC_MULT(0))
#define PDB_PRESCALE 4
#define usToTicks(us) ((us) * (F_BUS / 1000) / PDB_PRESCALE / 1000)
#define ticksToUs(ticks) ((ticks) * PDB_PRESCALE * 1000 / (F_BUS / 1000))
static uint16_t servo_active_mask = 0;
static uint16_t servo_allocated_mask = 0;
static uint8_t servo_pin[MAX_SERVOS];
static uint16_t servo_ticks[MAX_SERVOS];
typedef struct _pyb_servo_obj_t {
mp_obj_base_t base;
uint servo_id;
uint min_usecs;
uint max_usecs;
} pyb_servo_obj_t;
#define clamp(v, min_val, max_val) ((v) < (min_val) ? (min_val) : (v) > (max_val) ? (max_val) : (v))
static float map_uint_to_float(uint x, uint in_min, uint in_max, float out_min, float out_max)
{
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + (float)out_min;
}
static uint map_float_to_uint(float x, float in_min, float in_max, uint out_min, uint out_max)
{
return (int)((x - in_min) * (float)(out_max - out_min) / (in_max - in_min) + (float)out_min);
}
static mp_obj_t servo_obj_attach(mp_obj_t self_in, mp_obj_t pin_obj) {
pyb_servo_obj_t *self = self_in;
uint pin = mp_obj_get_int(pin_obj);
if (pin > CORE_NUM_DIGITAL) {
goto pin_error;
}
pinMode(pin, OUTPUT);
servo_pin[self->servo_id] = pin;
servo_active_mask |= (1 << self->servo_id);
if (!(SIM_SCGC6 & SIM_SCGC6_PDB)) {
SIM_SCGC6 |= SIM_SCGC6_PDB; // TODO: use bitband for atomic bitset
PDB0_MOD = 0xFFFF;
PDB0_CNT = 0;
PDB0_IDLY = 0;
PDB0_SC = PDB_CONFIG;
// TODO: maybe this should be a higher priority than most
// other interrupts (init all to some default?)
PDB0_SC = PDB_CONFIG | PDB_SC_SWTRIG;
}
NVIC_ENABLE_IRQ(IRQ_PDB);
return mp_const_none;
pin_error:
nlr_jump(mp_obj_new_exception_msg_varg(MP_QSTR_ValueError, "pin %d does not exist", pin));
}
static mp_obj_t servo_obj_detach(mp_obj_t self_in) {
//pyb_servo_obj_t *self = self_in;
return mp_const_none;
}
static mp_obj_t servo_obj_pin(mp_obj_t self_in) {
pyb_servo_obj_t *self = self_in;
return MP_OBJ_NEW_SMALL_INT(servo_pin[self->servo_id]);
}
static mp_obj_t servo_obj_min_usecs(int n_args, const mp_obj_t *args) {
pyb_servo_obj_t *self = args[0];
if (n_args == 1) {
// get min
return MP_OBJ_NEW_SMALL_INT(self->min_usecs);
}
// Set min
self->min_usecs = mp_obj_get_int(args[1]);
return mp_const_none;
}
static mp_obj_t servo_obj_max_usecs(int n_args, const mp_obj_t *args) {
pyb_servo_obj_t *self = args[0];
if (n_args == 1) {
// get max
return MP_OBJ_NEW_SMALL_INT(self->max_usecs);
}
// Set max
self->max_usecs = mp_obj_get_int(args[1]);
return mp_const_none;
}
static mp_obj_t servo_obj_angle(int n_args, const mp_obj_t *args) {
pyb_servo_obj_t *self = args[0];
if (n_args == 1) {
// get
float angle = map_uint_to_float(servo_ticks[self->servo_id],
usToTicks(self->min_usecs),
usToTicks(self->max_usecs),
0.0, 180.0);
return mp_obj_new_float(angle);
}
// Set
float angle = mp_obj_get_float(args[1]);
if (angle < 0.0F) {
angle = 0.0F;
}
if (angle > 180.0F) {
angle = 180.0F;
}
servo_ticks[self->servo_id] = map_float_to_uint(angle,
0.0F, 180.0F,
usToTicks(self->min_usecs),
usToTicks(self->max_usecs));
return mp_const_none;
}
static mp_obj_t servo_obj_usecs(int n_args, const mp_obj_t *args) {
pyb_servo_obj_t *self = args[0];
uint usecs;
if (n_args == 1) {
// get
return MP_OBJ_NEW_SMALL_INT(ticksToUs(servo_ticks[self->servo_id]));
}
// Set
usecs = mp_obj_get_int(args[1]);
if (self->min_usecs < self->max_usecs) {
usecs = clamp(usecs, self->min_usecs, self->max_usecs);
} else {
usecs = clamp(usecs, self->max_usecs, self->min_usecs);
}
servo_ticks[self->servo_id] = usToTicks(usecs);
return mp_const_none;
}
static mp_obj_t servo_obj_attached(mp_obj_t self_in) {
pyb_servo_obj_t *self = self_in;
uint attached = (servo_active_mask & (1 << self->servo_id)) != 0;
return MP_OBJ_NEW_SMALL_INT(attached);
}
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;
(void)kind;
print(env, "<Servo %lu>", self->servo_id);
}
static MP_DEFINE_CONST_FUN_OBJ_2(servo_obj_attach_obj, servo_obj_attach);
static MP_DEFINE_CONST_FUN_OBJ_1(servo_obj_detach_obj, servo_obj_detach);
static MP_DEFINE_CONST_FUN_OBJ_1(servo_obj_pin_obj, servo_obj_pin);
static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_min_usecs_obj, 1, 2, servo_obj_min_usecs);
static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_max_usecs_obj, 1, 2, servo_obj_max_usecs);
static MP_DEFINE_CONST_FUN_OBJ_1(servo_obj_attached_obj, servo_obj_attached);
static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_angle_obj, 1, 2, servo_obj_angle);
static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_usecs_obj, 1, 2, servo_obj_usecs);
static const mp_method_t servo_methods[] = {
{ "attach", &servo_obj_attach_obj },
{ "detach", &servo_obj_detach_obj },
{ "pin", &servo_obj_pin_obj },
{ "min_usecs", &servo_obj_min_usecs_obj },
{ "max_usecs", &servo_obj_max_usecs_obj },
{ "attached", &servo_obj_attached_obj },
{ "angle", &servo_obj_angle_obj },
{ "usecs", &servo_obj_usecs_obj },
{ NULL, NULL },
};
/*
* Notes:
*
* ISR needs to know pin #, ticks
*/
static const mp_obj_type_t servo_obj_type = {
{ &mp_type_type },
.name = MP_QSTR_Servo,
.print = servo_obj_print,
.methods = servo_methods,
};
/* servo = pyb.Servo(pin, [min_uecs, [max_usecs]]) */
mp_obj_t pyb_Servo(void) {
uint16_t mask;
pyb_servo_obj_t *self = m_new_obj(pyb_servo_obj_t);
self->base.type = &servo_obj_type;
self->min_usecs = MIN_PULSE_WIDTH;
self->max_usecs = MAX_PULSE_WIDTH;
/* Find an unallocated servo id */
self->servo_id = 0;
for (mask=1; mask < (1<<MAX_SERVOS); mask <<= 1) {
if (!(servo_allocated_mask & mask)) {
servo_allocated_mask |= mask;
servo_active_mask &= ~mask;
servo_ticks[self->servo_id] = usToTicks(DEFAULT_PULSE_WIDTH);
return self;
}
self->servo_id++;
}
m_del_obj(pyb_servo_obj_t, self);
nlr_jump(mp_obj_new_exception_msg(&mp_type_ValueError, "No available servo ids"));
return mp_const_none;
}
void pdb_isr(void)
{
static int8_t channel = 0, channel_high = MAX_SERVOS;
static uint32_t tick_accum = 0;
uint32_t ticks;
int32_t wait_ticks;
// first, if any channel was left high from the previous
// run, now is the time to shut it off
if (servo_active_mask & (1 << channel_high)) {
digitalWrite(servo_pin[channel_high], LOW);
channel_high = MAX_SERVOS;
}
// search for the next channel to turn on
while (channel < MAX_SERVOS) {
if (servo_active_mask & (1 << channel)) {
digitalWrite(servo_pin[channel], HIGH);
channel_high = channel;
ticks = servo_ticks[channel];
tick_accum += ticks;
PDB0_IDLY += ticks;
PDB0_SC = PDB_CONFIG | PDB_SC_LDOK;
channel++;
return;
}
channel++;
}
// when all channels have output, wait for the
// minimum refresh interval
wait_ticks = usToTicks(REFRESH_INTERVAL) - tick_accum;
if (wait_ticks < usToTicks(100)) wait_ticks = usToTicks(100);
else if (wait_ticks > 60000) wait_ticks = 60000;
tick_accum += wait_ticks;
PDB0_IDLY += wait_ticks;
PDB0_SC = PDB_CONFIG | PDB_SC_LDOK;
// if this wait is enough to satisfy the refresh
// interval, next time begin again at channel zero
if (tick_accum >= usToTicks(REFRESH_INTERVAL)) {
tick_accum = 0;
channel = 0;
}
}
Reference in New Issue View Git Blame Copy Permalink