Modified library for ESP32 native TWAI driver

examples/SineWaveCAN/SineWaveCAN.ino

@@ -1,141 +1,157 @@
-
-#include <Arduino.h>
 #include "ODriveCAN.h"
-
 // Documentation for this example can be found here:
 // https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html
 
-
 /* Configuration of example sketch -------------------------------------------*/
 
-// CAN bus baudrate. Make sure this matches for every device on the bus
-#define CAN_BAUDRATE 250000
-
-// ODrive node_id for odrv0
-#define ODRV0_NODE_ID 0
-
-// Uncomment below the line that corresponds to your hardware.
-// See also "Board-specific settings" to adapt the details for your hardware setup.
-
-// #define IS_TEENSY_BUILTIN // Teensy boards with built-in CAN interface (e.g. Teensy 4.1). See below to select which interface to use.
-// #define IS_ARDUINO_BUILTIN // Arduino boards with built-in CAN interface (e.g. Arduino Uno R4 Minima)
-// #define IS_MCP2515 // Any board with external MCP2515 based extension module. See below to configure the module.
-
-
-/* Board-specific includes ---------------------------------------------------*/
-
-#if defined(IS_TEENSY_BUILTIN) + defined(IS_ARDUINO_BUILTIN) + defined(IS_MCP2515) != 1
-#warning "Select exactly one hardware option at the top of this file."
+  // CAN bus baudrate. Make sure this matches for every device on the bus
+  #define CAN_BAUDRATE 250000
 
-#if CAN_HOWMANY > 0 || CANFD_HOWMANY > 0
-#define IS_ARDUINO_BUILTIN
-#warning "guessing that this uses HardwareCAN"
-#else
-#error "cannot guess hardware version"
-#endif
+  // ODrive node_id for odrv0
+  #define ODRV0_NODE_ID 0
 
-#endif
 
-#ifdef IS_ARDUINO_BUILTIN
-// See https://github.com/arduino/ArduinoCore-API/blob/master/api/HardwareCAN.h
-// and https://github.com/arduino/ArduinoCore-renesas/tree/main/libraries/Arduino_CAN
+  // Uncomment below the line that corresponds to your hardware.
+  // See also "Board-specific settings" to adapt the details for your hardware setup.
 
-#include <Arduino_CAN.h>
-#include <ODriveHardwareCAN.hpp>
-#endif // IS_ARDUINO_BUILTIN
-
-#ifdef IS_MCP2515
-// See https://github.com/sandeepmistry/arduino-CAN/
-#include "MCP2515.h"
-#include "ODriveMCPCAN.hpp"
-#endif // IS_MCP2515
-
-#ifdef IS_TEENSY_BUILTIN
-// See https://github.com/tonton81/FlexCAN_T4
-// clone https://github.com/tonton81/FlexCAN_T4.git into /src
-#include <FlexCAN_T4.h>
-#include "ODriveFlexCAN.hpp"
-struct ODriveStatus; // hack to prevent teensy compile error
-#endif // IS_TEENSY_BUILTIN
+  // #define IS_TEENSY_BUILTIN // Teensy boards with built-in CAN interface (e.g. Teensy 4.1). See below to select which interface to use.
+  // #define IS_ARDUINO_BUILTIN // Arduino boards with built-in CAN interface (e.g. Arduino Uno R4 Minima)
+  // #define IS_MCP2515 // Any board with external MCP2515 based extension module. See below to configure the module.
+  // #define IS_ESP32_TWAI // ESP32 boards using a external transceiver such as sn65hvd230. Uses Espressif's native TWAI driver. Connect and TX_PIN from ESP32 to D on transceiver and RX_PIN from ESP32 to R on transceiver. 
 
 
+/* Board-specific includes ---------------------------------------------------*/
 
+  #if defined(IS_TEENSY_BUILTIN) + defined(IS_ARDUINO_BUILTIN) + defined(IS_MCP2515) + defined(IS_ESP32_TWAI) != 1
+  #warning "Select exactly one hardware option at the top of this file."
+
+  #if CAN_HOWMANY > 0 || CANFD_HOWMANY > 0
+  #define IS_ARDUINO_BUILTIN
+  #warning "guessing that this uses HardwareCAN"
+  #else
+  #error "cannot guess hardware version"
+  #endif
+
+  #endif
+
+  #ifdef IS_ARDUINO_BUILTIN
+  // See https://github.com/arduino/ArduinoCore-API/blob/master/api/HardwareCAN.h
+  // and https://github.com/arduino/ArduinoCore-renesas/tree/main/libraries/Arduino_CAN
+
+  #include <Arduino_CAN.h>
+  #include <ODriveHardwareCAN.hpp>
+  #endif // IS_ARDUINO_BUILTIN
+
+  #ifdef IS_MCP2515
+  // See https://github.com/sandeepmistry/arduino-CAN/
+  #include "MCP2515.h"
+  #include "ODriveMCPCAN.hpp"
+  #endif // IS_MCP2515
+
+  #ifdef IS_TEENSY_BUILTIN
+  // See https://github.com/tonton81/FlexCAN_T4
+  // clone https://github.com/tonton81/FlexCAN_T4.git into /src
+  #include <FlexCAN_T4.h>
+  #include "ODriveFlexCAN.hpp"
+  struct ODriveStatus; // hack to prevent teensy compile error
+  #endif // IS_TEENSY_BUILTIN
+
+  #ifdef IS_ESP32_TWAI
+  // See https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/peripherals/twai.html
+  // https://github.com/espressif/arduino-esp32/tree/master/libraries/ESP32/examples/TWAI
+    // Pins used to connect to CAN bus transceiver:
+    #define RX_PIN 35
+    #define TX_PIN 36    
+    #define TRANSMIT_RATE_MS 50
+    #define POLLING_RATE_MS 50
+  #include "driver/twai.h"
+
+
+  #include "ODriveESP32TWAI.hpp"
+  #endif // IS_ESP32_TWAI
 
 /* Board-specific settings ---------------------------------------------------*/
+  /* Teensy */
+    #ifdef IS_TEENSY_BUILTIN
+
+    FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_intf;
+
+    bool setupCan() {
+      can_intf.begin();
+      can_intf.setBaudRate(CAN_BAUDRATE);
+      can_intf.setMaxMB(16);
+      can_intf.enableFIFO();
+      can_intf.enableFIFOInterrupt();
+      can_intf.onReceive(onCanMessage);
+      return true;
+    }
 
+    #endif // IS_TEENSY_BUILTIN
 
-/* Teensy */
-
-#ifdef IS_TEENSY_BUILTIN
-
-FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_intf;
-
-bool setupCan() {
-  can_intf.begin();
-  can_intf.setBaudRate(CAN_BAUDRATE);
-  can_intf.setMaxMB(16);
-  can_intf.enableFIFO();
-  can_intf.enableFIFOInterrupt();
-  can_intf.onReceive(onCanMessage);
-  return true;
-}
-
-#endif // IS_TEENSY_BUILTIN
-
+  /* MCP2515-based extension modules -*/
+    #ifdef IS_MCP2515
 
-/* MCP2515-based extension modules -*/
+    MCP2515Class& can_intf = CAN;
 
-#ifdef IS_MCP2515
+    // chip select pin used for the MCP2515
+    #define MCP2515_CS 10
 
-MCP2515Class& can_intf = CAN;
+    // interrupt pin used for the MCP2515
+    // NOTE: not all Arduino pins are interruptable, check the documentation for your board!
+    #define MCP2515_INT 2
 
-// chip select pin used for the MCP2515
-#define MCP2515_CS 10
+    // freqeuncy of the crystal oscillator on the MCP2515 breakout board. 
+    // common values are: 16 MHz, 12 MHz, 8 MHz
+    #define MCP2515_CLK_HZ 8000000
 
-// interrupt pin used for the MCP2515
-// NOTE: not all Arduino pins are interruptable, check the documentation for your board!
-#define MCP2515_INT 2
 
-// freqeuncy of the crystal oscillator on the MCP2515 breakout board. 
-// common values are: 16 MHz, 12 MHz, 8 MHz
-#define MCP2515_CLK_HZ 8000000
+    static inline void receiveCallback(int packet_size) {
+      if (packet_size > 8) {
+        return; // not supported
+      }
+      CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size};
+      CAN.readBytes(msg.buffer, packet_size);
+      onCanMessage(msg);
+    }
 
+    bool setupCan() {
+      // configure and initialize the CAN bus interface
+      CAN.setPins(MCP2515_CS, MCP2515_INT);
+      CAN.setClockFrequency(MCP2515_CLK_HZ);
+      if (!CAN.begin(CAN_BAUDRATE)) {
+        return false;
+      }
 
-static inline void receiveCallback(int packet_size) {
-  if (packet_size > 8) {
-    return; // not supported
-  }
-  CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size};
-  CAN.readBytes(msg.buffer, packet_size);
-  onCanMessage(msg);
-}
+      CAN.onReceive(receiveCallback);
+      return true;
+    }
 
-bool setupCan() {
-  // configure and initialize the CAN bus interface
-  CAN.setPins(MCP2515_CS, MCP2515_INT);
-  CAN.setClockFrequency(MCP2515_CLK_HZ);
-  if (!CAN.begin(CAN_BAUDRATE)) {
-    return false;
-  }
+    #endif // IS_MCP2515
 
-  CAN.onReceive(receiveCallback);
-  return true;
-}
+  /* ESP32 board using native TWAI driver */
+    #ifdef IS_ESP32_TWAI
 
-#endif // IS_MCP2515
+      TWAIClass can_intf;
 
+      bool setupCan() {
+        if (!can_intf.begin(CAN_BAUDRATE)) {
+          return false;
+        }
+        return true;
+      }
 
-/* Arduinos with built-in CAN */
+    #endif // IS_ESP32_TWAI
 
-#ifdef IS_ARDUINO_BUILTIN
+  /* Arduinos with built-in CAN */
+    #ifdef IS_ARDUINO_BUILTIN
 
-HardwareCAN& can_intf = CAN;
+    HardwareCAN& can_intf = CAN;
 
-bool setupCan() {
-  return can_intf.begin((CanBitRate)CAN_BAUDRATE);
-}
+    bool setupCan() {
+      return can_intf.begin((CanBitRate)CAN_BAUDRATE);
+    }
 
-#endif
+    #endif
 
 
 /* Example sketch ------------------------------------------------------------*/
@@ -175,16 +191,17 @@ void onCanMessage(const CanMsg& msg) {
   }
 }
 
+
+
 void setup() {
   Serial.begin(115200);
-
-  // Wait for up to 3 seconds for the serial port to be opened on the PC side.
-  // If no PC connects, continue anyway.
-  for (int i = 0; i < 30 && !Serial; ++i) {
-    delay(100);
+  long current_millis = 0;
+  while(!Serial){ 
+    if ((millis() - current_millis)  > 3000){ //check for connection for 3 seconds
+      break; //Break when connection found
+    }
   }
-  delay(200);
-
+  Serial.println("Serial ok");
 
   Serial.println("Starting ODriveCAN demo");
 
@@ -222,8 +239,10 @@ void setup() {
 
   Serial.println("Enabling closed loop control...");
   while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
+
     odrv0.clearErrors();
     delay(1);
+
     odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
 
     // Pump events for 150ms. This delay is needed for two reasons;
@@ -240,6 +259,7 @@ void setup() {
   }
 
   Serial.println("ODrive running!");
+
 }
 
 void loop() {
@@ -248,7 +268,7 @@ void loop() {
                         //
                         // This has been found to reduce the number of dropped messages, however it can be removed
                         // for applications requiring loop times over 100Hz.
-
+                        
   float SINE_PERIOD = 2.0f; // Period of the position command sine wave in seconds
 
   float t = 0.001 * millis();

src/ODriveESP32TWAI.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "TWAI_CAN_Class.h" // Include your custom TWAIClass header
+#include "ODriveCAN.h"
+
+// This is a convenience struct because the TWAIClass doesn't have a
+// native message type.
+struct CanMsg {
+    uint32_t id;
+    uint8_t len;
+    uint8_t buffer[8];
+};
+
+// Must be defined by the application if you want to use defaultCanReceiveCallback().
+void onCanMessage(const CanMsg& msg);
+
+
+static inline bool sendMsg(TWAIClass& can_intf, uint32_t id, uint8_t length, const uint8_t* data) {
+    // Send CAN message
+    can_intf.prepareMessage(id, length, !data);
+    if (data) {
+        for (int i = 0; i < length; ++i) {
+            can_intf.write(data[i], i);
+        }
+    }
+    return can_intf.endPacket();
+}
+
+static inline void onReceive(const CanMsg& msg, ODriveCAN& odrive) {
+    odrive.onReceive(msg.id, msg.len, msg.buffer);
+}
+
+static inline void pumpEvents(TWAIClass& intf) {
+    CanMsg msg;
+    int length = intf.parsePacket(); // Check if a packet is available
+
+    if (length > 0) {
+        msg.id = intf.packetId(); // Retrieve the ID of the received message
+        // Debug print to check if ID is read correctly
+        //Serial.print("Received Raw Frame ID: 0x"); // DEBUG PRINT uncomment to print all messages to serial console **********************************
+        //Serial.println(msg.id, HEX); // DEBUG PRINT uncomment to print all messages to serial console **********************************
+        msg.len = length;
+        intf.readBytes(msg.buffer, length); // Retrieve the data from the message
+        onCanMessage(msg); // Call the user-defined callback
+    }
+}
+
+CREATE_CAN_INTF_WRAPPER(TWAIClass)