diff --git a/Inc/motor_drivers.hpp b/Inc/motor_drivers.hpp index 1b07fd3dbe708105b3d785fd645bedc6e9f92535..0d67a214268ae4e45bc0dd88f326aa80225f6e5a 100644 --- a/Inc/motor_drivers.hpp +++ b/Inc/motor_drivers.hpp @@ -81,9 +81,10 @@ namespace MotorDrivers { virtual SocketFrame socket_Read() = 0; SocketFrame frame; std::deque data_buffer_; - std::atomic mutation {0}; - + virtual int16_t socket_ReadMutation(uint8_t index) = 0; + protected: + int16_t mutation[16] = {0}; virtual void socket_DetectMutation() = 0; }; @@ -180,15 +181,12 @@ 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); log logM2006 = log("M2006", "./logs/"); - private: - std::string crtTimestamp_; - std::string prvTimestamp_; }; diff --git a/Inc/motor_sockets/can_socket.hpp b/Inc/motor_sockets/can_socket.hpp index 0ed5a62d4b0975e2940b120ee0b1fa0e834b3ef1..ab087a6d82f187a90339dfbd1224faeb26bec062 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 a50b6abee4e2fd82238ddf91bd0c2ff11ab2a2c5..4701258e7ab5491bf8e1bab01a502791b2e598e9 100644 --- a/Src/can_socket.cpp +++ b/Src/can_socket.cpp @@ -38,30 +38,48 @@ bool MotorDrivers::CANSocket::socket_Init(){ } } +int16_t MotorDrivers::CANSocket::socket_ReadMutation(uint8_t index){ + int16_t mutation_value; + socket_PauseThread(); + mutation_value = mutation[index]; + mutation[index] = 0; + socket_ResumeThread(); + return mutation_value; + +} + 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(); + 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); - 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)); + 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] + && 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[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 if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] <= prv_prv_frame.data[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[i])); } + else mutation[i+8] = 0; } + } void MotorDrivers::CANSocket::socket_ReadingThread(){ @@ -76,13 +94,14 @@ 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); 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); @@ -107,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 97f50f5abbbe926b8de38544ff81d1946601d8e2..dcfe58059f2ecb8213738f54551a93958e0d68f8 100644 --- a/Src/motor_DJIM2006.cpp +++ b/Src/motor_DJIM2006.cpp @@ -1,5 +1,7 @@ #include "motor_drivers.hpp" + + 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 +11,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,56 +23,28 @@ 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; + crtOriginCycles_ += socket_->socket_ReadMutation(0); - - - 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));//cycle 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 09537b4ef9a5068a00da7def5c3fd879f2a09858..da3ee477b6c11abc57f8f9fe870ad4b036baa34b 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; }