From 4a8abc2060329ded5ec298b268e1039c105aa92f Mon Sep 17 00:00:00 2001 From: Philip Gladstone Date: Sun, 26 Jun 2016 08:19:06 -0400 Subject: [PATCH] Add support for driving instrument gauge stepper motors (#1355) --- app/driver/switec.c | 391 ++++++++++++++++++++++++++++++++++++ app/include/driver/switec.h | 21 ++ app/include/user_modules.h | 1 + app/modules/switec.c | 212 +++++++++++++++++++ docs/en/modules/switec.md | 153 ++++++++++++++ mkdocs.yml | 1 + 6 files changed, 779 insertions(+) create mode 100644 app/driver/switec.c create mode 100644 app/include/driver/switec.h create mode 100644 app/modules/switec.c create mode 100644 docs/en/modules/switec.md diff --git a/app/driver/switec.c b/app/driver/switec.c new file mode 100644 index 00000000..04d1cc29 --- /dev/null +++ b/app/driver/switec.c @@ -0,0 +1,391 @@ +/* + * Module for interfacing with Switec instrument steppers (and + * similar devices). These are the steppers that are used in automotive + * instrument panels and the like. Run off 5 volts at low current. + * + * Code inspired by: + * + * SwitecX25 Arduino Library + * Guy Carpenter, Clearwater Software - 2012 + * + * Licensed under the BSD2 license, see license.txt for details. + * + * NodeMcu integration by Philip Gladstone, N1DQ + */ + +#include "platform.h" +#include "c_types.h" +#include "../libc/c_stdlib.h" +#include "../libc/c_stdio.h" +#include "driver/switec.h" +#include "ets_sys.h" +#include "os_type.h" +#include "osapi.h" +#include "hw_timer.h" +#include "user_interface.h" +#include "task/task.h" + +#define N_STATES 6 +// +// First pin passed to setup corresponds to bit 3 +// On the motor, the pins are arranged +// +// 4 1 +// +// 3 2 +// +// The direction of rotation can be reversed by reordering the pins +// +// State 3 2 1 0 A B Value +// 0 1 0 0 1 - - 0x9 +// 1 0 0 0 1 . - 0x1 +// 2 0 1 1 1 + . 0x7 +// 3 0 1 1 0 + + 0x6 +// 4 1 1 1 0 . + 0xE +// 5 1 0 0 0 - . 0x8 +static const uint8_t stateMap[N_STATES] = {0x9, 0x1, 0x7, 0x6, 0xE, 0x8}; + +typedef struct { + uint8_t current_state; + uint8_t stopped; + int8_t dir; + uint32_t mask; + uint32_t pinstate[N_STATES]; + uint32_t next_time; + int16_t target_step; + int16_t current_step; + uint16_t vel; + uint16_t max_vel; + uint16_t min_delay; + task_handle_t task_number; +} DATA; + +static DATA *data[SWITEC_CHANNEL_COUNT]; +static volatile char timer_active; + +#define MAXVEL 255 + +// Note that this has to be global so that the compiler does not +// put it into ROM. +uint8_t switec_accel_table[][2] = { + { 20, 3000 >> 4}, + { 50, 1500 >> 4}, + { 100, 1000 >> 4}, + { 150, 800 >> 4}, + { MAXVEL, 600 >> 4} +}; + +static void ICACHE_RAM_ATTR timer_interrupt(os_param_t); + +#define TIMER_OWNER ((os_param_t) 'S') + + +// Just takes the channel number +int switec_close(uint32_t channel) +{ + if (channel >= sizeof(data) / sizeof(data[0])) { + return -1; + } + + DATA *d = data[channel]; + + if (!d) { + return 0; + } + + if (!d->stopped) { + return -1; + } + + // Set pins as input + gpio_output_set(0, 0, 0, d->mask); + + data[channel] = NULL; + c_free(d); + + // See if there are any other channels active + for (channel = 0; channel < sizeof(data)/sizeof(data[0]); channel++) { + if (data[channel]) { + break; + } + } + + // If not, then disable the interrupt + if (channel >= sizeof(data) / sizeof(data[0])) { + platform_hw_timer_close(TIMER_OWNER); + } + + return 0; +} + +static __attribute__((always_inline)) inline void write_io(DATA *d) +{ + uint32_t pin_state = d->pinstate[d->current_state]; + + gpio_output_set(pin_state, d->mask & ~pin_state, 0, 0); +} + +static __attribute__((always_inline)) inline void step_up(DATA *d) +{ + d->current_step++; + d->current_state = (d->current_state + 1) % N_STATES; + write_io(d); +} + +static __attribute__((always_inline)) inline void step_down(DATA *d) +{ + d->current_step--; + d->current_state = (d->current_state + N_STATES - 1) % N_STATES; + write_io(d); +} + +static void ICACHE_RAM_ATTR timer_interrupt(os_param_t p) +{ + // This function really is running at interrupt level with everything + // else masked off. It should take as little time as necessary. + // + (void) p; + + int i; + uint32_t delay = 0xffffffff; + + // Loop over the channels to figure out which one needs action + for (i = 0; i < sizeof(data) / sizeof(data[0]); i++) { + DATA *d = data[i]; + if (!d || d->stopped) { + continue; + } + + uint32_t now = system_get_time(); + if (now < d->next_time) { + int need_to_wait = d->next_time - now; + if (need_to_wait < delay) { + delay = need_to_wait; + } + continue; + } + + // This channel is past it's action time. Need to process it + + // Are we done yet? + if (d->current_step == d->target_step && d->vel == 0) { + d->stopped = 1; + d->dir = 0; + task_post_low(d->task_number, 0); + continue; + } + + // if stopped, determine direction + if (d->vel == 0) { + d->dir = d->current_step < d->target_step ? 1 : -1; + // do not set to 0 or it could go negative in case 2 below + d->vel = 1; + } + + // Move the pointer by one step in the correct direction + if (d->dir > 0) { + step_up(d); + } else { + step_down(d); + } + + // determine delta, number of steps in current direction to target. + // may be negative if we are headed away from target + int delta = d->dir > 0 ? d->target_step - d->current_step : d->current_step - d->target_step; + + if (delta > 0) { + // case 1 : moving towards target (maybe under accel or decel) + if (delta <= d->vel) { + // time to declerate + d->vel--; + } else if (d->vel < d->max_vel) { + // accelerating + d->vel++; + } else { + // at full speed - stay there + } + } else { + // case 2 : at or moving away from target (slow down!) + d->vel--; + } + + // vel now defines delay + uint8_t row = 0; + // this is why vel must not be greater than the last vel in the table. + while (switec_accel_table[row][0] < d->vel) { + row++; + } + + uint32_t micro_delay = switec_accel_table[row][1] << 4; + if (micro_delay < d->min_delay) { + micro_delay = d->min_delay; + } + + // Figure out when we next need to take action + d->next_time = d->next_time + micro_delay; + if (d->next_time < now) { + d->next_time = now + micro_delay; + } + + // Figure out how long to wait + int need_to_wait = d->next_time - now; + if (need_to_wait < delay) { + delay = need_to_wait; + } + } + + if (delay < 1000000) { + if (delay < 50) { + delay = 50; + } + timer_active = 1; + platform_hw_timer_arm_us(TIMER_OWNER, delay); + } else { + timer_active = 0; + } +} + + +// The pin numbers are actual platform GPIO numbers +int switec_setup(uint32_t channel, int *pin, int max_deg_per_sec, task_handle_t task_number ) +{ + if (channel >= sizeof(data) / sizeof(data[0])) { + return -1; + } + + if (data[channel]) { + if (switec_close(channel)) { + return -1; + } + } + + DATA *d = (DATA *) c_zalloc(sizeof(DATA)); + if (!d) { + return -1; + } + + if (!data[0] && !data[1] && !data[2]) { + // We need to stup the timer as no channel was active before + // no autoreload + if (!platform_hw_timer_init(TIMER_OWNER, FRC1_SOURCE, FALSE)) { + // Failed to get the timer + c_free(d); + return -1; + } + } + + data[channel] = d; + int i; + + for (i = 0; i < 4; i++) { + // Build the mask for the pins to be output pins + d->mask |= 1 << pin[i]; + + int j; + // Now build the hi states for the pins according to the 6 phases above + for (j = 0; j < N_STATES; j++) { + if (stateMap[j] & (1 << (3 - i))) { + d->pinstate[j] |= 1 << pin[i]; + } + } + } + + d->max_vel = MAXVEL; + if (max_deg_per_sec == 0) { + max_deg_per_sec = 400; + } + d->min_delay = 1000000 / (3 * max_deg_per_sec); + d->task_number = task_number; + +#ifdef SWITEC_DEBUG + for (i = 0; i < 4; i++) { + c_printf("pin[%d]=%d\n", i, pin[i]); + } + + c_printf("Mask=0x%x\n", d->mask); + for (i = 0; i < N_STATES; i++) { + c_printf("pinstate[%d]=0x%x\n", i, d->pinstate[i]); + } +#endif + + // Set all pins as outputs + gpio_output_set(0, 0, d->mask, 0); + + platform_hw_timer_set_func(TIMER_OWNER, timer_interrupt, 0); + + return 0; +} + +// All this does is to assert that the current position is 0 +int switec_reset(uint32_t channel) +{ + if (channel >= sizeof(data) / sizeof(data[0])) { + return -1; + } + + DATA *d = data[channel]; + + if (!d || !d->stopped) { + return -1; + } + + d->current_step = d->target_step = 0; + + return 0; +} + +// Just takes the channel number and the position +int switec_moveto(uint32_t channel, int pos) +{ + if (channel >= sizeof(data) / sizeof(data[0])) { + return -1; + } + + DATA *d = data[channel]; + + if (!d) { + return -1; + } + + if (pos < 0) { + // This ensures that we don't slam into the endstop + d->max_vel = 50; + } else { + d->max_vel = MAXVEL; + } + + d->target_step = pos; + + // If the pointer is not moving, setup so that we start it + if (d->stopped) { + // reset the timer to avoid possible time overflow giving spurious deltas + d->next_time = system_get_time() + 1000; + d->stopped = false; + + if (!timer_active) { + timer_interrupt(0); + } + } + + return 0; +} + +// Get the current position, direction and target position +int switec_getpos(uint32_t channel, int32_t *pos, int32_t *dir, int32_t *target) +{ + if (channel >= sizeof(data) / sizeof(data[0])) { + return -1; + } + + DATA *d = data[channel]; + + if (!d) { + return -1; + } + + *pos = d->current_step; + *dir = d->stopped ? 0 : d->dir; + *target = d->target_step; + + return 0; +} diff --git a/app/include/driver/switec.h b/app/include/driver/switec.h new file mode 100644 index 00000000..2fd5b5c0 --- /dev/null +++ b/app/include/driver/switec.h @@ -0,0 +1,21 @@ +/* + * Definitions to access the Switec driver + */ +#ifndef __SWITEC_H__ +#define __SWITEC_H__ + +#include "c_types.h" + +#define SWITEC_CHANNEL_COUNT 3 + +int switec_setup(uint32_t channel, int *pin, int max_deg_per_sec, task_handle_t taskNumber ); + +int switec_close(uint32_t channel); + +int switec_moveto(uint32_t channel, int pos); + +int switec_reset(uint32_t channel); + +int switec_getpos(uint32_t channel, int32_t *pos, int32_t *dir, int32_t *target); + +#endif diff --git a/app/include/user_modules.h b/app/include/user_modules.h index be3ad1fa..cbb794c1 100644 --- a/app/include/user_modules.h +++ b/app/include/user_modules.h @@ -54,6 +54,7 @@ //#define LUA_USE_MODULES_SNTP #define LUA_USE_MODULES_SPI //#define LUA_USE_MODULES_STRUCT +//#define LUA_USE_MODULES_SWITEC //#define LUA_USE_MODULES_TM1829 #define LUA_USE_MODULES_TMR //#define LUA_USE_MODULES_TSL2561 diff --git a/app/modules/switec.c b/app/modules/switec.c new file mode 100644 index 00000000..e9628540 --- /dev/null +++ b/app/modules/switec.c @@ -0,0 +1,212 @@ +/* + * Module for interfacing with Switec instrument steppers (and + * similar devices). These are the steppers that are used in automotive + * instrument panels and the like. Run off 5 volts at low current. + * + * Code inspired by: + * + * SwitecX25 Arduino Library + * Guy Carpenter, Clearwater Software - 2012 + * + * Licensed under the BSD2 license, see license.txt for details. + * + * NodeMcu integration by Philip Gladstone, N1DQ + */ + +#include "module.h" +#include "lauxlib.h" +#include "platform.h" +#include "c_types.h" +#include "task/task.h" +#include "driver/switec.h" + +// This is the reference to the callbacks for when the pointer +// stops moving. +static int stopped_callback[SWITEC_CHANNEL_COUNT] = { LUA_NOREF, LUA_NOREF, LUA_NOREF }; +static task_handle_t tasknumber; + +static void callback_free(lua_State* L, unsigned int id) +{ + luaL_unref(L, LUA_REGISTRYINDEX, stopped_callback[id]); + stopped_callback[id] = LUA_NOREF; +} + +static void callback_set(lua_State* L, unsigned int id, int argNumber) +{ + if (lua_type(L, argNumber) == LUA_TFUNCTION || lua_type(L, argNumber) == LUA_TLIGHTFUNCTION) { + lua_pushvalue(L, argNumber); // copy argument (func) to the top of stack + callback_free(L, id); + stopped_callback[id] = luaL_ref(L, LUA_REGISTRYINDEX); + } +} + +static void callback_execute(lua_State* L, unsigned int id) +{ + if (stopped_callback[id] != LUA_NOREF) { + int callback = stopped_callback[id]; + lua_rawgeti(L, LUA_REGISTRYINDEX, callback); + callback_free(L, id); + + lua_call(L, 0, 0); + } +} + +int platform_switec_exists( unsigned int id ) +{ + return (id < SWITEC_CHANNEL_COUNT); +} + +// Lua: setup(id, P1, P2, P3, P4, maxSpeed) +static int lswitec_setup( lua_State* L ) +{ + unsigned int id; + + id = luaL_checkinteger( L, 1 ); + MOD_CHECK_ID( switec, id ); + int pin[4]; + + if (switec_close(id)) { + return luaL_error( L, "Unable to setup stepper." ); + } + + int i; + for (i = 0; i < 4; i++) { + uint32_t gpio = luaL_checkinteger(L, 2 + i); + + luaL_argcheck(L, platform_gpio_exists(gpio), 2 + i, "Invalid pin"); + + pin[i] = pin_num[gpio]; + + platform_gpio_mode(gpio, PLATFORM_GPIO_OUTPUT, PLATFORM_GPIO_PULLUP); + } + + int deg_per_sec = 0; + if (lua_gettop(L) >= 6) { + deg_per_sec = luaL_checkinteger(L, 6); + } + + if (switec_setup(id, pin, deg_per_sec, tasknumber)) { + return luaL_error(L, "Unable to setup stepper."); + } + return 0; +} + +// Lua: close( id ) +static int lswitec_close( lua_State* L ) +{ + unsigned int id; + + id = luaL_checkinteger( L, 1 ); + MOD_CHECK_ID( switec, id ); + callback_free(L, id); + if (switec_close( id )) { + return luaL_error( L, "Unable to close stepper." ); + } + return 0; +} + +// Lua: reset( id ) +static int lswitec_reset( lua_State* L ) +{ + unsigned int id; + id = luaL_checkinteger( L, 1 ); + MOD_CHECK_ID( switec, id ); + if (switec_reset( id )) { + return luaL_error( L, "Unable to reset stepper." ); + } + return 0; +} + +// Lua: moveto( id, pos [, cb] ) +static int lswitec_moveto( lua_State* L ) +{ + unsigned int id; + + id = luaL_checkinteger( L, 1 ); + MOD_CHECK_ID( switec, id ); + int pos; + pos = luaL_checkinteger( L, 2 ); + + if (lua_gettop(L) >= 3) { + callback_set(L, id, 3); + } else { + callback_free(L, id); + } + + if (switec_moveto( id, pos )) { + return luaL_error( L, "Unable to move stepper." ); + } + return 0; +} + +// Lua: getpos( id ) -> position, moving +static int lswitec_getpos( lua_State* L ) +{ + unsigned int id; + + id = luaL_checkinteger( L, 1 ); + MOD_CHECK_ID( switec, id ); + int32_t pos; + int32_t dir; + int32_t target; + if (switec_getpos( id, &pos, &dir, &target )) { + return luaL_error( L, "Unable to get position." ); + } + lua_pushnumber(L, pos); + lua_pushnumber(L, dir); + return 2; +} + +static int lswitec_dequeue(lua_State* L) +{ + int id; + + for (id = 0; id < SWITEC_CHANNEL_COUNT; id++) { + if (stopped_callback[id] != LUA_NOREF) { + int32_t pos; + int32_t dir; + int32_t target; + if (!switec_getpos( id, &pos, &dir, &target )) { + if (dir == 0 && pos == target) { + callback_execute(L, id); + } + } + } + } + + return 0; +} + +static void lswitec_task(os_param_t param, uint8_t prio) +{ + (void) param; + (void) prio; + + lswitec_dequeue(lua_getstate()); +} + +static int switec_open(lua_State *L) +{ + (void) L; + + tasknumber = task_get_id(lswitec_task); + + return 0; +} + + +// Module function map +static const LUA_REG_TYPE switec_map[] = { + { LSTRKEY( "setup" ), LFUNCVAL( lswitec_setup ) }, + { LSTRKEY( "close" ), LFUNCVAL( lswitec_close ) }, + { LSTRKEY( "reset" ), LFUNCVAL( lswitec_reset ) }, + { LSTRKEY( "moveto" ), LFUNCVAL( lswitec_moveto) }, + { LSTRKEY( "getpos" ), LFUNCVAL( lswitec_getpos) }, +#ifdef SQITEC_DEBUG + { LSTRKEY( "dequeue" ), LFUNCVAL( lswitec_dequeue) }, +#endif + + { LNILKEY, LNILVAL } +}; + +NODEMCU_MODULE(SWITEC, "switec", switec_map, switec_open); diff --git a/docs/en/modules/switec.md b/docs/en/modules/switec.md new file mode 100644 index 00000000..f30e8c59 --- /dev/null +++ b/docs/en/modules/switec.md @@ -0,0 +1,153 @@ +# switec Module + +This module controls a Switec X.27 (or compatible) instrument stepper motor. These are the +stepper motors that are used in modern automotive instrument clusters. They are incredibly cheap +and can be found at your favorite auction site or Chinese shopping site. There are varieties +which are dual axis -- i.e. have two stepper motors driving two concentric shafts so you +can mount two needles from the same axis. + +These motors run off 5V (some may work off 3.3V). They draw under 20mA and are designed to be +driven directly from MCU pins. Since the nodemcu runs at 3.3V, a level translator is required. +An octal translator like the 74LVC4245A can perfom this translation. It also includes all the +protection diodes required. + +These motors can be driven off three pins, with `pin2` and `pin3` being the same GPIO pin. +If the motor is directly connected to the MCU, then the current load is doubled and may exceed +the maximum ratings. If, however, a driver chip is being used, then the load on the MCU is neglible +and the same MCU pin can be connected to two driver pins. In order to do this, just specify +the same pin for `pin2` and `pin3`. + +These motors do not have absolute positioning, but come with stops at both ends of the range. +The startup procedure is to drive the motor anti-clockwise until it is guaranteed that the needle +is on the step. Then this point can be set as zero. It is important not to let the motor +run into the endstops during normal operation as this will make the pointing inaccurate. +This module does not enforce any range limiting. + +### Sources + +These stepper motors are available at the following locations: + +- Amazon: These are [X27 Stepper motors](http://www.amazon.com/s/ref=nb_sb_noss?url=search-alias%3Daps&field-keywords=instrument+stepper+motor+x27&rh=i%3Aaps%2Ck%3Ainstrument+stepper+motor+x27) +- Aliexpress: These are [X27 Stepper motors](http://www.aliexpress.com/wholesale?catId=0&initiative_id=SB_20160221132322&SearchText=x27+stepper). They also have the [Dual shaft version](http://www.aliexpress.com/wholesale?catId=0&initiative_id=SB_20160221132428&SearchText=vid28-05) +- Ebay: These are [X27 Stepper motors](http://www.ebay.com/sch/i.html?_from=R40&_trksid=p2050601.m570.l1313.TR0.TRC0.H0.Xx27+stepper+motor.TRS0&_nkw=x27+stepper+motor&_sacat=0) + +##### Note + +This module uses the hardware timer interrupt and hence it cannot be used at the same time as the PWM module. +Both modules can be compiled into the same firmware image, but an application can only use one. It may be +possible for an application to alternate between `switec` and `pwm`, but care must be taken. + +## switec.setup() +Initialize the nodemcu to talk to a switec X.27 or compatible instrument stepper motor. The default +slew rate is set so that it should work for most motors. Some motors can run at 600 degress per second. + +#### Syntax +`switec.setup(channel, pin1, pin2, pin3, pin4 [, maxDegPerSec])` + +#### Parameters +- `channel` The switec module supports three stepper motors. The channel is either 0, 1 or 2. +- `pin1` This is a GPIO number and connects to pin 1 on the stepper. +- `pin2` This is a GPIO number and connects to pin 2 on the stepper. +- `pin3` This is a GPIO number and connects to pin 3 on the stepper. +- `pin4` This is a GPIO number and connects to pin 4 on the stepper. +- `maxDegPerSec` (optional) This can set to limit the maximum slew rate. The default is 400 degrees per second. + +#### Returns +Nothing. If the arguments are in error, or the operation cannot be completed, then an error is thrown. + +##### Note + +Once a channel is setup, it cannot be re-setup until the needle has stopped moving. + +#### Example + + switec.setup(0, 5,6,7,8) + +## switec.moveto() +Starts the needle moving to the specified position. If the needle is already moving, then the current +motion is cancelled, and the needle will move to the new position. It is possible to get a callback +when the needle stops moving. This is not normally required as multiple `moveto` operations can +be issued in quick succession. During the initial calibration, it is important. Note that the +callback is not guaranteed to be called -- it is possible that the needle never stops at the +target location before another `moveto` operation is triggered. + +#### Syntax +`switec.moveto(channel, position[, stoppedCallback)` + +#### Parameters +- `channel` The switec module supports three stepper motors. The channel is either 0, 1 or 2. +- `position` The position (number of steps clockwise) to move the needle. Typically in the range 0 to around 1000. +- `stoppedCallback` (optional) callback to be invoked when the needle stops moving. + +#### Errors +The channel must have been setup, otherwise an error is thrown. + +#### Example + + switec.moveto(0, 1000, function () + switec.moveto(0, 0) + end) + +## switec.reset() +This sets the current position of the needle as being zero. The needle must be stationary. + +#### Syntax +`switec.reset(channel)` + +#### Parameters +- `channel` The switec module supports three stepper motors. The channel is either 0, 1 or 2. + +#### Errors +The channel must have been setup and the needle must not be moving, otherwise an error is thrown. + +#### Example + + switec.reset(0) + +## switec.getpos() +Gets the current position of the needle and whether it is moving. + +#### Syntax +`switec.getpos(channel)` + +#### Parameters +- `channel` The switec module supports three stepper motors. The channel is either 0, 1 or 2. + +#### Returns +- `position` the current position of the needle +- `moving` 0 if the needle is stationary. 1 for clockwise, -1 for anti-clockwise. + +#### Example + + print switec.getpos(0) + +## switec.close() +Releases the resources associated with the stepper. + +#### Syntax +`switec.close(channel)` + +#### Parameters +- `channel` The switec module supports three stepper motors. The channel is either 0, 1 or 2. + +#### Errors +The needle must not be moving, otherwise an error is thrown. + +#### Example + + switec.close(0) + +## Calibration +In order to set the zero point correctly, the needle should be driven anti-clockwise until +it runs into the end stop. Then the zero point can be set. The value of -1000 is used as that is +larger than the range of the motor -- i.e. it drives anti-clockwise through the entire range and +onto the end stop. + + switec.setup(0, 5,6,7,8) + calibration = true + switec.moveto(0, -1000, function() + switec.reset(0) + calibration = false + end) + +Other `moveto` operations should not be performed while `calibration` is set. diff --git a/mkdocs.yml b/mkdocs.yml index 716f7d39..7c05db11 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -71,6 +71,7 @@ pages: - 'sntp': 'en/modules/sntp.md' - 'spi': 'en/modules/spi.md' - 'struct': 'en/modules/struct.md' + - 'switec': 'en/modules/switec.md' - 'tm1829': 'en/modules/tm1829.md' - 'tmr': 'en/modules/tmr.md' - 'tsl2561': 'en/modules/tsl2561.md'