123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392 |
- /*
- * 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 <stdint.h>
- #include <stddef.h>
- #include <stdlib.h>
- #include <stdio.h>
- #include "task/task.h"
- #include "driver/switec.h"
- #include "ets_sys.h"
- #include "os_type.h"
- #include "osapi.h"
- #include "hw_timer.h"
- #include "user_interface.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;
- 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 *) calloc(1, 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
- 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++) {
- printf("pin[%d]=%d\n", i, pin[i]);
- }
- printf("Mask=0x%x\n", d->mask);
- for (i = 0; i < N_STATES; i++) {
- 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;
- }
|