/* * 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; }