From c2b04703cc1c42e21800a80475a68531feb42f98 Mon Sep 17 00:00:00 2001 From: moonfeather Date: Tue, 14 May 2024 09:54:52 +0800 Subject: [PATCH 1/9] introduce mutation mechanism --- Inc/motor_drivers.hpp | 5 ++--- Src/can_socket.cpp | 14 +++++++------ Src/motor_DJIM2006.cpp | 45 ++++++++++++------------------------------ Src/motor_base.cpp | 6 +++--- 4 files changed, 26 insertions(+), 44 deletions(-) diff --git a/Inc/motor_drivers.hpp b/Inc/motor_drivers.hpp index 1b07fd3..01b8a79 100644 --- a/Inc/motor_drivers.hpp +++ b/Inc/motor_drivers.hpp @@ -171,7 +171,7 @@ namespace MotorDrivers { class MotorM2006 : public Motor{ public: - using Motor::Motor; + explicit MotorM2006(HWSocket *socket); void motor_Update() override; bool motor_getHWInfo() override; @@ -187,8 +187,7 @@ namespace MotorDrivers { log logM2006 = log("M2006", "./logs/"); private: - std::string crtTimestamp_; - std::string prvTimestamp_; + void motor_DetectMutation(); }; diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index a50b6ab..6d5fe60 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -39,10 +39,7 @@ bool MotorDrivers::CANSocket::socket_Init(){ } void MotorDrivers::CANSocket::socket_DetectMutation(){ - if(data_buffer_.size() < 2) { - // 如果数据缓冲区中的元素少于2，那么我们无法进行突变检测 - return; - } + if(data_buffer_.size() < 2) return; for(size_t i = 0; i < 8; i++){ SocketFrame prv_socket_frame = data_buffer_.back(); @@ -53,14 +50,18 @@ void MotorDrivers::CANSocket::socket_DetectMutation(){ can_frame prv_prv_frame = data_buffer_.back().can_frame_; data_buffer_.push_back(prv_socket_frame); - if((crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] < prv_prv_frame.data[i]) ){ + if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] < prv_prv_frame.data[i] ){ mutation |= (1 << (i + 8)); mutation |= (1 << (i + 8)); } - else if((crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] > prv_prv_frame.data[i])){ + else if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] > prv_prv_frame.data[i]){ mutation &= ~(1 << (i + 8)); mutation |= (1 << i); } + else{ + mutation &= ~(1 << (i + 8)); + mutation &= ~(1 << i); + } } } @@ -83,6 +84,7 @@ void MotorDrivers::CANSocket::socket_ReadingThread(){ if(retval == -1) perror("select error"); else if(retval) { read(socket_, &frame.can_frame_, sizeof(can_frame)); + socket_DetectMutation(); frame.timestamp_ = getTimeStamp(); data_buffer_.push_back(frame); diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index 97f50f5..39755fe 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -1,5 +1,17 @@ #include "motor_drivers.hpp" +void MotorDrivers::MotorM2006::motor_DetectMutation(){ + if(socket_->mutation && socket_->mutation & 128){ + if(socket_->mutation & 32768) crtOriginCycles_++; + else crtOriginCycles_--; + } +} + +MotorDrivers::MotorM2006::MotorM2006(HWSocket* socket): Motor(socket){ + std::thread mutationThread(&MotorDrivers::MotorM2006::motor_DetectMutation, this); + mutationThread.detach(); +}; + void MotorDrivers::MotorM2006::M2006Log(){ logM2006.logWrite(std::to_string(std::chrono::duration_cast (std::chrono::system_clock::now().time_since_epoch()).count()) + " - logM2006::"); @@ -9,7 +21,6 @@ void MotorDrivers::MotorM2006::M2006Log(){ void MotorDrivers::MotorM2006::motor_Update(){ - if(prvTimestamp_.empty()) prvTimestamp_ = getTimeStamp(); if(motor_getHWInfo()){ pid_pos.target = targetPos_; pid_speed.target = ((pid_pos.pidCalc(crtPos_) - crtPos_) * 36 / pid_pos.dtime * 10); @@ -22,39 +33,11 @@ void MotorDrivers::MotorM2006::motor_Update(){ bool MotorDrivers::MotorM2006::motor_getHWInfo(){ can_frame frame = socket_->frame.can_frame_; //crt frame - crtTimestamp_ = socket_->frame.timestamp_; + uint16_t p_int = static_cast(frame.data[0] << 8) | frame.data[1]; int16_t v_int = static_cast(frame.data[2] << 8) | frame.data[3]; int16_t t_int = static_cast(frame.data[4] << 8) | frame.data[5]; - auto nFrame = socket_->socket_Read(); - auto nTimestamp = nFrame.timestamp_; - int16_t np_int_prev[2]; int16_t cycle_prev = 0; - np_int_prev[0] = p_int; - - while(nTimestamp >= prvTimestamp_){ - int16_t nv_int = static_cast(nFrame.can_frame_.data[2] << 8) - | nFrame.can_frame_.data[3]; - - np_int_prev[1] = - static_cast(nFrame.can_frame_.data[0] << 8) | nFrame.can_frame_.data[1]; - if(np_int_prev[1] < np_int_prev[0] && nv_int >= 0){ - cycle_prev++; - } - else if(np_int_prev[1] > np_int_prev[0] && nv_int <= 0){ - cycle_prev--; - } - - np_int_prev[0] = np_int_prev[1]; - - nFrame = socket_->socket_Read(); - nTimestamp = nFrame.timestamp_; - } - - crtOriginCycles_ += cycle_prev; - - - if(crtOriginCycles_ >= 36){ crtCycles_++; crtOriginCycles_ -= 36; @@ -70,8 +53,6 @@ bool MotorDrivers::MotorM2006::motor_getHWInfo(){ crtVel_ = static_cast(v_int / 100.0);//rpm / 100.0 crtTorque_ = static_cast(t_int * 5.0 / 16384.0);//Nm - prvTimestamp_ = crtTimestamp_; - M2006Log(); return true; diff --git a/Src/motor_base.cpp b/Src/motor_base.cpp index 09537b4..da3ee47 100644 --- a/Src/motor_base.cpp +++ b/Src/motor_base.cpp @@ -8,9 +8,9 @@ void MotorDrivers::Motor::send_Empty(){ } bool MotorDrivers::Motor::motor_Init(){ - if(socket_->socket_Init()){ - return true; + if(!socket_->socket_Init()){ + return false; } - return false; + return true; } -- Gitee From 916be03bfa420a371f6a9a14e815a70584c6918a Mon Sep 17 00:00:00 2001 From: moonfeather Date: Tue, 14 May 2024 10:20:35 +0800 Subject: [PATCH 2/9] refactor: Update MotorM2006 class to properly handle mutation detection --- Inc/motor_drivers.hpp | 4 +++- Src/motor_DJIM2006.cpp | 21 ++++++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/Inc/motor_drivers.hpp b/Inc/motor_drivers.hpp index 01b8a79..a10e59a 100644 --- a/Inc/motor_drivers.hpp +++ b/Inc/motor_drivers.hpp @@ -171,7 +171,8 @@ namespace MotorDrivers { class MotorM2006 : public Motor{ public: - explicit MotorM2006(HWSocket *socket); + explicit MotorM2006(HWSocket* socket); + ~MotorM2006() override; void motor_Update() override; bool motor_getHWInfo() override; @@ -188,6 +189,7 @@ namespace MotorDrivers { private: void motor_DetectMutation(); + std::thread mutationThread; }; diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index 39755fe..b052e53 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -1,16 +1,23 @@ #include "motor_drivers.hpp" void MotorDrivers::MotorM2006::motor_DetectMutation(){ - if(socket_->mutation && socket_->mutation & 128){ - if(socket_->mutation & 32768) crtOriginCycles_++; - else crtOriginCycles_--; + while(true){ + if(socket_->mutation && socket_->mutation & 128){ + if(socket_->mutation & 32768) crtOriginCycles_++; + else crtOriginCycles_--; + } } } -MotorDrivers::MotorM2006::MotorM2006(HWSocket* socket): Motor(socket){ - std::thread mutationThread(&MotorDrivers::MotorM2006::motor_DetectMutation, this); - mutationThread.detach(); -}; +MotorDrivers::MotorM2006::MotorM2006(HWSocket* socket): + Motor(socket), + mutationThread(&MotorDrivers::MotorM2006::motor_DetectMutation, this){} + +MotorDrivers::MotorM2006::~MotorM2006(){ + if (mutationThread.joinable()){ + mutationThread.join(); + } +} void MotorDrivers::MotorM2006::M2006Log(){ logM2006.logWrite(std::to_string(std::chrono::duration_cast -- Gitee From a281bccbd447326f0336289fc583de041a91136e Mon Sep 17 00:00:00 2001 From: moonfeather Date: Tue, 14 May 2024 02:47:43 +0000 Subject: [PATCH 3/9] fix bugs --- Src/can_socket.cpp | 12 ++++++++---- Src/motor_DJIM2006.cpp | 2 ++ 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index 6d5fe60..ebab697 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -41,7 +41,7 @@ bool MotorDrivers::CANSocket::socket_Init(){ void MotorDrivers::CANSocket::socket_DetectMutation(){ if(data_buffer_.size() < 2) return; - for(size_t i = 0; i < 8; i++){ + for(size_t i = 0; i < 2; i++){ SocketFrame prv_socket_frame = data_buffer_.back(); can_frame crt_frame = frame.can_frame_; @@ -50,13 +50,17 @@ void MotorDrivers::CANSocket::socket_DetectMutation(){ can_frame prv_prv_frame = data_buffer_.back().can_frame_; data_buffer_.push_back(prv_socket_frame); - if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] < prv_prv_frame.data[i] ){ - mutation |= (1 << (i + 8)); + if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] > prv_prv_frame.data[i] ){ + mutation |= (1 << i); mutation |= (1 << (i + 8)); + logCAN.logWrite("Mutation Detected: Data[" + + std::to_string(i) + "] ++" + std::to_string(mutation)); } - else if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] > prv_prv_frame.data[i]){ + else if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] < prv_prv_frame.data[i]){ mutation &= ~(1 << (i + 8)); mutation |= (1 << i); + logCAN.logWrite("Mutation Detected: Data[" + + std::to_string(i) + "] --" + std::to_string(mutation)); } else{ mutation &= ~(1 << (i + 8)); diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index b052e53..4704057 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -2,7 +2,9 @@ void MotorDrivers::MotorM2006::motor_DetectMutation(){ while(true){ + logM2006.logWrite(std::to_string(socket_->mutation)); if(socket_->mutation && socket_->mutation & 128){ + if(socket_->mutation & 32768) crtOriginCycles_++; else crtOriginCycles_--; } -- Gitee From 58e35a57621667b88b0bffe1a962a4594db1f12f Mon Sep 17 00:00:00 2001 From: moonfeather Date: Tue, 14 May 2024 03:45:14 +0000 Subject: [PATCH 4/9] correct mutations --- Src/can_socket.cpp | 6 ++++-- Src/motor_DJIM2006.cpp | 12 +++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index ebab697..fb5509e 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -50,13 +50,15 @@ void MotorDrivers::CANSocket::socket_DetectMutation(){ can_frame prv_prv_frame = data_buffer_.back().can_frame_; data_buffer_.push_back(prv_socket_frame); - if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] > prv_prv_frame.data[i] ){ + if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] >= prv_prv_frame.data[i] + && !(mutation & (1 << i ) ) && crt_frame.data[i] - prv_frame.data[i] < -1){ mutation |= (1 << i); mutation |= (1 << (i + 8)); logCAN.logWrite("Mutation Detected: Data[" + std::to_string(i) + "] ++" + std::to_string(mutation)); } - else if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] < prv_prv_frame.data[i]){ + else if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] <= prv_prv_frame.data[i] + && !(mutation & (1 << i) ) && crt_frame.data[i] - prv_frame.data[i] > 1){ mutation &= ~(1 << (i + 8)); mutation |= (1 << i); logCAN.logWrite("Mutation Detected: Data[" + diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index 4704057..8b8cf11 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -2,10 +2,8 @@ void MotorDrivers::MotorM2006::motor_DetectMutation(){ while(true){ - logM2006.logWrite(std::to_string(socket_->mutation)); - if(socket_->mutation && socket_->mutation & 128){ - - if(socket_->mutation & 32768) crtOriginCycles_++; + if(socket_->mutation && socket_->mutation & 1){ + if(socket_->mutation & 256) crtOriginCycles_++; else crtOriginCycles_--; } } @@ -47,18 +45,18 @@ bool MotorDrivers::MotorM2006::motor_getHWInfo(){ int16_t v_int = static_cast(frame.data[2] << 8) | frame.data[3]; int16_t t_int = static_cast(frame.data[4] << 8) | frame.data[5]; - if(crtOriginCycles_ >= 36){ + while(crtOriginCycles_ >= 36){ crtCycles_++; crtOriginCycles_ -= 36; } - if(crtOriginCycles_ <= -36){ + while(crtOriginCycles_ <= -36){ crtCycles_--; crtOriginCycles_ += 36; } crtOriginPos_ = static_cast(p_int / 8191.0 * 360.0);//degree crtPos_ = static_cast(crtCycles_ + - (static_cast(crtOriginCycles_) / 36.0)) / 10.0;//cycle + (static_cast(crtOriginCycles_) / 36.0)) / 1000.0;//cycle crtVel_ = static_cast(v_int / 100.0);//rpm / 100.0 crtTorque_ = static_cast(t_int * 5.0 / 16384.0);//Nm -- Gitee From 25b8193bb8edffc341d5e7f50c446387d8975aed Mon Sep 17 00:00:00 2001 From: moonfeather Date: Tue, 14 May 2024 12:31:59 +0800 Subject: [PATCH 5/9] introduce lock and cv to mutation function --- Inc/motor_drivers.hpp | 4 +++- Src/can_socket.cpp | 31 ++++++++++++++++++++++--------- Src/motor_DJIM2006.cpp | 5 +++-- 3 files changed, 28 insertions(+), 12 deletions(-) diff --git a/Inc/motor_drivers.hpp b/Inc/motor_drivers.hpp index a10e59a..b5e94f7 100644 --- a/Inc/motor_drivers.hpp +++ b/Inc/motor_drivers.hpp @@ -81,7 +81,9 @@ namespace MotorDrivers { virtual SocketFrame socket_Read() = 0; SocketFrame frame; std::deque data_buffer_; - std::atomic mutation {0}; + std::atomic mutation {0}; + std::condition_variable mutation_cv; + std::mutex mutation_mtx; protected: virtual void socket_DetectMutation() = 0; diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index fb5509e..25315a2 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -39,36 +39,49 @@ bool MotorDrivers::CANSocket::socket_Init(){ } void MotorDrivers::CANSocket::socket_DetectMutation(){ + std::lock_guard lock(mutation_mtx); if(data_buffer_.size() < 2) return; + bool changed = false; - for(size_t i = 0; i < 2; i++){ - SocketFrame prv_socket_frame = data_buffer_.back(); + SocketFrame prv_socket_frame = data_buffer_.back(); - can_frame crt_frame = frame.can_frame_; - can_frame prv_frame = prv_socket_frame.can_frame_; - data_buffer_.pop_back(); - can_frame prv_prv_frame = data_buffer_.back().can_frame_; - data_buffer_.push_back(prv_socket_frame); + can_frame crt_frame = frame.can_frame_; + can_frame prv_frame = prv_socket_frame.can_frame_; + data_buffer_.pop_back(); + can_frame prv_prv_frame = data_buffer_.back().can_frame_; + data_buffer_.push_back(prv_socket_frame); + for(size_t i = 0; i < 8; i++){ + if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] >= prv_prv_frame.data[i] - && !(mutation & (1 << i ) ) && crt_frame.data[i] - prv_frame.data[i] < -1){ + && !(mutation & (1 << i ) ) && crt_frame.data[i] - prv_frame.data[i] < -1 + && mutation & (1 << (i + 16)) + ){ mutation |= (1 << i); mutation |= (1 << (i + 8)); logCAN.logWrite("Mutation Detected: Data[" + std::to_string(i) + "] ++" + std::to_string(mutation)); + changed = true; } else if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] <= prv_prv_frame.data[i] - && !(mutation & (1 << i) ) && crt_frame.data[i] - prv_frame.data[i] > 1){ + && !(mutation & (1 << i) ) && crt_frame.data[i] - prv_frame.data[i] > 1 + && mutation & (1 << (i + 16)) + ){ mutation &= ~(1 << (i + 8)); mutation |= (1 << i); logCAN.logWrite("Mutation Detected: Data[" + std::to_string(i) + "] --" + std::to_string(mutation)); + changed = true; } else{ mutation &= ~(1 << (i + 8)); mutation &= ~(1 << i); } } + + if(changed){ + mutation_cv.notify_all(); + } } void MotorDrivers::CANSocket::socket_ReadingThread(){ diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index 8b8cf11..91f395f 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -1,11 +1,12 @@ #include "motor_drivers.hpp" void MotorDrivers::MotorM2006::motor_DetectMutation(){ + socket_->mutation = 65536; while(true){ - if(socket_->mutation && socket_->mutation & 1){ + std::unique_lock lock(socket_->mutation_mtx); + socket_->mutation_cv.wait(lock, [this] { return (socket_->mutation.load()) ^ 65536; }); if(socket_->mutation & 256) crtOriginCycles_++; else crtOriginCycles_--; - } } } -- Gitee From ebadfddefc314f88c64cbe1cb58ef74f28d50fc9 Mon Sep 17 00:00:00 2001 From: moonfeather Date: Tue, 14 May 2024 05:01:04 +0000 Subject: [PATCH 6/9] fix bugs --- Inc/motor_drivers.hpp | 2 +- Src/can_socket.cpp | 4 ++-- Src/motor_DJIM2006.cpp | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/Inc/motor_drivers.hpp b/Inc/motor_drivers.hpp index b5e94f7..101b1c4 100644 --- a/Inc/motor_drivers.hpp +++ b/Inc/motor_drivers.hpp @@ -183,7 +183,7 @@ namespace MotorDrivers { uint16_t p_int_prev; float crtOriginPos_; int16_t crtCycles_; - int16_t crtOriginCycles_; + std::atomic crtOriginCycles_; PID_Type pid_speed = PID_Type(1.0, (float)0.2, 0.1, 300, 5000, 5, 1000); PID_Type pid_pos = PID_Type(2.0, (float)0.1, 0, 2, 5000, 0.1, 1000); diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index 25315a2..0bab67e 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -54,7 +54,7 @@ void MotorDrivers::CANSocket::socket_DetectMutation(){ for(size_t i = 0; i < 8; i++){ if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] >= prv_prv_frame.data[i] - && !(mutation & (1 << i ) ) && crt_frame.data[i] - prv_frame.data[i] < -1 + && !(mutation & (1 << i ) ) && crt_frame.data[i] - prv_frame.data[i] < -3 && mutation & (1 << (i + 16)) ){ mutation |= (1 << i); @@ -64,7 +64,7 @@ void MotorDrivers::CANSocket::socket_DetectMutation(){ changed = true; } else if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] <= prv_prv_frame.data[i] - && !(mutation & (1 << i) ) && crt_frame.data[i] - prv_frame.data[i] > 1 + && !(mutation & (1 << i) ) && crt_frame.data[i] - prv_frame.data[i] > 3 && mutation & (1 << (i + 16)) ){ mutation &= ~(1 << (i + 8)); diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index 91f395f..fa8bf52 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -7,6 +7,7 @@ void MotorDrivers::MotorM2006::motor_DetectMutation(){ socket_->mutation_cv.wait(lock, [this] { return (socket_->mutation.load()) ^ 65536; }); if(socket_->mutation & 256) crtOriginCycles_++; else crtOriginCycles_--; + // logM2006.logWrite("Mutation Detected!!" + socket_->mutation); } } @@ -57,7 +58,7 @@ bool MotorDrivers::MotorM2006::motor_getHWInfo(){ crtOriginPos_ = static_cast(p_int / 8191.0 * 360.0);//degree crtPos_ = static_cast(crtCycles_ + - (static_cast(crtOriginCycles_) / 36.0)) / 1000.0;//cycle + (static_cast(crtOriginCycles_) / 36.0)) / 10.0;//cycle crtVel_ = static_cast(v_int / 100.0);//rpm / 100.0 crtTorque_ = static_cast(t_int * 5.0 / 16384.0);//Nm -- Gitee From d4e01489966433bb0129d2b3899be8d782a6036c Mon Sep 17 00:00:00 2001 From: moonfeather Date: Wed, 15 May 2024 19:26:36 +0800 Subject: [PATCH 7/9] another way of implement. --- Inc/motor_drivers.hpp | 13 +++------- Inc/motor_sockets/can_socket.hpp | 2 ++ Src/can_socket.cpp | 44 +++++++++++++++----------------- Src/motor_DJIM2006.cpp | 21 ++------------- 4 files changed, 29 insertions(+), 51 deletions(-) diff --git a/Inc/motor_drivers.hpp b/Inc/motor_drivers.hpp index 101b1c4..72d9cfa 100644 --- a/Inc/motor_drivers.hpp +++ b/Inc/motor_drivers.hpp @@ -81,11 +81,10 @@ namespace MotorDrivers { virtual SocketFrame socket_Read() = 0; SocketFrame frame; std::deque data_buffer_; - std::atomic mutation {0}; - std::condition_variable mutation_cv; - std::mutex mutation_mtx; - + virtual int16_t socket_ReadMutation(uint8_t index) = 0; + protected: + int16_t mutation[16]; virtual void socket_DetectMutation() = 0; }; @@ -173,8 +172,7 @@ namespace MotorDrivers { class MotorM2006 : public Motor{ public: - explicit MotorM2006(HWSocket* socket); - ~MotorM2006() override; + using Motor::Motor; void motor_Update() override; bool motor_getHWInfo() override; @@ -189,9 +187,6 @@ namespace MotorDrivers { log logM2006 = log("M2006", "./logs/"); - private: - void motor_DetectMutation(); - std::thread mutationThread; }; diff --git a/Inc/motor_sockets/can_socket.hpp b/Inc/motor_sockets/can_socket.hpp index 0ed5a62..ab087a6 100644 --- a/Inc/motor_sockets/can_socket.hpp +++ b/Inc/motor_sockets/can_socket.hpp @@ -11,10 +11,12 @@ namespace MotorDrivers{ bool socket_Init() override; bool socket_Send(uint8_t* frame_data) override; SocketFrame socket_Read() override; + int16_t socket_ReadMutation(uint8_t index) override; protected: void socket_DetectMutation() override; + private: std::string socketName_; int32_t socket_; diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index 0bab67e..ac433ed 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -38,10 +38,18 @@ bool MotorDrivers::CANSocket::socket_Init(){ } } +int16_t MotorDrivers::CANSocket::socket_ReadMutation(uint8_t index){ + int16_t mutation_value; + socket_PauseThread(); + std::unique_lock lock(mtx); + mutation_value = mutation[index]; + socket_ResumeThread(); + return mutation_value; + +} + void MotorDrivers::CANSocket::socket_DetectMutation(){ - std::lock_guard lock(mutation_mtx); if(data_buffer_.size() < 2) return; - bool changed = false; SocketFrame prv_socket_frame = data_buffer_.back(); @@ -54,34 +62,24 @@ void MotorDrivers::CANSocket::socket_DetectMutation(){ for(size_t i = 0; i < 8; i++){ if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] >= prv_prv_frame.data[i] - && !(mutation & (1 << i ) ) && crt_frame.data[i] - prv_frame.data[i] < -3 - && mutation & (1 << (i + 16)) - ){ - mutation |= (1 << i); - mutation |= (1 << (i + 8)); + && crt_frame.data[i] - prv_frame.data[i] < -3 && !mutation[i+8] ) + { + mutation[i] ++; + mutation[i+8] = 1; logCAN.logWrite("Mutation Detected: Data[" + - std::to_string(i) + "] ++" + std::to_string(mutation)); - changed = true; + std::to_string(i) + "] ++" + std::to_string(mutation[i])); } else if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] <= prv_prv_frame.data[i] - && !(mutation & (1 << i) ) && crt_frame.data[i] - prv_frame.data[i] > 3 - && mutation & (1 << (i + 16)) - ){ - mutation &= ~(1 << (i + 8)); - mutation |= (1 << i); + && crt_frame.data[i] - prv_frame.data[i] > 3 && !mutation[i+8]) + { + mutation[i]--; + mutation[i+8] = 1; logCAN.logWrite("Mutation Detected: Data[" + - std::to_string(i) + "] --" + std::to_string(mutation)); - changed = true; - } - else{ - mutation &= ~(1 << (i + 8)); - mutation &= ~(1 << i); + std::to_string(i) + "] --" + std::to_string(mutation[i])); } + else mutation[i+8] = 0; } - if(changed){ - mutation_cv.notify_all(); - } } void MotorDrivers::CANSocket::socket_ReadingThread(){ diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index fa8bf52..60feb18 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -1,25 +1,6 @@ #include "motor_drivers.hpp" -void MotorDrivers::MotorM2006::motor_DetectMutation(){ - socket_->mutation = 65536; - while(true){ - std::unique_lock lock(socket_->mutation_mtx); - socket_->mutation_cv.wait(lock, [this] { return (socket_->mutation.load()) ^ 65536; }); - if(socket_->mutation & 256) crtOriginCycles_++; - else crtOriginCycles_--; - // logM2006.logWrite("Mutation Detected!!" + socket_->mutation); - } -} - -MotorDrivers::MotorM2006::MotorM2006(HWSocket* socket): - Motor(socket), - mutationThread(&MotorDrivers::MotorM2006::motor_DetectMutation, this){} -MotorDrivers::MotorM2006::~MotorM2006(){ - if (mutationThread.joinable()){ - mutationThread.join(); - } -} void MotorDrivers::MotorM2006::M2006Log(){ logM2006.logWrite(std::to_string(std::chrono::duration_cast @@ -47,6 +28,8 @@ bool MotorDrivers::MotorM2006::motor_getHWInfo(){ int16_t v_int = static_cast(frame.data[2] << 8) | frame.data[3]; int16_t t_int = static_cast(frame.data[4] << 8) | frame.data[5]; + crtOriginCycles_ += socket_->socket_ReadMutation(1); + while(crtOriginCycles_ >= 36){ crtCycles_++; crtOriginCycles_ -= 36; -- Gitee From fb6d4be158c93d773badd4058d7ae36e71e7774a Mon Sep 17 00:00:00 2001 From: moonfeather Date: Wed, 15 May 2024 11:47:01 +0000 Subject: [PATCH 8/9] fix bugs --- Inc/motor_drivers.hpp | 2 +- Src/can_socket.cpp | 1 + Src/motor_DJIM2006.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Inc/motor_drivers.hpp b/Inc/motor_drivers.hpp index 72d9cfa..0d67a21 100644 --- a/Inc/motor_drivers.hpp +++ b/Inc/motor_drivers.hpp @@ -84,7 +84,7 @@ namespace MotorDrivers { virtual int16_t socket_ReadMutation(uint8_t index) = 0; protected: - int16_t mutation[16]; + int16_t mutation[16] = {0}; virtual void socket_DetectMutation() = 0; }; diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index ac433ed..18a649b 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -43,6 +43,7 @@ int16_t MotorDrivers::CANSocket::socket_ReadMutation(uint8_t index){ socket_PauseThread(); std::unique_lock lock(mtx); mutation_value = mutation[index]; + mutation[index] = 0; socket_ResumeThread(); return mutation_value; diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index 60feb18..fc5e9dd 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -28,7 +28,7 @@ bool MotorDrivers::MotorM2006::motor_getHWInfo(){ int16_t v_int = static_cast(frame.data[2] << 8) | frame.data[3]; int16_t t_int = static_cast(frame.data[4] << 8) | frame.data[5]; - crtOriginCycles_ += socket_->socket_ReadMutation(1); + crtOriginCycles_ += socket_->socket_ReadMutation(0); while(crtOriginCycles_ >= 36){ crtCycles_++; -- Gitee From 49948f6e85af33a378c1043d549e77c71fb133c3 Mon Sep 17 00:00:00 2001 From: moonfeather Date: Wed, 15 May 2024 12:02:46 +0000 Subject: [PATCH 9/9] fix delay by extra lock statement --- Src/can_socket.cpp | 7 +------ Src/motor_DJIM2006.cpp | 2 +- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/Src/can_socket.cpp b/Src/can_socket.cpp index 18a649b..4701258 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -41,7 +41,6 @@ bool MotorDrivers::CANSocket::socket_Init(){ int16_t MotorDrivers::CANSocket::socket_ReadMutation(uint8_t index){ int16_t mutation_value; socket_PauseThread(); - std::unique_lock lock(mtx); mutation_value = mutation[index]; mutation[index] = 0; socket_ResumeThread(); @@ -95,7 +94,7 @@ void MotorDrivers::CANSocket::socket_ReadingThread(){ FD_ZERO(&readfds); FD_SET(socket_, &readfds); - tv.tv_usec = 1000; + tv.tv_usec = 500; retval = select(socket_ + 1, &readfds, nullptr, nullptr, &tv); @@ -127,24 +126,20 @@ void MotorDrivers::CANSocket::socket_ResumeThread(){ MotorDrivers::SocketFrame MotorDrivers::CANSocket::socket_Read(){ socket_PauseThread(); - std::unique_lock lock(mtx); MotorDrivers::SocketFrame frame = data_buffer_.back(); data_buffer_.pop_back(); - lock.unlock(); socket_ResumeThread(); return frame; } bool MotorDrivers::CANSocket::socket_Send(uint8_t* frame_data){ socket_PauseThread(); - std::unique_lock lock(mtx); std::copy(frame_data, frame_data + frame.can_frame_.len, frame.can_frame_.data); if(write(socket_, &frame.can_frame_, sizeof(can_frame)) < 0){ perror("socket send error"); socket_ResumeThread(); return false; } - lock.unlock(); socket_ResumeThread(); return true; } diff --git a/Src/motor_DJIM2006.cpp b/Src/motor_DJIM2006.cpp index fc5e9dd..dcfe580 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -41,7 +41,7 @@ bool MotorDrivers::MotorM2006::motor_getHWInfo(){ crtOriginPos_ = static_cast(p_int / 8191.0 * 360.0);//degree crtPos_ = static_cast(crtCycles_ + - (static_cast(crtOriginCycles_) / 36.0)) / 10.0;//cycle + (static_cast(crtOriginCycles_) / 36.0));//cycle crtVel_ = static_cast(v_int / 100.0);//rpm / 100.0 crtTorque_ = static_cast(t_int * 5.0 / 16384.0);//Nm -- Gitee