switec.c 8.5 KB

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