Basic MT6701 support

firmware/platformio.ini

@@ -19,7 +19,7 @@ monitor_flags =
 	--filter=esp32_exception_decoder
 lib_deps =
     bodmer/TFT_eSPI@2.4.25
-    askuric/Simple FOC @ ^2.2
+    askuric/Simple FOC @ 2.2.0
     infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
     bxparks/AceButton @ ^1.9.1
 

firmware/src/main.cpp

@@ -1,5 +1,5 @@
 #include <Arduino.h>
-#include <FastLED.h>
+// #include <FastLED.h>
 #include <SimpleFOC.h>
 #include <TFT_eSPI.h>
 
@@ -12,7 +12,7 @@ DisplayTask display_task = DisplayTask(1);
 MotorTask motor_task = MotorTask(0, display_task);
 InterfaceTask interface_task = InterfaceTask(1, motor_task);
 
-CRGB leds[1];
+// CRGB leds[1];
 
 
 void setup() {

firmware/src/motor_task.cpp

@@ -2,6 +2,7 @@
 
 #include "motor_task.h"
 #include "tlv_sensor.h"
+#include "mt6701_sensor.h"
 
 
 template <typename T> T CLAMP(const T& value, const T& low, const T& high) 
@@ -31,8 +32,8 @@ MotorTask::~MotorTask() {}
 BLDCMotor motor = BLDCMotor(7);
 BLDCDriver6PWM driver = BLDCDriver6PWM(27, 26, 25, 33, 32, 13);
 
-TlvSensor tlv = TlvSensor();
-
+// TlvSensor tlv = TlvSensor();
+MT6701Sensor encoder = MT6701Sensor();
 
 Commander command = Commander(Serial);
 
@@ -43,15 +44,15 @@ void MotorTask::run() {
     driver.voltage_power_supply = 5;
     driver.init();
 
-    Wire.begin();
-    Wire.setClock(400000);
-    tlv.init();
+    // Wire.begin();
+    // Wire.setClock(400000);
+    encoder.init();
 
     motor.linkDriver(&driver);
 
     motor.controller = MotionControlType::torque;
     motor.voltage_limit = 5;
-    motor.linkSensor(&tlv);
+    motor.linkSensor(&encoder);
 
     // Not actually using the velocity loop; but I'm using those PID variables
     // because SimpleFOC studio supports updating them easily over serial for tuning.
@@ -66,7 +67,7 @@ void MotorTask::run() {
 
     motor.init();
 
-    tlv.update();
+    encoder.update();
     delay(10);
 
     // Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
@@ -74,7 +75,7 @@ void MotorTask::run() {
     // it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
     // So this value is based on experimentation.
     // TODO: dig into SimpleFOC calibration and find/fix the issue
-    float zero_electric_offset = -0.6;
+    float zero_electric_offset = -0.2;
     motor.initFOC(zero_electric_offset, Direction::CCW);
     Serial.println(motor.zero_electric_angle);
 
@@ -168,7 +169,7 @@ void MotorTask::run() {
 
 
 
-        if (fabsf(motor.shaft_velocity) > 20) {
+        if (fabsf(motor.shaft_velocity) > 60) {
             // Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
             motor.move(0);
         } else {
@@ -186,6 +187,8 @@ void MotorTask::run() {
 
         motor.monitor();
         // command.run();
+
+        delay(1);
     }
 }
 

firmware/src/mt6701_sensor.cpp

@@ -0,0 +1,122 @@
+#include "mt6701_sensor.h"
+#include "driver/spi_master.h"
+
+#define PIN_CSN 12
+#define PIN_SDA 21
+#define PIN_SCL 22
+
+static const float ALPHA = 0.4;
+
+
+static uint8_t tableCRC6[64] = {
+ 0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09,
+ 0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11,
+ 0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39,
+ 0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21,
+ 0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A,
+ 0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32,
+ 0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A,
+ 0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02
+};
+
+/*32-bit input data, right alignment, Calculation over 18 bits (mult. of 6) */
+static uint8_t CRC6_43_18bit (uint32_t w_InputData)
+{
+ uint8_t b_Index = 0;
+ uint8_t b_CRC = 0;
+
+ b_Index = (uint8_t )(((uint32_t)w_InputData >> 12u) & 0x0000003Fu);
+
+ b_CRC = (uint8_t )(((uint32_t)w_InputData >> 6u) & 0x0000003Fu);
+ b_Index = b_CRC ^ tableCRC6[b_Index];
+
+ b_CRC = (uint8_t )((uint32_t)w_InputData & 0x0000003Fu);
+ b_Index = b_CRC ^ tableCRC6[b_Index];
+
+ b_CRC = tableCRC6[b_Index];
+
+ return b_CRC;
+} 
+
+
+
+MT6701Sensor::MT6701Sensor() {}
+
+void MT6701Sensor::init() {
+
+    pinMode(PIN_CSN, OUTPUT);
+    digitalWrite(PIN_CSN, HIGH);
+
+  spi_bus_config_t tx_bus_config = {
+      .mosi_io_num = -1,
+      .miso_io_num = PIN_SDA,
+      .sclk_io_num = PIN_SCL,
+      .quadwp_io_num = -1,
+      .quadhd_io_num = -1,
+      .max_transfer_sz = 1000,
+  };
+  esp_err_t ret = spi_bus_initialize(HSPI_HOST, &tx_bus_config, 1);
+  ESP_ERROR_CHECK(ret);
+
+  spi_device_interface_config_t tx_device_config = {
+      .command_bits=0,
+      .address_bits=0,
+      .dummy_bits=0,
+      .mode=1,
+      .duty_cycle_pos=0,
+      .cs_ena_pretrans=4,
+      .cs_ena_posttrans=0,
+      .clock_speed_hz=4000000,
+      .input_delay_ns=0,
+      .spics_io_num=PIN_CSN,
+      .flags = 0,
+      .queue_size=1,
+      .pre_cb=NULL,
+      .post_cb=NULL,
+  };
+  ret=spi_bus_add_device(HSPI_HOST, &tx_device_config, &spi_device_);
+  ESP_ERROR_CHECK(ret);
+
+
+  spi_transaction_.flags = SPI_TRANS_USE_RXDATA;
+  spi_transaction_.length = 24;
+  spi_transaction_.rxlength = 24;
+  spi_transaction_.tx_buffer = NULL;
+  spi_transaction_.rx_buffer = NULL;
+}
+
+float MT6701Sensor::getSensorAngle() {
+    uint32_t now = micros();
+    if (now - last_update_ > 100) {
+      
+      esp_err_t ret=spi_device_polling_transmit(spi_device_, &spi_transaction_);
+      assert(ret==ESP_OK);
+
+      uint32_t spi_32 = (spi_transaction_.rx_data[0] << 16) | (spi_transaction_.rx_data[1] << 8) | spi_transaction_.rx_data[2];
+      uint32_t angle_spi = spi_32 >> 10;
+
+      uint8_t field_status = (spi_32 >> 6) & 0x3;
+      uint8_t push_status = (spi_32 >> 8) & 0x1;
+      uint8_t loss_status = (spi_32 >> 9) & 0x1;
+
+      uint8_t received_crc = spi_32 & 0x3F;
+      uint8_t calculated_crc = CRC6_43_18bit(spi_32 >> 6);
+      
+      if (received_crc == calculated_crc) {
+        float new_angle = (float)angle_spi * 2 * PI / 16384;
+        float new_x = cosf(new_angle);
+        float new_y = sinf(new_angle);
+        x_ = new_x * ALPHA + x_ * (1-ALPHA);
+        y_ = new_y * ALPHA + y_ * (1-ALPHA);
+      } else {
+        Serial.printf("Bad CRC. expected %d, actual %d\n", calculated_crc, received_crc);
+      }
+
+      last_update_ = now;
+    }
+    float rad = -atan2f(y_, x_);
+    if (rad < 0) {
+        rad += 2*PI;
+    }
+    return rad;
+}

firmware/src/mt6701_sensor.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <SimpleFOC.h>
+#include "driver/spi_master.h"
+
+class MT6701Sensor : public Sensor {
+    public:
+        MT6701Sensor();
+
+        // initialize the sensor hardware
+        void init();
+
+        // Get current shaft angle from the sensor hardware, and 
+        // return it as a float in radians, in the range 0 to 2PI.
+        //  - This method is pure virtual and must be implemented in subclasses.
+        //    Calling this method directly does not update the base-class internal fields.
+        //    Use update() when calling from outside code.
+        float getSensorAngle();
+    private:
+
+        spi_device_handle_t spi_device_;
+        spi_transaction_t spi_transaction_ = {};
+
+        float x_;
+        float y_;
+        uint32_t last_update_;
+};