From 6f9c03676bdf66274c199d296e98e7027324d22e Mon Sep 17 00:00:00 2001 From: Dave Hylands Date: Tue, 14 Jan 2014 22:23:56 -0800 Subject: [PATCH] Updated teensy to work with latest on master Added analogRead, analogWriteXxx and servo support for teensy. --- teensy/Makefile | 7 +- teensy/led.c | 1 + teensy/lexerfatfs.c | 9 +- teensy/lexermemzip.c | 5 +- teensy/main.c | 151 ++++++++++++++++-------- teensy/qstrdefsport.h | 36 ++++++ teensy/servo.c | 264 ++++++++++++++++++++++++++++++++++++++++++ teensy/servo.h | 2 + teensy/usart.c | 38 +++--- 9 files changed, 448 insertions(+), 65 deletions(-) create mode 100644 teensy/qstrdefsport.h create mode 100644 teensy/servo.c create mode 100644 teensy/servo.h diff --git a/teensy/Makefile b/teensy/Makefile index 0cd7272e71..5630103e34 100644 --- a/teensy/Makefile +++ b/teensy/Makefile @@ -1,5 +1,8 @@ include ../py/mkenv.mk +# qstr definitions (must come before including py.mk) +QSTR_DEFS = qstrdefsport.h + # include py core make definitions include ../py/py.mk @@ -32,6 +35,8 @@ SRC_C = \ lexerfatfs.c \ lexermemzip.c \ memzip.c \ + servo.c \ + usart.c \ usb.c \ STM_SRC_C = $(addprefix stm/,\ @@ -54,7 +59,7 @@ SRC_TEENSY = \ usb_serial.c \ yield.c \ -OBJ = $(addprefix $(BUILD)/, $(SRC_C:.c=.o) $(STM_SRC_C:.c=.o) $(STM_SRC_S:.s=.o) $(SRC_TEENSY:.c=.o)) $(PY_O) +OBJ = $(PY_O) $(addprefix $(BUILD)/, $(SRC_C:.c=.o) $(STM_SRC_C:.c=.o) $(STM_SRC_S:.s=.o) $(SRC_TEENSY:.c=.o)) #LIB = -lreadline # the following is needed for BSD #LIB += -ltermcap diff --git a/teensy/led.c b/teensy/led.c index 0cef354c27..5eace0d373 100644 --- a/teensy/led.c +++ b/teensy/led.c @@ -2,6 +2,7 @@ #include "misc.h" #include "mpconfig.h" +#include "qstr.h" #include "obj.h" #include "led.h" diff --git a/teensy/lexerfatfs.c b/teensy/lexerfatfs.c index 2ded0756f7..95084b6501 100644 --- a/teensy/lexerfatfs.c +++ b/teensy/lexerfatfs.c @@ -2,11 +2,13 @@ #include #include "misc.h" +#include "mpconfig.h" +#include "qstr.h" #include "lexer.h" typedef int FIL; #include "../stm/lexerfatfs.h" -mp_lexer_t *mp_lexer_new_from_file(const char *filename, mp_lexer_file_buf_t *fb) { +mp_lexer_t *mp_lexer_new_from_file(const char *filename) { printf("import not implemented\n"); return NULL; } @@ -15,3 +17,8 @@ mp_lexer_t *mp_import_open_file(qstr mod_name) { printf("import not implemented\n"); return NULL; } + +mp_import_stat_t mp_import_stat(const char *path) { + // TODO implement me! + return MP_IMPORT_STAT_NO_EXIST; +} diff --git a/teensy/lexermemzip.c b/teensy/lexermemzip.c index 2f808ee429..3e15717acf 100644 --- a/teensy/lexermemzip.c +++ b/teensy/lexermemzip.c @@ -2,6 +2,8 @@ #include #include "misc.h" +#include "mpconfig.h" +#include "qstr.h" #include "lexer.h" #include "memzip.h" @@ -13,6 +15,7 @@ mp_lexer_t *mp_lexer_new_from_memzip_file(const char *filename) if (memzip_locate(filename, &data, &len) != MZ_OK) { return NULL; } - return mp_lexer_new_from_str_len(filename, (const char *)data, (uint)len, 0); + + return mp_lexer_new_from_str_len(qstr_from_str(filename), (const char *)data, (uint)len, 0); } diff --git a/teensy/main.c b/teensy/main.c index 0bc085aed9..bbfa7590fa 100644 --- a/teensy/main.c +++ b/teensy/main.c @@ -6,7 +6,7 @@ #include "nlr.h" #include "misc.h" #include "mpconfig.h" -#include "mpqstr.h" +#include "qstr.h" #include "lexer.h" #include "lexermemzip.h" #include "parse.h" @@ -15,6 +15,7 @@ #include "runtime0.h" #include "runtime.h" #include "repl.h" +#include "servo.h" #include "usb.h" #include "gc.h" #include "led.h" @@ -54,6 +55,32 @@ static const char *help_text = #endif ; +mp_obj_t pyb_analog_read(mp_obj_t pin_obj) { + uint pin = mp_obj_get_int(pin_obj); + int val = analogRead(pin); + return MP_OBJ_NEW_SMALL_INT(val); +} + +mp_obj_t pyb_analog_write(mp_obj_t pin_obj, mp_obj_t val_obj) { + uint pin = mp_obj_get_int(pin_obj); + int val = mp_obj_get_int(val_obj); + analogWrite(pin, val); + return mp_const_none; +} + +mp_obj_t pyb_analog_write_resolution(mp_obj_t res_obj) { + int res = mp_obj_get_int(res_obj); + analogWriteResolution(res); + return mp_const_none; +} + +mp_obj_t pyb_analog_write_frequency(mp_obj_t pin_obj, mp_obj_t freq_obj) { + uint pin = mp_obj_get_int(pin_obj); + int freq = mp_obj_get_int(freq_obj); + analogWriteFrequency(pin, freq); + return mp_const_none; +} + // get some help about available functions static mp_obj_t pyb_help(void) { printf("%s", help_text); @@ -181,16 +208,26 @@ mp_obj_t pyb_hid_send_report(mp_obj_t arg) { } #endif -static qstr pyb_config_source_dir = 0; -static qstr pyb_config_main = 0; +static mp_obj_t pyb_config_source_dir = MP_OBJ_NULL; +static mp_obj_t pyb_config_main = MP_OBJ_NULL; mp_obj_t pyb_source_dir(mp_obj_t source_dir) { - pyb_config_source_dir = mp_obj_get_qstr(source_dir); + if (MP_OBJ_IS_STR(source_dir)) { + pyb_config_source_dir = source_dir; + printf("source_dir = '"); + mp_obj_print(source_dir, PRINT_STR); + printf("'\n"); + } return mp_const_none; } mp_obj_t pyb_main(mp_obj_t main) { - pyb_config_main = mp_obj_get_qstr(main); + if (MP_OBJ_IS_STR(main)) { + pyb_config_main = main; + printf("main = '"); + mp_obj_print(main, PRINT_STR); + printf("'\n"); + } return mp_const_none; } @@ -205,7 +242,7 @@ mp_obj_t pyb_led(mp_obj_t state) { } mp_obj_t pyb_run(mp_obj_t filename_obj) { - const char *filename = qstr_str(mp_obj_get_qstr(filename_obj)); + const char *filename = qstr_str(mp_obj_str_get_qstr(filename_obj)); do_file(filename); return mp_const_none; } @@ -309,15 +346,24 @@ bool do_file(const char *filename) { return false; } - mp_parse_node_t pn = mp_parse(lex, MP_PARSE_FILE_INPUT); + qstr parse_exc_id; + const char *parse_exc_msg; + mp_parse_node_t pn = mp_parse(lex, MP_PARSE_FILE_INPUT, &parse_exc_id, &parse_exc_msg); qstr source_name = mp_lexer_source_name(lex); - mp_lexer_free(lex); if (pn == MP_PARSE_NODE_NULL) { + // parse error + mp_lexer_show_error_pythonic_prefix(lex); + printf("%s: %s\n", qstr_str(parse_exc_id), parse_exc_msg); + mp_lexer_free(lex); return false; } + mp_lexer_free(lex); + mp_obj_t module_fun = mp_compile(pn, source_name, false); + mp_parse_node_free(pn); + if (module_fun == mp_const_none) { return false; } @@ -329,7 +375,7 @@ bool do_file(const char *filename) { return true; } else { // uncaught exception - mp_obj_print((mp_obj_t)nlr.ret_val); + mp_obj_print((mp_obj_t)nlr.ret_val, PRINT_REPR); printf("\n"); return false; } @@ -340,7 +386,7 @@ void do_repl(void) { stdout_tx_str("Type \"help()\" for more information.\r\n"); vstr_t line; - vstr_init(&line); + vstr_init(&line, 32); for (;;) { vstr_reset(&line); @@ -366,12 +412,21 @@ void do_repl(void) { } } - mp_lexer_t *lex = mp_lexer_new_from_str_len("", vstr_str(&line), vstr_len(&line), 0); - mp_parse_node_t pn = mp_parse(lex, MP_PARSE_SINGLE_INPUT); - mp_lexer_free(lex); + mp_lexer_t *lex = mp_lexer_new_from_str_len(MP_QSTR__lt_stdin_gt_, vstr_str(&line), vstr_len(&line), 0); + qstr parse_exc_id; + const char *parse_exc_msg; + mp_parse_node_t pn = mp_parse(lex, MP_PARSE_SINGLE_INPUT, &parse_exc_id, &parse_exc_msg); + qstr source_name = mp_lexer_source_name(lex); - if (pn != MP_PARSE_NODE_NULL) { - mp_obj_t module_fun = mp_compile(pn, true); + if (pn == MP_PARSE_NODE_NULL) { + // parse error + mp_lexer_show_error_pythonic_prefix(lex); + printf("%s: %s\n", qstr_str(parse_exc_id), parse_exc_msg); + mp_lexer_free(lex); + } else { + // parse okay + mp_lexer_free(lex); + mp_obj_t module_fun = mp_compile(pn, source_name, true); if (module_fun != mp_const_none) { nlr_buf_t nlr; uint32_t start = micros(); @@ -381,11 +436,11 @@ void do_repl(void) { // optional timing if (0) { uint32_t ticks = micros() - start; // TODO implement a function that does this properly - printf("(took %lu us)\n", ticks); + printf("(took %lu ms)\n", ticks); } } else { // uncaught exception - mp_obj_print((mp_obj_t)nlr.ret_val); + mp_obj_print((mp_obj_t)nlr.ret_val, PRINT_REPR); printf("\n"); } } @@ -396,11 +451,15 @@ void do_repl(void) { } int main(void) { - pinMode(LED_BUILTIN, OUTPUT); + pinMode(LED_BUILTIN, OUTPUT); +#if 0 // Wait for host side to get connected while (!usb_vcp_is_connected()) { ; } +#else + delay(1000); +#endif led_init(); led_state(PYB_LED_BUILTIN, 1); @@ -415,28 +474,34 @@ soft_reset: qstr_init(); rt_init(); -#if 1 // add some functions to the python namespace { - rt_store_name(qstr_from_str_static("help"), rt_make_function_0(pyb_help)); - mp_obj_t m = mp_obj_new_module(qstr_from_str_static("pyb")); - rt_store_attr(m, qstr_from_str_static("info"), rt_make_function_0(pyb_info)); - rt_store_attr(m, qstr_from_str_static("source_dir"), rt_make_function_1(pyb_source_dir)); - rt_store_attr(m, qstr_from_str_static("main"), rt_make_function_1(pyb_main)); - rt_store_attr(m, qstr_from_str_static("gc"), rt_make_function_0(pyb_gc)); - rt_store_attr(m, qstr_from_str_static("delay"), rt_make_function_1(pyb_delay)); - rt_store_attr(m, qstr_from_str_static("led"), rt_make_function_1(pyb_led)); - rt_store_attr(m, qstr_from_str_static("Led"), rt_make_function_1(pyb_Led)); - rt_store_attr(m, qstr_from_str_static("gpio"), (mp_obj_t)&pyb_gpio_obj); - rt_store_name(qstr_from_str_static("pyb"), m); - rt_store_name(qstr_from_str_static("run"), rt_make_function_1(pyb_run)); - } -#endif + rt_store_name(MP_QSTR_help, rt_make_function_n(0, pyb_help)); + mp_obj_t m = mp_obj_new_module(MP_QSTR_pyb); + rt_store_attr(m, MP_QSTR_info, rt_make_function_n(0, pyb_info)); + rt_store_attr(m, MP_QSTR_source_dir, rt_make_function_n(1, pyb_source_dir)); + rt_store_attr(m, MP_QSTR_main, rt_make_function_n(1, pyb_main)); + rt_store_attr(m, MP_QSTR_gc, rt_make_function_n(0, pyb_gc)); + rt_store_attr(m, MP_QSTR_delay, rt_make_function_n(1, pyb_delay)); + rt_store_attr(m, MP_QSTR_led, rt_make_function_n(1, pyb_led)); + rt_store_attr(m, MP_QSTR_Led, rt_make_function_n(1, pyb_Led)); + rt_store_attr(m, MP_QSTR_analogRead, rt_make_function_n(1, pyb_analog_read)); + rt_store_attr(m, MP_QSTR_analogWrite, rt_make_function_n(2, pyb_analog_write)); + rt_store_attr(m, MP_QSTR_analogWriteResolution, rt_make_function_n(1, pyb_analog_write_resolution)); + rt_store_attr(m, MP_QSTR_analogWriteFrequency, rt_make_function_n(2, pyb_analog_write_frequency)); + rt_store_attr(m, MP_QSTR_gpio, (mp_obj_t)&pyb_gpio_obj); + rt_store_attr(m, MP_QSTR_Servo, rt_make_function_n(0, pyb_Servo)); + rt_store_name(MP_QSTR_pyb, m); + rt_store_name(MP_QSTR_run, rt_make_function_n(1, pyb_run)); + } + + printf("About execute /boot.py\n"); if (!do_file("/boot.py")) { printf("Unable to open '/boot.py'\n"); flash_error(4); } + printf("Done executing /boot.py\n"); // Turn bootup LED off led_state(PYB_LED_BUILTIN, 0); @@ -445,21 +510,23 @@ soft_reset: { vstr_t *vstr = vstr_new(); vstr_add_str(vstr, "/"); - if (pyb_config_source_dir == 0) { + if (pyb_config_source_dir == MP_OBJ_NULL) { vstr_add_str(vstr, "src"); } else { - vstr_add_str(vstr, qstr_str(pyb_config_source_dir)); + vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_source_dir)); } vstr_add_char(vstr, '/'); - if (pyb_config_main == 0) { + if (pyb_config_main == MP_OBJ_NULL) { vstr_add_str(vstr, "main.py"); } else { - vstr_add_str(vstr, qstr_str(pyb_config_main)); + vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_main)); } + printf("About execute '%s'\n", vstr_str(vstr)); if (!do_file(vstr_str(vstr))) { printf("Unable to open '%s'\n", vstr_str(vstr)); flash_error(3); } + printf("Done executing '%s'\n", vstr_str(vstr)); vstr_free(vstr); } @@ -471,16 +538,6 @@ soft_reset: goto soft_reset; } -double __aeabi_f2d(float x) { - // TODO - return 0.0; -} - -float __aeabi_d2f(double x) { - // TODO - return 0.0; -} - double sqrt(double x) { // TODO return 0.0; diff --git a/teensy/qstrdefsport.h b/teensy/qstrdefsport.h new file mode 100644 index 0000000000..8b23a86bf0 --- /dev/null +++ b/teensy/qstrdefsport.h @@ -0,0 +1,36 @@ +// qstrs specific to this port + +Q(help) +Q(pyb) +Q(info) +Q(sd_test) +Q(stop) +Q(standby) +Q(source_dir) +Q(main) +Q(sync) +Q(gc) +Q(delay) +Q(switch) +Q(servo) +Q(pwm) +Q(accel) +Q(mma_read) +Q(mma_mode) +Q(hid) +Q(time) +Q(rand) +Q(Led) +Q(led) +Q(Servo) +Q(I2C) +Q(gpio) +Q(Usart) +Q(ADC) +Q(open) +Q(analogRead) +Q(analogWrite) +Q(analogWriteResolution) +Q(analogWriteFrequency) +Q(run) + diff --git a/teensy/servo.c b/teensy/servo.c new file mode 100644 index 0000000000..3d6c9420d2 --- /dev/null +++ b/teensy/servo.c @@ -0,0 +1,264 @@ +#include +#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) { + pyb_servo_obj_t *self = self_in; + print(env, "", 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_const_type }, + "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<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_QSTR_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; + } +} diff --git a/teensy/servo.h b/teensy/servo.h new file mode 100644 index 0000000000..c4a5bd49b1 --- /dev/null +++ b/teensy/servo.h @@ -0,0 +1,2 @@ +mp_obj_t pyb_Servo(void); + diff --git a/teensy/usart.c b/teensy/usart.c index 419dac9527..a700e8e379 100644 --- a/teensy/usart.c +++ b/teensy/usart.c @@ -1,30 +1,38 @@ #include "misc.h" +#include "mpconfig.h" +#include "qstr.h" +#include "obj.h" #include "../stm/usart.h" -void usart_init(void) { +pyb_usart_t pyb_usart_global_debug = PYB_USART_NONE; + +void usart_init(pyb_usart_t usart_id, uint32_t baudrate) +{ + (void)usart_id; + (void)baudrate; } -bool usart_is_enabled(void) { +bool usart_rx_any(pyb_usart_t usart_id) +{ + (void)usart_id; return false; } -bool usart_rx_any(void) { - return false; -} - -int usart_rx_char(void) { +int usart_rx_char(pyb_usart_t usart_id) +{ + (void)usart_id; return 0; } -void usart_tx_char(int c) { - (void)c; -} - -void usart_tx_str(const char *str) { +void usart_tx_str(pyb_usart_t usart_id, const char *str) +{ + (void)usart_id; (void)str; } -void usart_tx_strn_cooked(const char *str, int len) { - (void)str; - (void)len; +void usart_tx_strn_cooked(pyb_usart_t usart_id, const char *str, int len) +{ + (void)usart_id; + (void)str; + (void)len; }