Firmware Version 1.0.4

firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp

@@ -7,6 +7,7 @@
 #include "hardware/spi.h"
 #include "hardware/gpio.h"
 #include "pico/stdlib.h"
+#include "utilities/logging.h"
 
 void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint pin_miso, int32_t baudrate_hz) {
   // Set GPIO functions to SPI
@@ -17,22 +18,27 @@ void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint
   // SPI format: 8 bits, mode 3 (CPOL=1, CPHA=1)
   spi_init(spi, baudrate_hz);
   spi_set_format(spi, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
+
+  sleep_ms(10);
 }
 
 MT6835Encoder::MT6835Encoder(spi_inst_t* spi, uint cs_pin) : spi(spi), cs_pin(cs_pin) {
-  // nop
+  if (cs_pin >= 0) {
+      gpio_init(cs_pin);
+      gpio_set_dir(cs_pin, GPIO_OUT);
+      gpio_put(cs_pin, 1);  // CS high
+  }
 }
 
 MT6835Encoder::~MT6835Encoder() {
   // nop
 }
 
-void MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
-  if (cs_pin >= 0) {
-      gpio_init(cs_pin);
-      gpio_set_dir(cs_pin, GPIO_OUT);
-      gpio_put(cs_pin, 1);  // CS high
-  }
+bool MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
+  sleep_ms(100);
+
+  if(is_connected() == false)
+    return false;
 
   set_rotation_direction(0);    // needs to be set, otherwise might be random
   set_bandwidth(bandwidth);
@@ -40,6 +46,25 @@ void MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
 
   last_raw_angle = 0;
   abs_raw_angle = 0;
+  initialized = true;
+
+  return true;
+}
+
+bool MT6835Encoder::is_connected() {
+  uint8_t check_bytes[] = {37, 109, 179, 251, 1};
+  for(int i=0; i<sizeof(check_bytes); i++) {
+    write_register(MT6835_REG_USERID, check_bytes[i]);
+    uint8_t user_id = read_register(MT6835_REG_USERID);
+    if(user_id != check_bytes[i])
+      return false;
+  }
+
+  return true;
+}
+
+bool MT6835Encoder::is_initialized() {
+  return initialized;
 }
 
 void MT6835Encoder::reset_abs_angle(int32_t abs_raw_angle) {

firmware/MotionControllerRP/src/hardware/MT6835_encoder.h

@@ -145,7 +145,10 @@ class MT6835Encoder {
       MT6835Encoder(spi_inst_t *spi, uint cs_pin);
       virtual ~MT6835Encoder();
 
-      void init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
+      bool init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
+      bool is_connected();
+      bool is_initialized();
+
       void reset_abs_angle(int32_t abs_raw_angle=0);  // resets the total revolutions of abs angle
       void reset_abs_angle_period();                  // Brings abs angle into [0..2pi)
       float read_abs_angle();                         // returns the absolute angle in radians
@@ -198,6 +201,7 @@ class MT6835Encoder {
       bool check_crc = false;
 
   private:
+      bool initialized=false;
       spi_inst_t *spi;
       uint cs_pin;
       uint8_t last_status = 0;

firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp

@@ -36,6 +36,7 @@ TB6612MotorDriver::TB6612MotorDriver(
 {
   max_pwm = (1 << pwm_resolution) - 1;
   set_amplitude(0.0f, false);
+  field_angle = 0.0f;
 }
 
 void init_output_pin(uint8_t pin, bool value) {
@@ -113,6 +114,9 @@ float TB6612MotorDriver::get_amplitude() const {
 }
 
 void TB6612MotorDriver::rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step) {
+  if(delta_angle == 0.0f)
+    return;
+    
   float start = field_angle;
   float rad_per_µs = (delta_angle >= 0.0f) ? rad_per_s*1e-6f : -rad_per_s*1e-6f;
 

firmware/MotionControllerRP/src/hardware/spi_interface.cpp

@@ -0,0 +1,111 @@
+#include "spi_interface.h"
+
+//*** CLASS *****************************************************************************
+
+SpiDma::SpiDma(spi_inst_t* spi, uint sck, uint mosi, uint miso, uint cs)
+        : spi(spi), sck(sck), mosi(mosi), miso(miso), cs(cs) 
+{
+}
+
+void SpiDma::begin(uint32_t baudrate_hz) {
+  // SPI init
+  spi_init(spi, baudrate_hz);
+  spi_set_format(spi, 8,
+                  SPI_CPOL_0,
+                  SPI_CPHA_0,
+                  SPI_MSB_FIRST);
+
+  gpio_set_function(sck, GPIO_FUNC_SPI);
+  gpio_set_function(mosi, GPIO_FUNC_SPI);
+  gpio_set_function(miso, GPIO_FUNC_SPI);
+
+  // Manual CS
+  gpio_init(cs);
+  gpio_set_dir(cs, GPIO_OUT);
+  gpio_put(cs, 1);
+
+  // DMA TX channel
+  dma_tx = dma_claim_unused_channel(true);
+  cfg_tx = dma_channel_get_default_config(dma_tx);
+  channel_config_set_transfer_data_size(&cfg_tx, DMA_SIZE_8);
+  channel_config_set_dreq(&cfg_tx, spi_get_dreq(spi, true));
+  channel_config_set_read_increment(&cfg_tx, true);
+  channel_config_set_write_increment(&cfg_tx, false);
+
+  // --- DMA RX channel ---
+  dma_rx = dma_claim_unused_channel(true);
+  cfg_rx = dma_channel_get_default_config(dma_rx);
+  channel_config_set_transfer_data_size(&cfg_rx, DMA_SIZE_8);
+  channel_config_set_dreq(&cfg_rx, spi_get_dreq(spi, false));
+  channel_config_set_read_increment(&cfg_rx, false);
+  channel_config_set_write_increment(&cfg_rx, true);
+
+  // Configure but don't start yet
+  dma_channel_configure(
+      dma_tx, &cfg_tx,
+      &spi_get_hw(spi)->dr,
+      nullptr, 0, false);
+
+  dma_channel_configure(
+      dma_rx, &cfg_rx,
+      nullptr,
+      &spi_get_hw(spi)->dr,
+      0, false);
+}
+
+bool SpiDma::is_busy() const {
+    return dma_channel_is_busy(dma_tx) ||
+           dma_channel_is_busy(dma_rx);
+}
+
+void SpiDma::transfer(const void* tx_buf, void* rx_buf, size_t bytes) {
+    while (is_busy()) {}
+
+    gpio_put(cs, 0);
+
+    // Ensure minimum CS low time (50 ns) - Approx 14 cycles at 250 MHz
+    for (int i = 0; i < 14; i++) __asm volatile("nop");
+
+    // Setup RX DMA - must be set first!
+    if (rx_buf != nullptr) {
+        dma_channel_set_write_addr(dma_rx, rx_buf, false);
+        dma_channel_set_trans_count(dma_rx, bytes, false);
+    }
+
+    // Setup TX DMA
+    if (tx_buf != nullptr) {
+        dma_channel_set_read_addr(dma_tx, tx_buf, false);
+        dma_channel_set_trans_count(dma_tx, bytes, true);
+    }
+}
+
+void SpiDma::wait_for_finish() {
+  while (is_busy()) {}
+  gpio_put(cs, 1);
+
+  // Ensure minimum CS low time (50 ns) - Approx 14 cycles at 250 MHz
+  for (int i = 0; i < 14; i++) __asm volatile("nop");
+}
+
+void SpiDma::abort() {
+    // Stop TX DMA if running
+    if (dma_channel_is_busy(dma_tx)) {
+        dma_channel_abort(dma_tx);
+    }
+
+    // Stop RX DMA if running
+    if (dma_channel_is_busy(dma_rx)) {
+        dma_channel_abort(dma_rx);
+    }
+
+    // Flush the SPI FIFOs to prevent leftover data
+    spi_get_hw(spi)->dr = 0;       // clear DR (TX FIFO)
+    spi_get_hw(spi)->sr;           // read SR to clear flags
+
+    // Deassert CS pin to leave SPI slave in idle state
+    gpio_put(cs, 1);
+}
+
+
+
+

firmware/MotionControllerRP/src/hardware/spi_interface.h

@@ -0,0 +1,37 @@
+// --------------------------------------------------------------------------------------
+// Project: MicroManipulatorStepper
+// License: MIT (see LICENSE file for full description)
+//          All text in here must be included in any redistribution.
+// Author:  M. S. (diffraction limited)
+// --------------------------------------------------------------------------------------
+
+#pragma once
+
+#include "hardware/spi.h"
+#include "hardware/dma.h"
+#include "hardware/gpio.h"
+
+//*** CLASS *****************************************************************************
+
+class SpiDma {
+  public:
+      SpiDma(spi_inst_t* spi, uint sck, uint mosi, uint miso, uint cs);
+
+      void begin(uint32_t baudrate_hz);
+      bool is_busy() const;
+      void transfer(const void* tx_buf, void* rx_buf, size_t bytes);
+      void wait_for_finish();
+      void abort();
+
+  private:
+      spi_inst_t* spi;
+      uint sck, mosi, miso, cs;
+
+      int dma_tx;
+      int dma_rx;
+      dma_channel_config cfg_tx;
+      dma_channel_config cfg_rx;
+};
+
+
+