From 56bd28341c4703b2eb5823f0ebcc5efd188ba568 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Wed, 6 Sep 2023 05:03:20 +0800
Subject: [PATCH] update

---
 components/eq_20_asb_motor/eq20_servomotor.cpp | 54 ++++++++++++++++++++++----
 components/eq_20_asb_motor/eq20_servomotor.hpp | 10 ++---
 components/modbus/modbus_block_host.hpp        |  5 ++-
 3 files changed, 54 insertions(+), 15 deletions(-)

diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp
index e6bf880..34e916b 100644
--- a/components/eq_20_asb_motor/eq20_servomotor.cpp
+++ b/components/eq_20_asb_motor/eq20_servomotor.cpp
@@ -1,6 +1,8 @@
 #include "eq20_servomotor.hpp"
 
 #include <stdint.h>
+
+#include "sdk\components\modbus\modbus_basic.hpp"
 using namespace std;
 using namespace iflytop;
 
@@ -11,18 +13,54 @@ static void dumpbuf(const char *mark, const char *buf, int len) {
   }
   printf("\n");
 }
-void ServoMotor::init(ModbusBlockHost *modbusBlockHost) {
+void Eq20ServoMotor::init(ModbusBlockHost *modbusBlockHost) {
   //  this->com = com;
   m_modbusBlockHost = modbusBlockHost;
 }
-bool ServoMotor::writereg32(int deviceid, uint32_t regadd, int value) {  //
-  m_modbusBlockHost->writeReg10(deviceid, regadd, value, 50);
-}
-bool ServoMotor::readreg(int deviceid, uint32_t regadd, int &value) {
-  uint16_t value16[2];
-  m_modbusBlockHost->readReg03Muti(deviceid, regadd, value16, 2, 50);
-  value = value16[0] + (value16[1] << 16);
+bool Eq20ServoMotor::writereg32(int deviceid, uint32_t regadd, int value) {  //
+  m_modbusBlockHost->cleanRxBuff();
+
+  uint8_t sendBuff[30] = {0};
+  sendBuff[0]          = deviceid;
+  sendBuff[1]          = 0x10;
+  sendBuff[2]          = regadd / 256;  // regadd hi
+  sendBuff[3]          = regadd % 256;  // regadd li
+  sendBuff[4]          = (0);
+  sendBuff[5]          = (2);
+  sendBuff[6]          = 4;
+
+  sendBuff[7]  = (value >> 8) & 0xff;
+  sendBuff[8]  = (value >> 0) & 0xff;
+  sendBuff[9]  = (value >> 24) & 0xff;
+  sendBuff[10] = (value >> 16) & 0xff;
+  modbus_pack_crc_to_packet(sendBuff, 13);
+
+  int sendBuff_length = 13;  // 2byte УÑé
+  dumpbuf("tx", (char *)sendBuff, sendBuff_length);
+
+  // int tx = com->send((char *)sendBuff, sendBuff_length);
+  HAL_StatusTypeDef statu = HAL_UART_Transmit(m_modbusBlockHost->huart, sendBuff, sendBuff_length, 1000);
+  if (statu != HAL_OK) {
+    ZLOGE("Eq20ServoMotor", "uart send error");
+    return false;
+  }
+
+  char rxbuf[30] = {0};
+  // int  rxsize    = com->receive(rxbuf, 8, 100);
+  statu = HAL_UART_Receive(m_modbusBlockHost->huart, (uint8_t *)rxbuf, 8, 100);
+
+  if (statu != HAL_OK) {
+    ZLOGE("Eq20ServoMotor", "uart send error,overtime");
+    return false;
+  }
+  // dumpbuf("rx", rxbuf, rxsize);
   return true;
 }
+// bool Eq20ServoMotor::readreg(int deviceid, uint32_t regadd, int &value) {
+//   uint16_t value16[2];
+//   m_modbusBlockHost->readReg03Muti(deviceid, regadd, value16, 2, 50);
+//   value = value16[0] + (value16[1] << 16);
+//   return true;
+// }
 
 //
diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp
index b5fe3dc..5d389a6 100644
--- a/components/eq_20_asb_motor/eq20_servomotor.hpp
+++ b/components/eq_20_asb_motor/eq20_servomotor.hpp
@@ -6,17 +6,17 @@
 namespace iflytop {
 using namespace std;
 
-class ServoMotor {
+class Eq20ServoMotor {
  private:
-  ModbusBlockHost    *m_modbusBlockHost;
+  ModbusBlockHost *m_modbusBlockHost;
 
  public:
-  ServoMotor(/* args */){};
-  ~ServoMotor(){};
+  Eq20ServoMotor(/* args */){};
+  ~Eq20ServoMotor(){};
 
   void init(ModbusBlockHost *modbusBlockHost);
   bool writereg32(int deviceid, uint32_t regadd, int value);
-  bool readreg(int deviceid, uint32_t regadd, int &value);
+  bool writePn(int deviceid, uint32_t pnadd, int value) { writereg32(deviceid, pnadd * 2, value); }
 
  private:
 };
diff --git a/components/modbus/modbus_block_host.hpp b/components/modbus/modbus_block_host.hpp
index cef4318..9cf8173 100644
--- a/components/modbus/modbus_block_host.hpp
+++ b/components/modbus/modbus_block_host.hpp
@@ -1,9 +1,9 @@
 #pragma once
 #include "sdk/os/zos.hpp"
 
-
 namespace iflytop {
 class ModbusBlockHost {
+ public:
   UART_HandleTypeDef *huart;
 
   uint8_t txbuff[100];
@@ -20,8 +20,9 @@ class ModbusBlockHost {
   bool writeReg06(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems);
   bool writeReg10(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems);
 
- private:
   void cleanRxBuff();
+
+ private:
   void uarttx(uint8_t *buff, size_t len);
   bool uartrx(uint8_t *buff, size_t len, int overtimems);
 };