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();