Add support for driving instrument gauge stepper motors (nodemcu#1355)

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;
+}