c5966128c7
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.
266 lines
8.4 KiB
C
266 lines
8.4 KiB
C
#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;
|
|
}
|
|
}
|