switec.c 8.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391
  1. /*
  2. * Module for interfacing with Switec instrument steppers (and
  3. * similar devices). These are the steppers that are used in automotive
  4. * instrument panels and the like. Run off 5 volts at low current.
  5. *
  6. * Code inspired by:
  7. *
  8. * SwitecX25 Arduino Library
  9. * Guy Carpenter, Clearwater Software - 2012
  10. *
  11. * Licensed under the BSD2 license, see license.txt for details.
  12. *
  13. * NodeMcu integration by Philip Gladstone, N1DQ
  14. */
  15. #include "platform.h"
  16. #include "c_types.h"
  17. #include "../libc/c_stdlib.h"
  18. #include "../libc/c_stdio.h"
  19. #include "driver/switec.h"
  20. #include "ets_sys.h"
  21. #include "os_type.h"
  22. #include "osapi.h"
  23. #include "hw_timer.h"
  24. #include "user_interface.h"
  25. #include "task/task.h"
  26. #define N_STATES 6
  27. //
  28. // First pin passed to setup corresponds to bit 3
  29. // On the motor, the pins are arranged
  30. //
  31. // 4 1
  32. //
  33. // 3 2
  34. //
  35. // The direction of rotation can be reversed by reordering the pins
  36. //
  37. // State 3 2 1 0 A B Value
  38. // 0 1 0 0 1 - - 0x9
  39. // 1 0 0 0 1 . - 0x1
  40. // 2 0 1 1 1 + . 0x7
  41. // 3 0 1 1 0 + + 0x6
  42. // 4 1 1 1 0 . + 0xE
  43. // 5 1 0 0 0 - . 0x8
  44. static const uint8_t stateMap[N_STATES] = {0x9, 0x1, 0x7, 0x6, 0xE, 0x8};
  45. typedef struct {
  46. uint8_t current_state;
  47. uint8_t stopped;
  48. int8_t dir;
  49. uint32_t mask;
  50. uint32_t pinstate[N_STATES];
  51. uint32_t next_time;
  52. int16_t target_step;
  53. int16_t current_step;
  54. uint16_t vel;
  55. uint16_t max_vel;
  56. uint16_t min_delay;
  57. task_handle_t task_number;
  58. } DATA;
  59. static DATA *data[SWITEC_CHANNEL_COUNT];
  60. static volatile char timer_active;
  61. #define MAXVEL 255
  62. // Note that this has to be global so that the compiler does not
  63. // put it into ROM.
  64. uint8_t switec_accel_table[][2] = {
  65. { 20, 3000 >> 4},
  66. { 50, 1500 >> 4},
  67. { 100, 1000 >> 4},
  68. { 150, 800 >> 4},
  69. { MAXVEL, 600 >> 4}
  70. };
  71. static void ICACHE_RAM_ATTR timer_interrupt(os_param_t);
  72. #define TIMER_OWNER ((os_param_t) 'S')
  73. // Just takes the channel number
  74. int switec_close(uint32_t channel)
  75. {
  76. if (channel >= sizeof(data) / sizeof(data[0])) {
  77. return -1;
  78. }
  79. DATA *d = data[channel];
  80. if (!d) {
  81. return 0;
  82. }
  83. if (!d->stopped) {
  84. return -1;
  85. }
  86. // Set pins as input
  87. gpio_output_set(0, 0, 0, d->mask);
  88. data[channel] = NULL;
  89. c_free(d);
  90. // See if there are any other channels active
  91. for (channel = 0; channel < sizeof(data)/sizeof(data[0]); channel++) {
  92. if (data[channel]) {
  93. break;
  94. }
  95. }
  96. // If not, then disable the interrupt
  97. if (channel >= sizeof(data) / sizeof(data[0])) {
  98. platform_hw_timer_close(TIMER_OWNER);
  99. }
  100. return 0;
  101. }
  102. static __attribute__((always_inline)) inline void write_io(DATA *d)
  103. {
  104. uint32_t pin_state = d->pinstate[d->current_state];
  105. gpio_output_set(pin_state, d->mask & ~pin_state, 0, 0);
  106. }
  107. static __attribute__((always_inline)) inline void step_up(DATA *d)
  108. {
  109. d->current_step++;
  110. d->current_state = (d->current_state + 1) % N_STATES;
  111. write_io(d);
  112. }
  113. static __attribute__((always_inline)) inline void step_down(DATA *d)
  114. {
  115. d->current_step--;
  116. d->current_state = (d->current_state + N_STATES - 1) % N_STATES;
  117. write_io(d);
  118. }
  119. static void ICACHE_RAM_ATTR timer_interrupt(os_param_t p)
  120. {
  121. // This function really is running at interrupt level with everything
  122. // else masked off. It should take as little time as necessary.
  123. //
  124. (void) p;
  125. int i;
  126. uint32_t delay = 0xffffffff;
  127. // Loop over the channels to figure out which one needs action
  128. for (i = 0; i < sizeof(data) / sizeof(data[0]); i++) {
  129. DATA *d = data[i];
  130. if (!d || d->stopped) {
  131. continue;
  132. }
  133. uint32_t now = system_get_time();
  134. if (now < d->next_time) {
  135. int need_to_wait = d->next_time - now;
  136. if (need_to_wait < delay) {
  137. delay = need_to_wait;
  138. }
  139. continue;
  140. }
  141. // This channel is past it's action time. Need to process it
  142. // Are we done yet?
  143. if (d->current_step == d->target_step && d->vel == 0) {
  144. d->stopped = 1;
  145. d->dir = 0;
  146. task_post_low(d->task_number, 0);
  147. continue;
  148. }
  149. // if stopped, determine direction
  150. if (d->vel == 0) {
  151. d->dir = d->current_step < d->target_step ? 1 : -1;
  152. // do not set to 0 or it could go negative in case 2 below
  153. d->vel = 1;
  154. }
  155. // Move the pointer by one step in the correct direction
  156. if (d->dir > 0) {
  157. step_up(d);
  158. } else {
  159. step_down(d);
  160. }
  161. // determine delta, number of steps in current direction to target.
  162. // may be negative if we are headed away from target
  163. int delta = d->dir > 0 ? d->target_step - d->current_step : d->current_step - d->target_step;
  164. if (delta > 0) {
  165. // case 1 : moving towards target (maybe under accel or decel)
  166. if (delta <= d->vel) {
  167. // time to declerate
  168. d->vel--;
  169. } else if (d->vel < d->max_vel) {
  170. // accelerating
  171. d->vel++;
  172. } else {
  173. // at full speed - stay there
  174. }
  175. } else {
  176. // case 2 : at or moving away from target (slow down!)
  177. d->vel--;
  178. }
  179. // vel now defines delay
  180. uint8_t row = 0;
  181. // this is why vel must not be greater than the last vel in the table.
  182. while (switec_accel_table[row][0] < d->vel) {
  183. row++;
  184. }
  185. uint32_t micro_delay = switec_accel_table[row][1] << 4;
  186. if (micro_delay < d->min_delay) {
  187. micro_delay = d->min_delay;
  188. }
  189. // Figure out when we next need to take action
  190. d->next_time = d->next_time + micro_delay;
  191. if (d->next_time < now) {
  192. d->next_time = now + micro_delay;
  193. }
  194. // Figure out how long to wait
  195. int need_to_wait = d->next_time - now;
  196. if (need_to_wait < delay) {
  197. delay = need_to_wait;
  198. }
  199. }
  200. if (delay < 1000000) {
  201. if (delay < 50) {
  202. delay = 50;
  203. }
  204. timer_active = 1;
  205. platform_hw_timer_arm_us(TIMER_OWNER, delay);
  206. } else {
  207. timer_active = 0;
  208. }
  209. }
  210. // The pin numbers are actual platform GPIO numbers
  211. int switec_setup(uint32_t channel, int *pin, int max_deg_per_sec, task_handle_t task_number )
  212. {
  213. if (channel >= sizeof(data) / sizeof(data[0])) {
  214. return -1;
  215. }
  216. if (data[channel]) {
  217. if (switec_close(channel)) {
  218. return -1;
  219. }
  220. }
  221. DATA *d = (DATA *) c_zalloc(sizeof(DATA));
  222. if (!d) {
  223. return -1;
  224. }
  225. if (!data[0] && !data[1] && !data[2]) {
  226. // We need to stup the timer as no channel was active before
  227. // no autoreload
  228. if (!platform_hw_timer_init(TIMER_OWNER, FRC1_SOURCE, FALSE)) {
  229. // Failed to get the timer
  230. c_free(d);
  231. return -1;
  232. }
  233. }
  234. data[channel] = d;
  235. int i;
  236. for (i = 0; i < 4; i++) {
  237. // Build the mask for the pins to be output pins
  238. d->mask |= 1 << pin[i];
  239. int j;
  240. // Now build the hi states for the pins according to the 6 phases above
  241. for (j = 0; j < N_STATES; j++) {
  242. if (stateMap[j] & (1 << (3 - i))) {
  243. d->pinstate[j] |= 1 << pin[i];
  244. }
  245. }
  246. }
  247. d->max_vel = MAXVEL;
  248. if (max_deg_per_sec == 0) {
  249. max_deg_per_sec = 400;
  250. }
  251. d->min_delay = 1000000 / (3 * max_deg_per_sec);
  252. d->task_number = task_number;
  253. #ifdef SWITEC_DEBUG
  254. for (i = 0; i < 4; i++) {
  255. c_printf("pin[%d]=%d\n", i, pin[i]);
  256. }
  257. c_printf("Mask=0x%x\n", d->mask);
  258. for (i = 0; i < N_STATES; i++) {
  259. c_printf("pinstate[%d]=0x%x\n", i, d->pinstate[i]);
  260. }
  261. #endif
  262. // Set all pins as outputs
  263. gpio_output_set(0, 0, d->mask, 0);
  264. platform_hw_timer_set_func(TIMER_OWNER, timer_interrupt, 0);
  265. return 0;
  266. }
  267. // All this does is to assert that the current position is 0
  268. int switec_reset(uint32_t channel)
  269. {
  270. if (channel >= sizeof(data) / sizeof(data[0])) {
  271. return -1;
  272. }
  273. DATA *d = data[channel];
  274. if (!d || !d->stopped) {
  275. return -1;
  276. }
  277. d->current_step = d->target_step = 0;
  278. return 0;
  279. }
  280. // Just takes the channel number and the position
  281. int switec_moveto(uint32_t channel, int pos)
  282. {
  283. if (channel >= sizeof(data) / sizeof(data[0])) {
  284. return -1;
  285. }
  286. DATA *d = data[channel];
  287. if (!d) {
  288. return -1;
  289. }
  290. if (pos < 0) {
  291. // This ensures that we don't slam into the endstop
  292. d->max_vel = 50;
  293. } else {
  294. d->max_vel = MAXVEL;
  295. }
  296. d->target_step = pos;
  297. // If the pointer is not moving, setup so that we start it
  298. if (d->stopped) {
  299. // reset the timer to avoid possible time overflow giving spurious deltas
  300. d->next_time = system_get_time() + 1000;
  301. d->stopped = false;
  302. if (!timer_active) {
  303. timer_interrupt(0);
  304. }
  305. }
  306. return 0;
  307. }
  308. // Get the current position, direction and target position
  309. int switec_getpos(uint32_t channel, int32_t *pos, int32_t *dir, int32_t *target)
  310. {
  311. if (channel >= sizeof(data) / sizeof(data[0])) {
  312. return -1;
  313. }
  314. DATA *d = data[channel];
  315. if (!d) {
  316. return -1;
  317. }
  318. *pos = d->current_step;
  319. *dir = d->stopped ? 0 : d->dir;
  320. *target = d->target_step;
  321. return 0;
  322. }