From 6e3e7e49fa861d898806482cddd501e7f46d650a Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 21 Mar 2024 22:05:41 +0800 Subject: [PATCH] add motor status monitor --- .settings/language.settings.xml | 4 +- .vscode/c_cpp_properties.json | 87 +++--------------------- pipeline_disinfection_liquid_path_control.launch | 7 +- sdk | 2 +- usrc/hardware.cpp | 24 +++++++ usrc/hardware.hpp | 5 ++ usrc/main.cpp | 20 ++++++ 7 files changed, 66 insertions(+), 83 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 99332dc..4ca3985 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6a8db16..bea4761 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -3,88 +3,21 @@ { "name": "app", "includePath": [ - "Drivers/STM32F4xx_HAL_Driver/Inc", - "Drivers/CMSIS/Include", - "./", "./Core/Inc", - "Drivers/CMSIS/Device/ST/STM32F4xx/Include/" + "./Drivers/STM32F4xx_HAL_Driver/Inc", + "./Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "./Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "./Drivers/CMSIS/Include", + "C:/ST/STM32CubeIDE_1.13.0/STM32CubeIDE/plugins/com.st.stm32cube.ide.mcu.externaltools.gnu-tools-for-stm32.11.3.rel1.win32_1.1.0.202305231506/tools/arm-none-eabi/include", + "./usrc/", + "./" ], "defines": [ + "DEBUG", "USE_HAL_DRIVER", - "STM32F407xx", - "__CC_ARM", - "__arm__", - "__align(x)=", - "__ALIGNOF__(x)=", - "__alignof__(x)=", - "__asm(x)=", - "__forceinline=", - "__restrict=", - "__global_reg(n)=", - "__inline=", - "__int64=long long", - "__INTADDR__(expr)=0", - "__irq=", - "__packed=", - "__pure=", - "__smc(n)=", - "__svc(n)=", - "__svc_indirect(n)=", - "__svc_indirect_r7(n)=", - "__value_in_regs=", - "__weak=", - "__writeonly=", - "__declspec(x)=", - "__attribute__(x)=", - "__nonnull__(x)=", - "__register=", - "__breakpoint(x)=", - "__cdp(x,y,z)=", - "__clrex()=", - "__clz(x)=0U", - "__current_pc()=0U", - "__current_sp()=0U", - "__disable_fiq()=", - "__disable_irq()=", - "__dmb(x)=", - "__dsb(x)=", - "__enable_fiq()=", - "__enable_irq()=", - "__fabs(x)=0.0", - "__fabsf(x)=0.0f", - "__force_loads()=", - "__force_stores()=", - "__isb(x)=", - "__ldrex(x)=0U", - "__ldrexd(x)=0U", - "__ldrt(x)=0U", - "__memory_changed()=", - "__nop()=", - "__pld(...)=", - "__pli(...)=", - "__qadd(x,y)=0", - "__qdbl(x)=0", - "__qsub(x,y)=0", - "__rbit(x)=0U", - "__rev(x)=0U", - "__return_address()=0U", - "__ror(x,y)=0U", - "__schedule_barrier()=", - "__semihost(x,y)=0", - "__sev()=", - "__sqrt(x)=0.0", - "__sqrtf(x)=0.0f", - "__ssat(x,y)=0", - "__strex(x,y)=0U", - "__strexd(x,y)=0", - "__strt(x,y)=", - "__swp(x,y)=0U", - "__usat(x,y)=0U", - "__wfe()=", - "__wfi()=", - "__yield()=", - "__vfp_status(x,y)=0" + "STM32F407xx" ], + "compilerPath": "C:/ST/STM32CubeIDE_1.13.0/STM32CubeIDE/plugins/com.st.stm32cube.ide.mcu.externaltools.gnu-tools-for-stm32.11.3.rel1.win32_1.1.0.202305231506/tools/bin/arm-none-eabi-g++.exe", "intelliSenseMode": "${default}" } ], diff --git a/pipeline_disinfection_liquid_path_control.launch b/pipeline_disinfection_liquid_path_control.launch index 1915d2c..05fb83b 100644 --- a/pipeline_disinfection_liquid_path_control.launch +++ b/pipeline_disinfection_liquid_path_control.launch @@ -34,10 +34,11 @@ - + + @@ -79,9 +80,9 @@ - + - + diff --git a/sdk b/sdk index 5751b82..15dc8ec 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 5751b8278aab13053c715341d3c1fde48f770280 +Subproject commit 15dc8ec2ad5398c9f0becc6336285f061f5f6da8 diff --git a/usrc/hardware.cpp b/usrc/hardware.cpp index 2032a75..a1ecbb0 100644 --- a/usrc/hardware.cpp +++ b/usrc/hardware.cpp @@ -104,12 +104,22 @@ void Hardware::initialize(int deviceId) { m_motor1.initialize(&cfg); int32_t chipv = m_motor1.readChipVERSION(); ZLOGI(TAG, "m_motor1:%lx", chipv); + + while (!m_motor1.ping()) { + ZLOGI(TAG, "waitting for motor1 become ready"); + HAL_Delay(100); + } + m_motor1.reInitialize(); + m_motor1.setIHOLD_IRUN(1, 20, 0); m_motor1.setMotorShaft(true); m_motor1.setAcceleration(300000); m_motor1.setDeceleration(300000); // m_motor1.rotate(1000000); + + auto gstate = m_motor1.getGState(); + ZLOGI(TAG, "motor1: reset:%d drv_err:%d uv_cp:%d", gstate.reset, gstate.drv_err, gstate.uv_cp); } { @@ -118,11 +128,22 @@ void Hardware::initialize(int deviceId) { m_motor2.initialize(&cfg); int32_t chipv = m_motor2.readChipVERSION(); ZLOGI(TAG, "m_motor2:%lx", chipv); + + while (!m_motor2.ping()) { + ZLOGI(TAG, "waitting for motor2 become ready"); + HAL_Delay(100); + } + + m_motor2.reInitialize(); + m_motor2.setIHOLD_IRUN(1, 20, 0); // 5W m_motor2.setMotorShaft(true); m_motor2.setAcceleration(300000); m_motor2.setDeceleration(300000); + + auto gstate = m_motor2.getGState(); + ZLOGI(TAG, "motor2: reset:%d drv_err:%d uv_cp:%d", gstate.reset, gstate.drv_err, gstate.uv_cp); // m_motor2.rotate(1000000); } @@ -377,3 +398,6 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int return 0; } void Hardware::loop() {} + +TMC5130 *Hardware::getMotor1() { return &m_motor1; } +TMC5130 *Hardware::getMotor2() { return &m_motor2; } \ No newline at end of file diff --git a/usrc/hardware.hpp b/usrc/hardware.hpp index be3a8dc..65e0302 100644 --- a/usrc/hardware.hpp +++ b/usrc/hardware.hpp @@ -1,4 +1,6 @@ #include +#include "sdk\components\tmc\ic\ztmc4361A.hpp" +#include "sdk\components\tmc\ic\ztmc5130.hpp" namespace iflytop { class Hardware { @@ -10,6 +12,9 @@ class Hardware { void initialize(int deviceId); int32_t process_rx_packet(from_where_t fromwhere, uint8_t *packet, int32_t len, uint8_t *receipt, int32_t &receiptsize, bool &matching); void loop(); + + TMC5130 *getMotor1(); + TMC5130 *getMotor2(); }; } // namespace iflytop diff --git a/usrc/main.cpp b/usrc/main.cpp index 41c5549..5c7081c 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -148,6 +148,26 @@ void Main::run() { m_canReceiver.init(cfg); m_canReceiver.registerListener(this); + ZHALCORE::getInstance()->regPeriodJob( + [this](ZHALCORE::Context &context) { // + auto *m1 = m_hardware.getMotor1(); + auto *m2 = m_hardware.getMotor2(); + + DevStatusReg_t sreg1 = m1->getDevStatus(); + GState_t gstate1 = m1->getGState(); + DevStatusReg_t sreg2 = m2->getDevStatus(); + GState_t gstate2 = m2->getGState(); + + // ZLOGI(TAG, "m1: stst:%d olb:%d ola:%d s2gb:%d s2ga:%d otpw:%d ot:%d stallguard:%d cs_actual:%d fsactive:%d sg_result:%d", sreg1.stst, sreg1.olb, + // sreg1.ola, sreg1.s2gb, sreg1.s2ga, sreg1.otpw, sreg1.ot, sreg1.stallguard, sreg1.cs_actual, sreg1.fsactive, sreg1.sg_result); + + ZLOGI(TAG, "m2: stst:%d olb:%d ola:%d s2gb:%d s2ga:%d otpw:%d ot:%d stallguard:%d cs_actual:%d fsactive:%d sg_result:%d", sreg2.stst, sreg2.olb, + sreg2.ola, sreg2.s2gb, sreg2.s2ga, sreg2.otpw, sreg2.ot, sreg2.stallguard, sreg2.cs_actual, sreg2.fsactive, sreg2.sg_result); + ZLOGI(TAG, "m2: reset:%d drv_err:%d uv_cp:%d", gstate2.reset, gstate2.drv_err, gstate2.uv_cp); + + }, + 1000); + ZLOGI(TAG, "init done"); while (1) { ZHALCORE::getInstance()->loop();