Merge pull request #27 from chikuta/support-fw-1.2.1.5

Support fw 1.2.1.5

README.md

@@ -12,18 +12,24 @@ M5 stack library for cybergear
 
 * M5Stack Basic V2.7
 
-## H/W Components
+## H/W Components (MCP2515)
 
 * [Xiaomi Cybergear](https://www.mi.com/cyber-gear)
 * [M5Stack Basic V2.7](https://shop.m5stack.com/collections/m5-controllers/products/esp32-basic-core-lot-development-kit-v2-7)
 * [M5Stack Commu module \[M001\]](https://shop.m5stack.com/products/commu-module)
 * [XT30(2+2)-F](https://www.china-amass.com/product/contain/1Yf5h7G4u1927079)
 * [Grove Cable](https://www.seeedstudio.com/Grove-Universal-4-Pin-Buckled-20cm-Cable-5-PCs-pack.html)
 
-## H/W Connection
-
 ![image](https://github.com/project-sternbergia/cybergear_m5/assets/147309062/c36d82cf-e91a-45da-ac53-a79e8d8fc730)
 
+## H/W Components (ESP32 + CAN Transceiver Unit)
+
+* [Xiaomi Cybergear](https://www.mi.com/cyber-gear)
+* [M5Stack Basic V2.7](https://shop.m5stack.com/collections/m5-controllers/products/esp32-basic-core-lot-development-kit-v2-7)
+* [LAN Module W5500 with PoE V12](https://shop.m5stack.com/products/lan-module-w5500-with-poe-v12) or [CANBus Unit(CA-IS3050G)](https://shop.m5stack.com/products/canbus-unitca-is3050g)
+* [XT30(2+2)-F](https://www.china-amass.com/product/contain/1Yf5h7G4u1927079)
+* [Grove Cable](https://www.seeedstudio.com/Grove-Universal-4-Pin-Buckled-20cm-Cable-5-PCs-pack.html)
+
 ## How to use Official GUI tool
 
 This software requires a specific CAN to USB module.
@@ -47,6 +53,7 @@ Tested(for reference):
 cd ~/Arduino/libraries
 git clone https://github.com/coryjfowler/MCP_CAN_lib.git
 git clone https://github.com/Locoduino/RingBuffer.git
+git clone [email protected]:project-sternbergia/arduino-CAN.git
 git clone https://github.com/project-sternbergia/cybergear_m5.git
 ```
 
@@ -84,7 +91,7 @@ After that write [cybergear_m5/examples/cybergear_bilateral.ino](https://github.
 
 ## References
 
-* Xiaomi Cybergear 微电机使用说明书
+* [Xiaomi Cybergear 微电机使用说明书](https://web.vip.miui.com/page/info/mio/mio/detail?postId=40233100)
 
 ## LICENSE
 

examples/dump_ram_test/dump_ram_test.ino

@@ -0,0 +1,72 @@
+#include <Arduino.h>
+#include <M5Stack.h>
+#include "cybergear_driver.hh"
+
+#define USE_ESP32_CAN
+#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;
+uint8_t MOT_CAN_ID = 0x7F;
+
+// init cybergeardriver
+CybergearDriver driver = CybergearDriver(MASTER_CAN_ID, MOT_CAN_ID);
+
+#ifdef USE_ESP32_CAN
+CybergearCanInterfaceEsp32 interface;
+#else
+CybergearCanInterfaceMcp interface;
+#endif
+
+void setup()
+{
+  M5.begin(true, false, true);
+
+  // init cybergear driver
+  M5.Lcd.printf("Start read write motor param test\n");
+
+  interface.init();
+  driver.init(&interface);
+  driver.init_motor(MODE_POSITION);
+  delay(1000);
+  driver.enable_motor();
+}
+
+void loop()
+{
+  M5.update();
+
+  // get motor parameter
+  driver.set_position_ref(0.0f);
+  driver.dump_motor_param();
+  driver.process_packet();
+
+  MotorParameter param = driver.get_motor_param();
+  M5.Lcd.fillScreen(BLACK);
+  M5.Lcd.setCursor(0, 0);
+  M5.Lcd.printf("Received motor param\n");
+  M5.Lcd.printf("  stamp    : %u\n", param.stamp_usec);
+  M5.Lcd.printf("  run_mode : %d\n", param.run_mode);
+  M5.Lcd.printf("  iq_ref : %f\n", param.iq_ref);
+  M5.Lcd.printf("  spd_ref : %f\n", param.spd_ref);
+  M5.Lcd.printf("  limit_torque : %f\n", param.limit_torque);
+  M5.Lcd.printf("  cur_kp : %f\n", param.cur_kp);
+  M5.Lcd.printf("  cur_ki : %f\n", param.cur_ki);
+  M5.Lcd.printf("  cur_filt_gain : %f\n", param.cur_filt_gain);
+  M5.Lcd.printf("  loc_ref : %f\n", param.loc_ref);
+  M5.Lcd.printf("  limit_spd : %f\n", param.limit_spd);
+  M5.Lcd.printf("  limit_cur : %f\n", param.limit_cur);
+  M5.Lcd.printf("  mech_pos : %f\n", param.mech_pos);
+  M5.Lcd.printf("  iqf : %f\n", param.iqf);
+  M5.Lcd.printf("  mech_vel : %f\n", param.mech_vel);
+  M5.Lcd.printf("  vbus : %f\n", param.vbus);
+  M5.Lcd.printf("  rotation : %d\n", param.rotation);
+  M5.Lcd.printf("  loc_kp : %f\n", param.loc_kp);
+  M5.Lcd.printf("  spd_kp : %f\n", param.spd_kp);
+  M5.Lcd.printf("  spd_ki : %f\n", param.spd_ki);
+  delay(5000);
+}

src/cybergear_can_interface_esp32.cpp

@@ -56,6 +56,7 @@ bool CybergearCanInterfaceEsp32::init(uint8_t rx_pin, uint8_t tx_pin)
 
 bool CybergearCanInterfaceEsp32::send_message(uint32_t id, const uint8_t *data, uint8_t len, bool ext)
 {
+  CG_DEBUG_FUNC
   // change packet type
   if (ext) {
     CAN.beginExtendedPacket(id);
@@ -71,6 +72,7 @@ bool CybergearCanInterfaceEsp32::send_message(uint32_t id, const uint8_t *data,
 
 bool CybergearCanInterfaceEsp32::read_message(unsigned long& id, uint8_t *data, uint8_t& len)
 {
+  CG_DEBUG_FUNC
   // check empty
   if (buffer.isEmpty()) return false;
 
@@ -87,5 +89,6 @@ bool CybergearCanInterfaceEsp32::read_message(unsigned long& id, uint8_t *data,
 
 bool CybergearCanInterfaceEsp32::available()
 {
+  CG_DEBUG_FUNC
   return (buffer.isEmpty() == false);
 }

src/cybergear_can_interface_mcp.cpp

@@ -14,14 +14,14 @@ bool CybergearCanInterfaceMcp::init(uint8_t cs_pin, uint8_t int_pin)
 {
   if (pcan_ != nullptr)
   {
-    CG_DEBUG_PRINLN("Failed to open can intetrface. CAN interface has already opened.");
+    CG_DEBUG_PRINTLN("Failed to open can intetrface. CAN interface has already opened.");
     return false;
   }
 
   pcan_ = new MCP_CAN(cs_pin);
   if (!pcan_->begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ) == CAN_OK)
   {
-    CG_DEBUG_PRINLN("Failed to open can intetrface.");
+    CG_DEBUG_PRINTLN("Failed to open can intetrface.");
     return false;
   }
 

src/cybergear_controller.cpp

@@ -3,7 +3,6 @@
 #include "cybergear_driver.hh"
 #include "cybergear_driver_defs.hh"
 #include "cybergear_driver_utils.hh"
-#include <cmath>
 
 CybergearController::CybergearController(uint8_t master_can_id)
   : can_(NULL)