API tools faq
paste
ripred

teensy_tmc2209_test2.ino

ripred
Oct 21st, 2025
1,164
0
Never
1
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 36.17 KB | Source Code | 0 0
raw download clone embed print report
  1. /**
  2.  * teensy_tmc2209_test2.ino
  3.  *
  4.  * Teensy 4.1 + ST7735_t3 + TMC2209 + Adafruit seesaw (quad encoder)
  5.  * - Double-buffered framebuffer (DMA) for flicker-free updates
  6.  * - Per-microstep on-screen position updates during continuous sweep
  7.  */
  8.  
  9. #include <TMC2209.h>
  10. #include <string.h>
  11. #include <stdlib.h>
  12. #include <SPI.h>
  13. #include <Adafruit_GFX.h>
  14. #include <ST7735_t3.h>
  15. #include "Adafruit_seesaw.h"
  16. #include <seesaw_neopixel.h>
  17. #include <climits>
  18.  
  19. #define   TFT_CS          10
  20. #define   TFT_DC           9
  21. #define   TFT_RST          8
  22. #define   TFT_BACKLIGHT    6
  23.  
  24. // Display resolution (after rotation 3 treat width=160, height=128)
  25. static constexpr int16_t TFT_W = 160;
  26. static constexpr int16_t TFT_H = 128;
  27. static constexpr size_t FB_PIXELS = (size_t)TFT_W * (size_t)TFT_H;
  28. static constexpr size_t FB_BYTES  = FB_PIXELS * sizeof(uint16_t);
  29.  
  30. ST7735_t3 tft = ST7735_t3(TFT_CS, TFT_DC, TFT_RST);
  31.  
  32. #define   SEESAW_ADDR   0x49
  33. #define   SS_NEO_PIN      18
  34.  
  35. uint8_t switch_pins[4] = {12, 14, 17, 9};
  36.  
  37. Adafruit_seesaw ss = Adafruit_seesaw(&Wire);
  38. seesaw_NeoPixel pixels = seesaw_NeoPixel(4, SS_NEO_PIN, NEO_GRB + NEO_KHZ800);
  39.  
  40. int32_t enc_positions[4] = {0, 0, 0, 0};
  41.  
  42. // Maximum number of motors supported (limited by Serial1-8)
  43. static constexpr int MAX_MOTORS = 7;
  44.  
  45. // Teensy 4.1 Serial1-8 TX/RX pin mappings (standard defaults)
  46. const int8_t tx_pins[9] = {-1, 1, 8, 15, 17, 21, 25, 29, 35};  // Index 1-8
  47. const int8_t rx_pins[9] = {-1, 0, 7, 14, 16, 20, 24, 28, 34};
  48.  
  49. // Default pins for STEP, DIR, and UART per motor (0-6 for motors 1-7)
  50. const int8_t default_step_pins[7] = {31, 36, 12, 18, 22, 26, 30};
  51. const int8_t default_dir_pins[7]   = {32, 37, 13, 19, 23, 27, 31};
  52. const int8_t default_uart_numbers[7] = {7, 8, 3, 4, 5, 6, 1};
  53.  
  54. // Global input buffer
  55. const int INPUT_BUFFER_SIZE = 256;
  56. char user_input[INPUT_BUFFER_SIZE];
  57.  
  58. /*───────────────────────────────────────────────────────────────────────────
  59.   DMA framebuffer glue (double-buffered, non-blocking pushes)
  60. ───────────────────────────────────────────────────────────────────────────*/
  61.  
  62. // 2 full-size RGB565 framebuffers in non-cacheable RAM for DMA
  63. DMAMEM static uint16_t fb_a[FB_PIXELS];
  64. DMAMEM static uint16_t fb_b[FB_PIXELS];
  65.  
  66. static bool tft_fb_ok = false;
  67. static uint16_t *draw_fb = nullptr;   // buffer we currently render into
  68.  
  69. inline void tft_fb_prepare_draw() {
  70.     if (tft_fb_ok) {
  71.         tft.setFrameBuffer(draw_fb);
  72.     }
  73. }
  74.  
  75. inline void tft_fb_wait_idle() {
  76.     if (tft_fb_ok) {
  77.         tft.waitUpdateAsyncComplete();
  78.     }
  79. }
  80.  
  81. // Start a DMA push if bus is idle; swap draw buffer and seed it with the last frame
  82. inline void tft_fb_push_if_idle() {
  83.     if (!tft_fb_ok) {
  84.         return;
  85.     }
  86.     if (!tft.asyncUpdateActive()) {
  87.         // Send the frame we just finished drawing
  88.         uint16_t *sending = draw_fb;
  89.  
  90.         // Switch draw_fb to the other buffer first
  91.         draw_fb = (draw_fb == fb_a) ? fb_b : fb_a;
  92.  
  93.         // Start non-blocking DMA transfer from 'sending'
  94.         tft.setFrameBuffer(sending);
  95.         tft.updateScreenAsync(false);
  96.  
  97.         // Point drawing back at draw_fb and COPY the previous frame into it
  98.         // so all static UI/artwork persists across buffer flips.
  99.         tft.setFrameBuffer(draw_fb);
  100.         memcpy(draw_fb, sending, FB_BYTES);
  101.     }
  102. }
  103.  
  104. // For legacy call sites that expect a push, allow optional blocking push.
  105. inline void tft_fb_push(bool block = false) {
  106.     tft_fb_push_if_idle();
  107.     if (block && tft_fb_ok) {
  108.         tft.waitUpdateAsyncComplete();
  109.     }
  110. }
  111.  
  112. /*───────────────────────────────────────────────────────────────────────────
  113.   GFX helpers: controlled line spacing + minimal DRY wrappers
  114. ───────────────────────────────────────────────────────────────────────────*/
  115. constexpr int16_t EXTRA_LINE_SPACING_PX = 2;   // adjust spacing here
  116.  
  117. static uint8_t gfx_text_size_x = 1;
  118. static uint8_t gfx_text_size_y = 1;
  119. static int16_t gfx_home_x = 0;
  120. static int16_t gfx_home_y = 0;
  121.  
  122. inline int16_t gfx_text_height() {
  123.     return 8 * gfx_text_size_y;  // built-in 5x7 font uses 8px line step
  124. }
  125.  
  126. inline int16_t gfx_line_height() {
  127.     return gfx_text_height() + EXTRA_LINE_SPACING_PX;
  128. }
  129.  
  130. inline void gfx_set_text_size(uint8_t s) {
  131.     if (s < 1) s = 1;
  132.     gfx_text_size_x = s;
  133.     gfx_text_size_y = s;
  134.     tft.setTextSize(s);
  135. }
  136.  
  137. inline void gfx_set_cursor(int16_t x, int16_t y) {
  138.     gfx_home_x = x;
  139.     gfx_home_y = y;
  140.     tft.setCursor(x, y);
  141. }
  142.  
  143. inline void gfx_newline() {
  144.     tft.setCursor(gfx_home_x, tft.getCursorY() + gfx_line_height());
  145. }
  146.  
  147. inline void gfx_print(const String &s) { tft.print(s); }
  148. inline void gfx_print(const char *s)   { tft.print(s); }
  149. inline void gfx_print(long v)          { tft.print(v); }
  150.  
  151. inline void gfx_println(const String &s) { tft.print(s); gfx_newline(); }
  152. inline void gfx_println(const char *s)   { tft.print(s); gfx_newline(); }
  153. inline void gfx_println(long v)          { tft.print(v); gfx_newline(); }
  154.  
  155. /*───────────────────────────────────────────────────────────────────────────
  156.   UI: shared menu text
  157. ───────────────────────────────────────────────────────────────────────────*/
  158. const char *MENU_TITLE = "Menu:";
  159. const char *MENU_ITEMS[] = {
  160.     "1. Config Stepping",
  161.     "2. Continuous Sweep",
  162.     "3. Config Modes",
  163.     "4. Set Velocity",
  164.     "5. Test Encoders"
  165. };
  166. constexpr int MENU_ITEM_COUNT = 5;
  167.  
  168. /*───────────────────────────────────────────────────────────────────────────
  169.   Stepper structure
  170. ───────────────────────────────────────────────────────────────────────────*/
  171. struct StepperMotor {
  172.     TMC2209 tmc;
  173.     uint8_t step_pin;
  174.     uint8_t dir_pin;
  175.     uint8_t uart_number;              // 1 to 8 for Serial1-8
  176.     long full_steps_per_rev = 200;    // Default full steps per revolution
  177.     uint16_t microsteps = 1;          // Default to full stepping
  178.     long speed_steps_per_sec = 200;   // Default steps per second
  179.     long last_start_pos = 0;          // Default start position
  180.     long last_stop_pos = 200;         // Default to one revolution (full_steps * microsteps)
  181.     long current_position = 0;        // Track position in microsteps
  182.     long current_velocity = 0;        // For velocity mode
  183.     bool velocity_mode = false;       // false: position (STEP/DIR), true: velocity (VActual)
  184.     bool constant_current = true;     // true: fixed current, false: automatic (CoolStep)
  185.     bool initialized = false;
  186. };
  187.  
  188. StepperMotor motors[MAX_MOTORS];
  189. int motor_count = 2;
  190.  
  191. /*───────────────────────────────────────────────────────────────────────────
  192.   Prototypes
  193. ───────────────────────────────────────────────────────────────────────────*/
  194. void update_display(bool full_update = false);
  195. long get_numeric_input(String prompt, long default_val, long min_val = LONG_MIN, long max_val = LONG_MAX);
  196. long ask_value(const String &prompt, long def, long min_val = LONG_MIN, long max_val = LONG_MAX);
  197. HardwareSerial* get_serial(uint8_t uart_number);
  198. void initialize_motors();
  199. void move_to_position(int motor_index, long target_position, long microsteps_per_sec);
  200. void configure_stepping();
  201. void configure_modes();
  202. void set_velocity();
  203. void continuous_sweep();
  204. void test_encoders();
  205. void print_menu();
  206. uint32_t wheel(byte wheel_pos);
  207. inline void update_display_microstep(int motor_index);
  208.  
  209. /*───────────────────────────────────────────────────────────────────────────
  210.   DRY helper: ask numeric value (Serial echo included)
  211. ───────────────────────────────────────────────────────────────────────────*/
  212. long ask_value(const String &prompt, long def, long min_val, long max_val) {
  213.     long v = get_numeric_input(prompt, def, min_val, max_val);
  214.     Serial.println(String(v));
  215.     return v;
  216. }
  217.  
  218. /*───────────────────────────────────────────────────────────────────────────
  219.   Serial port selector
  220. ───────────────────────────────────────────────────────────────────────────*/
  221. HardwareSerial* get_serial(uint8_t uart_number) {
  222.     switch (uart_number) {
  223.         case 1: return &Serial1;
  224.         case 2: return &Serial2;
  225.         case 3: return &Serial3;
  226.         case 4: return &Serial4;
  227.         case 5: return &Serial5;
  228.         case 6: return &Serial6;
  229.         case 7: return &Serial7;
  230.         case 8: return &Serial8;
  231.         default: return nullptr;
  232.     }
  233. }
  234.  
  235. /*───────────────────────────────────────────────────────────────────────────
  236.   Motor init
  237. ───────────────────────────────────────────────────────────────────────────*/
  238. void initialize_motors() {
  239.     for (int i = 0; i < motor_count; i++) {
  240.         HardwareSerial* ser = get_serial(motors[i].uart_number);
  241.         if (ser == nullptr) {
  242.             Serial.println("Invalid UART number for motor " + String(i + 1) + ". Skipping.");
  243.             continue;
  244.         }
  245.         motors[i].tmc.setup(*ser, 115200, TMC2209::SERIAL_ADDRESS_0);
  246.         pinMode(motors[i].step_pin, OUTPUT);
  247.         pinMode(motors[i].dir_pin, OUTPUT);
  248.         digitalWrite(motors[i].step_pin, LOW);
  249.         digitalWrite(motors[i].dir_pin, LOW);
  250.  
  251.         // Driver configuration
  252.         motors[i].tmc.setMicrostepsPerStep(motors[i].microsteps);
  253.         motors[i].tmc.setRunCurrent(83);
  254.         motors[i].tmc.setHoldCurrent(25);
  255.         motors[i].tmc.setHoldDelay(50);
  256.         motors[i].tmc.enableStealthChop();
  257.         if (motors[i].constant_current) {
  258.             motors[i].tmc.disableAutomaticCurrentScaling();
  259.         } else {
  260.             motors[i].tmc.enableCoolStep();
  261.         }
  262.         if (motors[i].velocity_mode) {
  263.             motors[i].tmc.moveAtVelocity(motors[i].current_velocity);
  264.         } else {
  265.             motors[i].tmc.moveUsingStepDirInterface();
  266.         }
  267.         motors[i].tmc.disable();  // Start disabled
  268.  
  269.         motors[i].initialized = true;
  270.         Serial.println("Motor " + String(i + 1) + " initialized.");
  271.  
  272.         // Output the 4 pins for this motor
  273.         Serial.println("Motor " + String(i + 1) + " pins:");
  274.         Serial.print("STEP: "); Serial.println(motors[i].step_pin);
  275.         Serial.print("DIR: ");  Serial.println(motors[i].dir_pin);
  276.         Serial.print("TX: ");   Serial.println(tx_pins[motors[i].uart_number]);
  277.         Serial.print("RX: ");   Serial.println(rx_pins[motors[i].uart_number]);
  278.     }
  279. }
  280.  
  281. /*───────────────────────────────────────────────────────────────────────────
  282.   Movement (per-microstep HUD updates, non-blocking DMA pushes)
  283. ───────────────────────────────────────────────────────────────────────────*/
  284. void move_to_position(int motor_index, long target_position, long microsteps_per_sec) {
  285.     if (!motors[motor_index].initialized) {
  286.         Serial.println("Motor not initialized.");
  287.         return;
  288.     }
  289.  
  290.     motors[motor_index].tmc.enable();
  291.  
  292.     long current_pos = motors[motor_index].current_position;
  293.     long distance = labs(target_position - current_pos);
  294.     if (distance == 0) {
  295.         return;
  296.     }
  297.  
  298.     bool dir_high = (target_position > current_pos);
  299.     const long step_delta = dir_high ? 1 : -1;
  300.  
  301.     digitalWrite(motors[motor_index].dir_pin, dir_high ? HIGH : LOW);
  302.  
  303.     unsigned long delay_us = 1000000UL / (microsteps_per_sec ? microsteps_per_sec : 1);
  304.     if (delay_us < 2) delay_us = 2;
  305.     unsigned long half_delay = delay_us / 2;
  306.     if (half_delay < 1) half_delay = 1;
  307.  
  308.     for (long i = 0; i < distance; i++) {
  309.         if (Serial.available()) {
  310.             Serial.read();
  311.             Serial.println("Movement interrupted.");
  312.             break;
  313.         }
  314.  
  315.         // One microstep pulse
  316.         digitalWrite(motors[motor_index].step_pin, HIGH);
  317.         delayMicroseconds(half_delay);
  318.         digitalWrite(motors[motor_index].step_pin, LOW);
  319.         delayMicroseconds(delay_us - half_delay);
  320.  
  321.         // Update model and HUD
  322.         current_pos += step_delta;
  323.         motors[motor_index].current_position = current_pos;
  324.         update_display_microstep(motor_index);   // draw into off-screen buffer + async push (if idle)
  325.     }
  326.  
  327.     motors[motor_index].tmc.disable();
  328.  
  329.     // Final touch-up (in case of early break)
  330.     update_display_microstep(motor_index);
  331. }
  332.  
  333. /*───────────────────────────────────────────────────────────────────────────
  334.   Numeric input via Serial or encoder 0 (with TFT prompt)
  335. ───────────────────────────────────────────────────────────────────────────*/
  336. long get_numeric_input(String prompt, long default_val, long min_val, long max_val) {
  337.     Serial.print(prompt);
  338.  
  339.     // Header
  340.     tft_fb_wait_idle();
  341.     tft_fb_prepare_draw();
  342.     tft.fillScreen(ST7735_BLACK);
  343.     tft.setTextColor(ST7735_WHITE, ST7735_BLACK);
  344.     gfx_set_text_size(1);
  345.     gfx_set_cursor(0, 0);
  346.     gfx_println(prompt);
  347.  
  348.     long current = default_val;
  349.  
  350.     // Value line
  351.     const int16_t value_y = gfx_line_height();
  352.     tft.setCursor(0, value_y);
  353.     tft.print("Value: ");
  354.     tft.print(current);
  355.     tft_fb_push(false);
  356.  
  357.     String serial_buf = "";
  358.     int32_t last_enc0 = ss.getEncoderPosition(0);
  359.     bool confirmed = false;
  360.  
  361.     while (!confirmed) {
  362.         if (Serial.available()) {
  363.             int c = Serial.read();
  364.             if (c == '\r' || c == '\n') {
  365.                 if (serial_buf.length() > 0) {
  366.                     current = atol(serial_buf.c_str());
  367.                 }
  368.                 confirmed = true;
  369.             } else {
  370.                 serial_buf += (char)c;
  371.             }
  372.         }
  373.  
  374.         int32_t new_enc0 = ss.getEncoderPosition(0);
  375.         long delta = new_enc0 - last_enc0;
  376.         if (delta != 0) {
  377.             // ++tmw - add acceleration
  378.             if (delta > 7) {
  379.                 delta = 100;
  380.             }
  381.             else if (delta > 2) {
  382.                 delta = 10;
  383.             }
  384.             new_enc0 = last_enc0 + delta;
  385.             current += delta;
  386.             if (min_val != LONG_MIN) current = max(current, min_val);
  387.             if (max_val != LONG_MAX) current = min(current, max_val);
  388.             last_enc0 = new_enc0;
  389.  
  390.             // Update value line (opaque text, single line)
  391.             tft_fb_prepare_draw();
  392.             tft.fillRect(0, value_y, tft.width(), gfx_text_height(), ST7735_BLACK);
  393.             tft.setCursor(0, value_y);
  394.             tft.print("Value: ");
  395.             tft.print(current);
  396.             tft_fb_push(false);
  397.         }
  398.  
  399.         if (!ss.digitalRead(switch_pins[0])) {
  400.             delay(50);
  401.             if (!ss.digitalRead(switch_pins[0])) {
  402.                 confirmed = true;
  403.                 while (!ss.digitalRead(switch_pins[0])) {
  404.                     /* wait for button release */
  405.                 }
  406.                 delay(10);
  407.             }
  408.         }
  409.         delay(5);
  410.     }
  411.  
  412.     return current;
  413. }
  414.  
  415. /*───────────────────────────────────────────────────────────────────────────
  416.   Configuration UIs
  417. ───────────────────────────────────────────────────────────────────────────*/
  418. void configure_stepping() {
  419.     long new_motor_count = ask_value(
  420.         "Enter number of attached stepper motors (1 to " + String(MAX_MOTORS) + "): ",
  421.         motor_count, 1, MAX_MOTORS
  422.     );
  423.     motor_count = new_motor_count;
  424.  
  425.     for (int i = 0; i < motor_count; i++) {
  426.         long step_pin = ask_value(
  427.             "For motor " + String(i + 1) + ", enter STEP pin (default " + String(default_step_pins[i]) + "): ",
  428.             default_step_pins[i], 0, 40
  429.         );
  430.         motors[i].step_pin = step_pin;
  431.  
  432.         long dir_pin = ask_value(
  433.             "For motor " + String(i + 1) + ", enter DIR pin (default " + String(default_dir_pins[i]) + "): ",
  434.             default_dir_pins[i], 0, 40
  435.         );
  436.         motors[i].dir_pin = dir_pin;
  437.  
  438.         long uart_number = ask_value(
  439.             "For motor " + String(i + 1) + ", enter UART number (default " + String(default_uart_numbers[i]) + "): ",
  440.             default_uart_numbers[i], 1, 8
  441.         );
  442.         motors[i].uart_number = uart_number;
  443.  
  444.         long full_steps_per_rev = ask_value(
  445.             "For motor " + String(i + 1) + ", enter full steps per revolution (default 200): ",
  446.             200
  447.         );
  448.         motors[i].full_steps_per_rev = full_steps_per_rev;
  449.  
  450.         long microsteps = ask_value(
  451.             "For motor " + String(i + 1) + ", enter microsteps (default 1): ",
  452.             1, 1, 256
  453.         );
  454.         motors[i].microsteps = microsteps;
  455.  
  456.         long speed_steps_per_sec = ask_value(
  457.             "For motor " + String(i + 1) + ", enter speed steps per second (default 200): ",
  458.             200
  459.         );
  460.         motors[i].speed_steps_per_sec = speed_steps_per_sec;
  461.  
  462.         if (motors[i].last_stop_pos == 200) {
  463.             motors[i].last_stop_pos = motors[i].full_steps_per_rev * motors[i].microsteps;
  464.         }
  465.  
  466.         motors[i].current_position = 0;
  467.         motors[i].initialized = false;
  468.     }
  469.  
  470.     initialize_motors();
  471. }
  472.  
  473. void configure_modes() {
  474.     for (int i = 0; i < motor_count; i++) {
  475.         long vel = ask_value(
  476.             "For motor " + String(i + 1) + ", velocity mode (1=yes,0=no): ",
  477.             motors[i].velocity_mode ? 1 : 0, 0, 1
  478.         );
  479.         motors[i].velocity_mode = (vel == 1);
  480.  
  481.         long constc = ask_value(
  482.             "For motor " + String(i + 1) + ", constant current mode (1=yes,0=no): ",
  483.             motors[i].constant_current ? 1 : 0, 0, 1
  484.         );
  485.         motors[i].constant_current = (constc == 1);
  486.     }
  487.     initialize_motors();
  488. }
  489.  
  490. void set_velocity() {
  491.     bool any_velocity = false;
  492.     for (int i = 0; i < motor_count; i++) {
  493.         if (motors[i].velocity_mode) {
  494.             any_velocity = true;
  495.             long vel = ask_value(
  496.                 "Enter velocity for motor " + String(i + 1) + " (microsteps/sec, signed): ",
  497.                 motors[i].current_velocity
  498.             );
  499.             motors[i].current_velocity = vel;
  500.             motors[i].tmc.moveAtVelocity(vel);
  501.         }
  502.     }
  503.     if (!any_velocity) {
  504.         Serial.println("No motors in velocity mode.");
  505.     }
  506. }
  507.  
  508. /*───────────────────────────────────────────────────────────────────────────
  509.   Continuous sweep
  510. ───────────────────────────────────────────────────────────────────────────*/
  511. void continuous_sweep() {
  512.     if (motor_count == 0) {
  513.         Serial.println("No motors configured.");
  514.         return;
  515.     }
  516.  
  517.     int motor_num = ask_value(
  518.         "Enter motor number to sweep (1 to " + String(motor_count) + ", or >" + String(motor_count) + " for all): ",
  519.         1, 1
  520.     );
  521.     bool sweep_all = (motor_num > motor_count);
  522.  
  523.     if (!sweep_all) {
  524.         int motor_index = motor_num - 1;
  525.         if (motor_index < 0 || motor_index >= motor_count || !motors[motor_index].initialized) {
  526.             Serial.println("Invalid motor.");
  527.             return;
  528.         }
  529.         if (motors[motor_index].velocity_mode) {
  530.             Serial.println("Cannot sweep in velocity mode.");
  531.             return;
  532.         }
  533.  
  534.         long pos1 = ask_value(
  535.             "Enter first position (microsteps, default " + String(motors[motor_index].last_start_pos) + "): ",
  536.             motors[motor_index].last_start_pos
  537.         );
  538.         motors[motor_index].last_start_pos = pos1;
  539.  
  540.         long pos2 = ask_value(
  541.             "Enter second position (microsteps, default " + String(motors[motor_index].last_stop_pos) + "): ",
  542.             motors[motor_index].last_stop_pos
  543.         );
  544.         motors[motor_index].last_stop_pos = pos2;
  545.  
  546.         // Render full status before movement (no intermediate black push)
  547.         update_display(true);
  548.  
  549.         Serial.println("Starting continuous sweep. Press any key to stop.");
  550.  
  551.         bool sweeping = true;
  552.         while (sweeping) {
  553.             move_to_position(motor_index, pos1 + 1, motors[motor_index].speed_steps_per_sec * motors[motor_index].microsteps);
  554.             if (Serial.available()) { Serial.read(); sweeping = false; }
  555.             move_to_position(motor_index, pos2 + 1, motors[motor_index].speed_steps_per_sec * motors[motor_index].microsteps);
  556.             if (Serial.available()) { Serial.read(); sweeping = false; }
  557.         }
  558.         Serial.println("Sweep stopped.");
  559.     } else {
  560.         bool any_velocity = false;
  561.         for (int i = 0; i < motor_count; i++) {
  562.             if (motors[i].velocity_mode) { any_velocity = true; break; }
  563.         }
  564.         if (any_velocity) {
  565.             Serial.println("Cannot sweep all; some motors in velocity mode.");
  566.             return;
  567.         }
  568.         long pos1[MAX_MOTORS];
  569.         long pos2[MAX_MOTORS];
  570.         for (int i = 0; i < motor_count; i++) {
  571.             pos1[i] = ask_value(
  572.                 "For motor " + String(i + 1) + ", enter first position (microsteps, default " + String(motors[i].last_start_pos) + "): ",
  573.                 motors[i].last_start_pos
  574.             );
  575.             motors[i].last_start_pos = pos1[i];
  576.  
  577.             pos2[i] = ask_value(
  578.                 "For motor " + String(i + 1) + ", enter second position (microsteps, default " + String(motors[i].last_stop_pos) + "): ",
  579.                 motors[i].last_stop_pos
  580.             );
  581.             motors[i].last_stop_pos = pos2[i];
  582.         }
  583.  
  584.         update_display(true);
  585.  
  586.         Serial.println("Starting continuous sweep for all motors. Press any key to stop.");
  587.  
  588.         bool sweeping = true;
  589.         while (sweeping) {
  590.             for (int i = 0; i < motor_count; i++) {
  591.                 move_to_position(i, pos1[i], motors[i].speed_steps_per_sec * motors[i].microsteps);
  592.                 if (Serial.available()) { Serial.read(); sweeping = false; break; }
  593.             }
  594.             for (int i = 0; i < motor_count; i++) {
  595.                 move_to_position(i, pos2[i], motors[i].speed_steps_per_sec * motors[i].microsteps);
  596.                 if (Serial.available()) { Serial.read(); sweeping = false; break; }
  597.             }
  598.         }
  599.         Serial.println("Sweep stopped.");
  600.     }
  601. }
  602.  
  603. /*───────────────────────────────────────────────────────────────────────────
  604.   Encoder test
  605. ───────────────────────────────────────────────────────────────────────────*/
  606. void test_encoders() {
  607.     Serial.println("Test Encoders. Press any button to exit.");
  608.  
  609.     tft_fb_wait_idle();
  610.     tft_fb_prepare_draw();
  611.     tft.fillScreen(ST7735_BLACK);
  612.     gfx_set_cursor(0, 0);
  613.     tft.setTextColor(ST7735_MAGENTA, ST7735_BLACK);
  614.     gfx_set_text_size(1);
  615.     gfx_println("Test Encoders");
  616.     tft.setTextColor(ST7735_YELLOW, ST7735_BLACK);
  617.     tft_fb_push(false);
  618.  
  619.     const int16_t list_y = gfx_line_height();
  620.     bool done = false;
  621.  
  622.     while (!done) {
  623.         bool changed = false;
  624.         for (int e = 0; e < 4; e++) {
  625.             int32_t new_pos = ss.getEncoderPosition(e);
  626.             if (new_pos != enc_positions[e]) {
  627.                 Serial.print("Enc"); Serial.print(e); Serial.print(": "); Serial.println(new_pos);
  628.                 enc_positions[e] = new_pos;
  629.                 pixels.setPixelColor(e, wheel((new_pos * 4) & 255));
  630.                 pixels.show();
  631.                 changed = true;
  632.             }
  633.             if (!ss.digitalRead(switch_pins[e])) {
  634.                 delay(50);
  635.                 if (!ss.digitalRead(switch_pins[e])) {
  636.                     Serial.print("Button "); Serial.print(e); Serial.println(" pressed");
  637.                     done = true;
  638.                 }
  639.             }
  640.         }
  641.  
  642.         if (changed) {
  643.             tft_fb_prepare_draw();
  644.             tft.fillRect(0, list_y, tft.width(), tft.height() - list_y, ST7735_BLACK);
  645.             gfx_set_cursor(0, list_y);
  646.             for (int e = 0; e < 4; e++) {
  647.                 tft.print("Enc"); tft.print(e); tft.print(": "); tft.print(enc_positions[e]);
  648.                 gfx_newline();
  649.             }
  650.             tft_fb_push(false);
  651.         }
  652.         delay(10);
  653.     }
  654. }
  655.  
  656. /*───────────────────────────────────────────────────────────────────────────
  657.   Menu printing (Serial + TFT)
  658. ───────────────────────────────────────────────────────────────────────────*/
  659. void print_menu() {
  660.     // Serial menu
  661.     Serial.println();
  662.     Serial.println(MENU_TITLE);
  663.     for (int i = 0; i < MENU_ITEM_COUNT; ++i) {
  664.         Serial.println(MENU_ITEMS[i]);
  665.     }
  666.     Serial.print("Enter choice: ");
  667.  
  668.     // TFT menu
  669.     tft_fb_wait_idle();
  670.     tft_fb_prepare_draw();
  671.     tft.fillScreen(ST7735_BLACK);
  672.     gfx_set_cursor(0, 0);
  673.     tft.setTextColor(ST7735_MAGENTA, ST7735_BLACK);
  674.     gfx_set_text_size(1);
  675.     gfx_println(MENU_TITLE);
  676.  
  677.     tft.setTextColor(ST7735_WHITE, ST7735_BLACK);
  678.     for (int i = 0; i < MENU_ITEM_COUNT; ++i) {
  679.         gfx_println(MENU_ITEMS[i]);
  680.     }
  681.  
  682.     tft.setTextColor(ST7735_YELLOW, ST7735_BLACK);
  683.     gfx_println("Modes:");
  684.     for (int i = 0; i < motor_count; i++) {
  685.         tft.print("M");
  686.         tft.print(i + 1);
  687.         tft.print(": ");
  688.         tft.print(motors[i].velocity_mode ? "Vel" : "Pos");
  689.         tft.print(" ");
  690.         tft.print(motors[i].constant_current ? "Const" : "Auto");
  691.         gfx_newline();
  692.     }
  693.     tft_fb_push(false);
  694. }
  695.  
  696. /*───────────────────────────────────────────────────────────────────────────
  697.   Status display
  698. ───────────────────────────────────────────────────────────────────────────*/
  699. void update_display(bool full_update) {
  700.     gfx_set_text_size(1);
  701.     tft.setTextColor(ST7735_YELLOW, ST7735_BLACK);
  702.  
  703.     tft_fb_prepare_draw();
  704.  
  705.     if (full_update) {
  706.         tft.fillScreen(ST7735_BLACK);
  707.         gfx_set_cursor(0, 0);
  708.         tft.setTextColor(ST7735_MAGENTA, ST7735_BLACK);
  709.         gfx_println("Motor Status:");
  710.  
  711.         tft.setTextColor(ST7735_YELLOW, ST7735_BLACK);
  712.         for (int i = 0; i < motor_count; i++) {
  713.             tft.print("M"); tft.print(i + 1); tft.print(": Pos ");
  714.             char buf[16];
  715.             sprintf(buf, "%4ld", motors[i].current_position);
  716.             tft.print(buf);
  717.             tft.setCursor(tft.getCursorX() + 2, tft.getCursorY());
  718.             tft.print(" ");
  719.             if (motors[i].velocity_mode) {
  720.                 tft.print("Vel "); tft.print(motors[i].current_velocity);
  721.             } else {
  722.                 tft.print("Pos");
  723.             }
  724.             tft.print(" ");
  725.             tft.print(motors[i].constant_current ? "Const" : "Auto");
  726.             gfx_newline();
  727.         }
  728.         tft_fb_push_if_idle();
  729.     } else {
  730.         const int16_t first_line_y = gfx_line_height();
  731.         for (int i = 0; i < motor_count; i++) {
  732.             int16_t y = first_line_y + i * gfx_line_height();
  733.  
  734.             // Position field (opaque text overwrites cleanly)
  735.             tft.setCursor(48, y);
  736.             char buf[16];
  737.             sprintf(buf, "%4ld", motors[i].current_position);
  738.             tft.print(buf);
  739.  
  740.             // Velocity field (if applicable)
  741.             if (motors[i].velocity_mode) {
  742.                 tft.setCursor(90, y);
  743.                 tft.print("Vel ");
  744.                 tft.print(motors[i].current_velocity);
  745.             }
  746.         }
  747.         tft_fb_push_if_idle();
  748.     }
  749. }
  750.  
  751. /*───────────────────────────────────────────────────────────────────────────
  752.   Microstep HUD update: rewrite only the numeric position for motor i
  753. ───────────────────────────────────────────────────────────────────────────*/
  754. inline void update_display_microstep(int motor_index) {
  755.     const int16_t first_line_y = gfx_line_height();   // after "Motor Status:" header
  756.     const int16_t y = first_line_y + motor_index * gfx_line_height();
  757.  
  758.     tft_fb_prepare_draw();
  759.     tft.setTextColor(ST7735_YELLOW, ST7735_BLACK);
  760.     tft.setCursor(48, y);
  761.  
  762.     char buf[16];
  763.     sprintf(buf, "%5ld", motors[motor_index].current_position);
  764.     tft.print(buf);
  765.  
  766.     if (motors[motor_index].velocity_mode) {
  767.         tft.setCursor(90, y);
  768.         tft.print("Vel ");
  769.         tft.print(motors[motor_index].current_velocity);
  770.     }
  771.  
  772.     tft_fb_push_if_idle();   // queue non-blocking DMA when idle
  773. }
  774.  
  775. /*───────────────────────────────────────────────────────────────────────────
  776.   Color wheel for seesaw neopixels
  777. ───────────────────────────────────────────────────────────────────────────*/
  778. uint32_t wheel(byte wheel_pos) {
  779.     wheel_pos = 255 - wheel_pos;
  780.     if (wheel_pos < 85) {
  781.         return pixels.Color(255 - wheel_pos * 3, 0, wheel_pos * 3);
  782.     }
  783.     if (wheel_pos < 170) {
  784.         wheel_pos -= 85;
  785.         return pixels.Color(0, wheel_pos * 3, 255 - wheel_pos * 3);
  786.     }
  787.     wheel_pos -= 170;
  788.     return pixels.Color(wheel_pos * 3, 255 - wheel_pos * 3, 0);
  789. }
  790.  
  791. /*───────────────────────────────────────────────────────────────────────────
  792.   Setup / Loop
  793. ───────────────────────────────────────────────────────────────────────────*/
  794. void setup() {
  795.     Serial.begin(9600);
  796.     while (!Serial) {}
  797.     Serial.println("Teensy 4.1 Stepper Motor Control with TMC2209");
  798.     memset(user_input, 0, sizeof(user_input));
  799.  
  800.     // TFT init
  801.     tft.initR(INITR_BLACKTAB);
  802.     tft.setRotation(3); // landscape
  803.     tft.setTextWrap(false);
  804.  
  805.     // Bind framebuffers without heap, enable FB mode
  806.     memset(fb_a, 0, FB_BYTES);
  807.     memset(fb_b, 0, FB_BYTES);
  808.     tft.setFrameBuffer(fb_a);
  809.     tft_fb_ok = (tft.useFrameBuffer(true) != 0);
  810.     draw_fb = fb_a;
  811.     tft.fillScreen(ST7735_BLACK);
  812.     if (tft_fb_ok) {
  813.         tft.updateScreen(); // initial blocking push to prime the panel
  814.     }
  815.  
  816.     // Backlight
  817.     pinMode(TFT_BACKLIGHT, OUTPUT);
  818.     analogWrite(TFT_BACKLIGHT, 96);
  819.  
  820.     // seesaw init
  821.     if (!ss.begin(SEESAW_ADDR) || !pixels.begin(SEESAW_ADDR)) {
  822.         Serial.println("Couldn't find seesaw!");
  823.         while (1) {
  824.             delay(10);
  825.         }
  826.     }
  827.     Serial.println("seesaw started");
  828.     for (int e = 0; e < 4; e++) {
  829.         ss.pinMode(switch_pins[e], INPUT_PULLUP);
  830.         enc_positions[e] = ss.getEncoderPosition(e);
  831.     }
  832.     pixels.setBrightness(255);
  833.     pixels.show();
  834.  
  835.     // Default motor configuration
  836.     motor_count = 2;
  837.     // Motor 1
  838.     motors[0].step_pin = default_step_pins[0];
  839.     motors[0].dir_pin = default_dir_pins[0];
  840.     motors[0].uart_number = default_uart_numbers[0];
  841.     motors[0].full_steps_per_rev = 200;
  842.     motors[0].microsteps = 64;
  843.     motors[0].speed_steps_per_sec = 50;
  844.     motors[0].last_start_pos = 0;
  845.     motors[0].last_stop_pos = 3200;
  846.     // Motor 2
  847.     motors[1].step_pin = default_step_pins[1];
  848.     motors[1].dir_pin = default_dir_pins[1];
  849.     motors[1].uart_number = default_uart_numbers[1];
  850.     motors[1].full_steps_per_rev = 200;
  851.     motors[1].microsteps = 1;
  852.     motors[1].speed_steps_per_sec = 400;
  853.     motors[1].last_start_pos = 0;
  854.     motors[1].last_stop_pos = 200;
  855.  
  856.     initialize_motors();
  857.  
  858.     // Splash / ready
  859.     tft.setTextColor(ST7735_MAGENTA, ST7735_BLACK);
  860.     gfx_set_text_size(1);
  861.     gfx_set_cursor(0, 0);
  862.     gfx_println("Teensy TMC2209 Control");
  863.     gfx_println("Ready");
  864.     tft_fb_push(false);
  865.  
  866.     print_menu();
  867. }
  868.  
  869. void loop() {
  870.     int choice = ask_value("Enter choice: ", 1, 1, 5);
  871.     switch (choice) {
  872.         case 1:
  873.             configure_stepping();
  874.             update_display(true);
  875.             break;
  876.         case 2:
  877.             continuous_sweep();
  878.             break;
  879.         case 3:
  880.             configure_modes();
  881.             update_display(true);
  882.             break;
  883.         case 4:
  884.             set_velocity();
  885.             update_display(true);
  886.             break;
  887.         case 5:
  888.             test_encoders();
  889.             break;
  890.         default:
  891.             Serial.println("Invalid choice.");
  892.             break;
  893.     }
  894.     update_display(false);
  895.     print_menu();
  896. }
Tags: TMC2209 Teensy quadrature encoder OLED TFT display
Advertisement
Comments
  • User was banned
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!