diff --git a/components/step_motor_ctrl_module/tmc51x0_motor.cpp b/components/step_motor_ctrl_module/tmc51x0_motor.cpp index 6bcce96..1f5d74c 100644 --- a/components/step_motor_ctrl_module/tmc51x0_motor.cpp +++ b/components/step_motor_ctrl_module/tmc51x0_motor.cpp @@ -662,40 +662,56 @@ int32_t TMC51X0Motor::motor_set_enc_resolution(int32_t enc_resolution) { */ int32_t enc_resolution_tmp = enc_resolution * 4; - int32_t enc_const_integral = 0; - int32_t enc_const_fractional = 0; + int16_t enc_const_integral = 0; + int16_t enc_const_fractional = 0; switch (abs(enc_resolution_tmp)) { case 1000: enc_const_integral = 51; - enc_const_fractional = 2; + enc_const_fractional = 0.2 * 10000; break; case 1024: enc_const_integral = 50; - enc_const_fractional = 0; + enc_const_fractional = 0 * 10000; break; case 4000: enc_const_integral = 12; - enc_const_fractional = 8; + enc_const_fractional = 0.8 * 10000; break; case 4096: enc_const_integral = 12; - enc_const_fractional = 5; + enc_const_fractional = 0.5 * 10000; break; case 16384: enc_const_integral = 3; - enc_const_fractional = 125; + enc_const_fractional = 0.125 * 10000; break; default: return err::kparam_out_of_range; break; } - if (enc_resolution_tmp < 0) { - enc_const_integral = -enc_const_integral; - } + // if (enc_resolution_tmp < 0) { + // enc_const_integral = -enc_const_integral; + // } m_motor_enc_resolution = enc_resolution; - int32_t setval = enc_const_integral * (2 ^ 16) + enc_const_fractional; + uint32_t setval = 0; + uint8_t* psetval = (uint8_t*)&setval; + + // if (enc_resolution_tmp < 0) { + // enc_const_integral = -enc_const_integral; + // } + + // if (enc_resolution_tmp < 0) { + // enc_const_fractional = enc_const_fractional; + // } + + memcpy(psetval + 2, &enc_const_integral, 2); + memcpy(psetval, &enc_const_fractional, 2); + + // int32_t setval = (enc_const_integral << 16) + enc_const_fractional * 10000; + // ZLOGI(TAG, "setval:%d", setval); + m_stepM1->writeInt(TMC5130_ENCMODE, 0x1 << 10); m_stepM1->writeInt(TMC5130_ENC_CONST, setval); return 0; } @@ -716,7 +732,14 @@ int32_t TMC51X0Motor::motor_read_enc_val(int32_t* enc_val) { if (m_motor_enc_resolution == 0) { return motor_read_pos(enc_val); } + // ZLOGI(TAG, "%d , %d", m_stepM1->getENCVAL(), m_stepM1->getXACTUAL()); + // m_stepM1->setENCVAL(0); inverse_kinematics(m_stepM1->getENCVAL(), *enc_val); + + if (m_motor_enc_resolution < 0) { + *enc_val = -*enc_val; + } + return 0; } diff --git a/components/subcanmodule/zcancmder_subboard_initer.cpp b/components/subcanmodule/zcancmder_subboard_initer.cpp index 86dbb2a..9b0c12d 100644 --- a/components/subcanmodule/zcancmder_subboard_initer.cpp +++ b/components/subcanmodule/zcancmder_subboard_initer.cpp @@ -93,6 +93,8 @@ void ZCancmderSubboardIniter::loop() { OSDefaultSchduler::getInstance()->loop(); zcanCmder.loop(); // cmder.schedule(); + // uint8_t table[20] = {1, 23, 4, 5, 6, 7, 8, 9}; + // zcanCmder.sendPacket(table, 20); } } #endif \ No newline at end of file diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index 33335ca..b53f214 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -259,6 +259,12 @@ void TMC5130::writeInt(uint8_t address, int32_t value) { CriticalContext cc; writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); } + void TMC5130::writeInt(uint8_t address, int32_t mask, int32_t shift, int32_t value){ + CriticalContext cc; + writeInt(address, FIELD_SET(readInt(address), mask, shift, value)); + + } + int32_t TMC5130::readInt(uint8_t address) { CriticalContext cc; address = TMC_ADDRESS(address); diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index 2d1c30c..f87d9a7 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -88,7 +88,7 @@ class TMC5130 : public IStepperMotor { const uint8_t *m_registerAccessTable; const int32_t *m_defaultRegisterResetState; - int32_t m_scale = 10000; + int32_t m_scale = 51200; int32_t m_scale_deceleration = 1; mres_type_t m_MRES = kmres_256; @@ -156,6 +156,7 @@ class TMC5130 : public IStepperMotor { void readWriteArray(uint8_t *data, size_t length); void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4); void writeInt(uint8_t address, int32_t value); + void writeInt(uint8_t address, int32_t mask, int32_t shift, int32_t value); int32_t readInt(uint8_t address); void setNoAccLimit(bool enable) override; diff --git a/components/zcancmder/zcanreceiver.cpp b/components/zcancmder/zcanreceiver.cpp index f86d834..d9c721f 100644 --- a/components/zcancmder/zcanreceiver.cpp +++ b/components/zcancmder/zcanreceiver.cpp @@ -305,6 +305,9 @@ void ZCanCmder::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle) uint8_t nframe = (pHeader.ExtId & 0xFF00) >> 8; uint8_t frameId = (pHeader.ExtId & 0x00FF); CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0]; + // ZLOGI(TAG, "rx packet %d %d %d", from, nframe, frameId); + + //在中断中打印，代码会卡死 if (from != rxbuf->id) { // 目前只接收来自主机的消息 continue; @@ -327,6 +330,7 @@ void ZCanCmder::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle) rxbuf->m_canPacketNum++; } if (nframe == frameId + 1) { + // ZLOGI(TAG, "rx packet %d", rxbuf->m_canPacketNum); rxbuf->dataIsReady = true; } } @@ -342,6 +346,7 @@ void ZCanCmder::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) { void ZCanCmder::loop() { CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0]; if (rxbuf->dataIsReady) { + // ZLOGI(TAG, "rx packet %d", rxbuf->m_canPacketNum); int dataoff = 0; for (size_t i = 0; i < rxbuf->m_canPacketNum; i++) { memcpy(rxbuf->rxdata + dataoff, rxbuf->m_canPacket[i].aData, rxbuf->m_canPacket[i].dlc); diff --git a/components/zcancmder/zcanreceiver.hpp b/components/zcancmder/zcanreceiver.hpp index a2dcebe..a977640 100644 --- a/components/zcancmder/zcanreceiver.hpp +++ b/components/zcancmder/zcanreceiver.hpp @@ -76,6 +76,7 @@ class ZCanCmder : public ZCanIRQListener, public IZCanCmder { virtual int32_t sendAck(zcr_cmd_header_t *rx_cmd_header, int32_t *ackvar, int32_t nack) override; virtual int32_t sendErrorAck(zcr_cmd_header_t *rx_cmd_header, int32_t errorcode) override; virtual int32_t triggerEvent(zcr_cmd_header_t *cmd_header, uint8_t *data, int32_t len) override; + void sendPacket(uint8_t *packet, size_t len); void loop(); @@ -84,7 +85,6 @@ class ZCanCmder : public ZCanIRQListener, public IZCanCmder { virtual void STM32_HAL_onCAN_Error(CAN_HandleTypeDef *can); private: - void sendPacket(uint8_t *packet, size_t len); bool sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems); HAL_StatusTypeDef initializeFilter();