Merge pull request #21 from chikuta/add-esp32-interface

Add esp32 interface

examples/change_motor_can_id/change_motor_can_id.ino

@@ -1,17 +1,13 @@
 #include <Arduino.h>
 #include <math.h>
-#include <mcp_can.h>
 #include <M5Stack.h>
-#include "cybergear_controller.hh"
+#include "cybergear_driver.hh"
 
-/**
- * @brief Init can interface
- */
-void init_can();
-
-// init MCP_CAN object
-#define CAN0_INT 15  // Set INT to pin 2
-MCP_CAN CAN0(12);    // Set CS to pin 10
+#ifdef USE_ESP32_CAN
+#include "cybergear_can_interface_esp32.hh"
+#else
+#include "cybergear_can_interface_mcp.hh"
+#endif
 
 // setup master can id and motor can id (default cybergear can id is 0x7F)
 uint8_t MASTER_CAN_ID = 0x00;
@@ -21,6 +17,12 @@ uint8_t MOT_NEXT_CAN_ID = 0x7E;
 // init cybergeardriver
 CybergearDriver driver = CybergearDriver(MASTER_CAN_ID, MOT_CURRENT_CAN_ID);
 
+#ifdef USE_ESP32_CAN
+CybergearCanInterfaceEsp32 interface;
+#else
+CybergearCanInterfaceMcp interface;
+#endif
+
 void setup()
 {
   M5.begin();
@@ -29,8 +31,8 @@ void setup()
   M5.Lcd.printf("Start change_motor_can_id\n");
   M5.Lcd.printf("Change motor can id from [0x%02x] to [0x%02x]\n", MOT_CURRENT_CAN_ID, MOT_NEXT_CAN_ID);
 
-  init_can();
-  driver.init(&CAN0);
+  interface.init();
+  driver.init(&interface);
   driver.init_motor(MODE_POSITION);
   driver.change_motor_can_id(MOT_NEXT_CAN_ID);
 }
@@ -39,10 +41,3 @@ void loop()
 {
   M5.update();
 }
-
-void init_can()
-{
-  CAN0.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ);
-  CAN0.setMode(MCP_NORMAL);  // Set operation mode to normal so the MCP2515 sends acks to received data.
-  pinMode(CAN0_INT, INPUT);  // Configuring pin for /INT input
-}

examples/control_mode_example/control_mode_example.ino

@@ -1,19 +1,18 @@
 #include <Arduino.h>
 #include <math.h>
-#include <mcp_can.h>
 #include <M5Stack.h>
 #include "cybergear_driver.hh"
 
+#ifdef USE_ESP32_CAN
+#include "cybergear_can_interface_esp32.hh"
+#else
+#include "cybergear_can_interface_mcp.hh"
+#endif
+
 #define INC_POSITION  20.0
 #define INC_VELOCITY  0.4
 #define INC_TORQUE    0.04
 
-
-/**
- * @brief Init can interface
- */
-void init_can();
-
 /**
  * @brief Draw display
  *
@@ -31,18 +30,18 @@ void draw_display(uint8_t mode, bool is_mode_change = false);
  */
 void get_color_and_mode_str(uint8_t mode, uint16_t & color, String & mode_str);
 
-
-// init MCP_CAN object
-#define CAN0_INT 15  // Set INT to pin 2
-MCP_CAN CAN0(12);    // Set CS to pin 10
-
 // setup master can id and motor can id (default cybergear can id is 0x7F)
 uint8_t MASTER_CAN_ID = 0x00;
-uint8_t MOT_CAN_ID = 0x7F;
+uint8_t MOT_CAN_ID = 0x7E;
 
 // init cybergeardriver
 CybergearDriver driver = CybergearDriver(MASTER_CAN_ID, MOT_CAN_ID);
 MotorStatus motor_status;
+#ifdef USE_ESP32_CAN
+CybergearCanInterfaceEsp32 interface;
+#else
+CybergearCanInterfaceMcp interface;
+#endif
 
 // init sprite for display
 TFT_eSprite sprite = TFT_eSprite(&sprite);
@@ -68,8 +67,9 @@ void setup()
   sprite.createSprite(M5.Lcd.width(), M5.Lcd.height());
 
   // init cybergear driver
-  init_can();
-  driver.init(&CAN0);
+  interface.init(15, 5);
+  interface.init();
+  driver.init(&interface);
   driver.init_motor(mode);
   driver.set_limit_speed(init_speed);
   driver.enable_motor();
@@ -210,22 +210,10 @@ void loop()
   }
 
   // update and get motor data
-  if ( driver.process_can_packet() ) {
+  if ( driver.process_packet() ) {
     motor_status = driver.get_motor_status();
     draw_display(mode);
   }
 
   delay(200);
 }
-
-void init_can()
-{
-  if (CAN0.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ) == CAN_OK) {
-      sprite.printf("MCP2515 Initialized Successfully!\n");
-
-  } else {
-      sprite.printf("Error Initializing MCP2515...");
-  }
-  CAN0.setMode(MCP_NORMAL);  // Set operation mode to normal so the MCP2515 sends acks to received data.
-  pinMode(CAN0_INT, INPUT);  // Configuring pin for /INT input
-}

examples/cybergear_bilateral/cybergear_bilateral.ino

@@ -1,17 +1,12 @@
 #include <Arduino.h>
-#include <math.h>
-#include <mcp_can.h>
 #include <M5Stack.h>
+#include <math.h>
 #include "cybergear_controller.hh"
-
-/**
- * @brief Init can interface
- */
-void init_can();
-
-// init MCP_CAN object
-#define CAN0_INT 15  // Set INT to pin 2
-MCP_CAN CAN0(12);    // Set CS to pin 10
+#ifdef USE_ESP32_CAN
+#include "cybergear_can_interface_esp32.hh"
+#else
+#include "cybergear_can_interface_mcp.hh"
+#endif
 
 // setup master can id and motor can id (default cybergear can id is 0x7F)
 uint8_t MASTER_CAN_ID = 0x00;
@@ -25,18 +20,20 @@ std::vector<float> currents = {0.0f, 0.0f};
 
 // init cybergeardriver
 CybergearController controller = CybergearController(MASTER_CAN_ID);
-
+#ifdef USE_ESP32_CAN
+CybergearCanInterfaceEsp32 interface;
+#else
+CybergearCanInterfaceMcp interface;
+#endif
 
 void setup()
 {
   M5.begin();
 
-  // init cybergear driver
-  init_can();
-
   // init position offset
   M5.Lcd.print("Init motors ... ");
-  controller.init(motor_ids, MODE_POSITION, &CAN0);
+  interface.init();
+  controller.init(motor_ids, MODE_POSITION, &interface);
   controller.enable_motors();
   M5.Lcd.println("done");
 
@@ -47,7 +44,7 @@ void setup()
 
   // start bilateral mode
   M5.Lcd.print("starting leader follower demo ... ");
-  controller.init(motor_ids, MODE_CURRENT, &CAN0);
+  controller.init(motor_ids, MODE_CURRENT, &interface);
   controller.enable_motors();
   M5.Lcd.println("done");
 }
@@ -61,7 +58,7 @@ void loop()
 
   // update and get motor data
   std::vector<MotorStatus> status_list;
-  if ( controller.process_can_packet() ) {
+  if ( controller.process_packet() ) {
     controller.get_motor_status(status_list);
   }
 
@@ -72,10 +69,3 @@ void loop()
   currents[0] = min(max(currents[0], -MAX_CURRENT), MAX_CURRENT);
   currents[1] = min(max(currents[1], -MAX_CURRENT), MAX_CURRENT);
 }
-
-void init_can()
-{
-  CAN0.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ);
-  CAN0.setMode(MCP_NORMAL);  // Set operation mode to normal so the MCP2515 sends acks to received data.
-  pinMode(CAN0_INT, INPUT);  // Configuring pin for /INT input
-}

examples/cybergear_m5_bridge/cybergear_m5_bridge.ino

@@ -1,21 +1,14 @@
 #include <Arduino.h>
 #include <math.h>
-#include <mcp_can.h>
 #include <M5Stack.h>
 #include "ros2_logo.hh"
 #include "cybergear_bridge.hh"
 #include "cybergear_controller.hh"
-
-
-/**
- * @brief Init can interface
- */
-void init_can();
-void can_receive_task();
-
-// init MCP_CAN object
-#define CAN0_INT 15  // Set INT to pin 2
-MCP_CAN CAN0(12);    // Set CS to pin 10
+#ifdef USE_ESP32_CAN
+#include "cybergear_can_interface_esp32.hh"
+#else
+#include "cybergear_can_interface_mcp.hh"
+#endif
 
 // setup master can id and motor can id (default cybergear can id is 0x7F)
 uint8_t MASTER_CAN_ID = 0x00;
@@ -26,20 +19,25 @@ std::vector<uint8_t> motor_ids = {MOT_LEADER_CAN_ID, MOT_FOLLOWER_CAN_ID};
 // init cybergeardriver
 CybergearController controller = CybergearController(MASTER_CAN_ID);
 CybergearBridge bridge = CybergearBridge(&controller, &Serial);
+#ifdef USE_ESP32_CAN
+CybergearCanInterfaceEsp32 interface;
+#else
+CybergearCanInterfaceMcp interface;
+#endif
 
 
 void setup()
 {
-  M5.begin(true,true,false);
+  M5.begin(true, true, false);
   Serial.begin(2000000);
   Serial.flush();
 
   M5.Lcd.fillScreen(WHITE);
   M5.Lcd.drawBitmap((M5.Lcd.width() - imgWidth)/2, (M5.Lcd.height() - imgHeight)/2, imgWidth, imgHeight, (uint16_t *)img);
 
   // init cybergear driver
-  init_can();
-  controller.init(motor_ids, MODE_CURRENT, &CAN0);
+  interface.init();
+  controller.init(motor_ids, MODE_CURRENT, &interface);
 
   delay(1000);
   M5.Lcd.fillScreen(BLACK);
@@ -52,10 +50,3 @@ void loop()
   bridge.process_request_command();
   bridge.process_motor_response();
 }
-
-void init_can()
-{
-  CAN0.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ);
-  CAN0.setMode(MCP_NORMAL);  // Set operation mode to normal so the MCP2515 sends acks to received data.
-  pinMode(CAN0_INT, INPUT);  // Configuring pin for /INT input
-}

platformio.ini

@@ -16,3 +16,5 @@ lib_deps =
   m5stack/M5Stack@^0.4.6
   coryjfowler/mcp_can@^1.5.0
   Locoduino/RingBuffer@^1.0.4
+  https://github.com/project-sternbergia/arduino-CAN
+