/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // #include #include #include #include #include #include #include #include #include using namespace mrpt; using namespace mrpt::comms; using namespace mrpt::hwdrivers; using namespace mrpt::serialization; using namespace std::literals; CServoeNeck::CServoeNeck() : m_usbSerialNumber("eNeck001"), m_PrevAngles(0) { m_offsets.resize(3, 0); } CServoeNeck::~CServoeNeck() = default; bool CServoeNeck::queryFirmwareVersion(std::string& out_firmwareVersion) { try { mrpt::serialization::CMessage msg, msgRx; // Try to connect to the device: if (!checkConnectionAndConnect()) return false; msg.type = 0x10; archiveFrom(*this).sendMessage(msg); if (archiveFrom(*this).receiveMessage(msgRx)) { msgRx.getContentAsString(out_firmwareVersion); std::this_thread::sleep_for(200ms); return true; } else return false; } catch (...) { Close(); return false; } } // end-queryFirmwareVersion /*------------------------------------------------------------- angle2RegValue -------------------------------------------------------------*/ unsigned int CServoeNeck::angle2RegValue(const double angle /* rad */) { const uint16_t reg = 1250 + (1000 / M_PI) * (angle - M_PI * 0.5); // equation: v = s*(a-a0) // + v0 where s = 450/pi; // a0 = 0 and v0 = 750 // cout << "Reg: " << reg << endl; return (reg); // return ( (uint16_t)( (m_MaxValue/20)*(-2*angle/M_PI+1.5) ) ); // // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750 // return ( (uint16_t)( (m_MaxValue/20)*(2*angle/M_PI+1.5) ) ); // // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750 // return ( (uint16_t)( (900/M_PI)*nangle + 750 ) ); // equation: // v // = // s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750 } // end-angle2RegValue /*------------------------------------------------------------- regValue2angle -------------------------------------------------------------*/ double CServoeNeck::regValue2angle(const uint16_t value) { double angle = M_PI * 0.5 + (M_PI / 1000) * (value - 1250); return angle; // return ( -M_PI*0.5*(20*value/m_MaxValue-1.5) ); // // equation: // angle // = // (pi/2)*(20*OCR/ICR-1.5) // return ( -M_PI*0.25+(M_PI/1800)*(value-1050) ); // // equation: // angle // = // (pi/2)*(20*OCR/ICR-1.5) // return ( M_PI*0.5*(20*value/m_MaxValue-1.5) ); // // equation: // angle // = // (pi/2)*(20*OCR/ICR-1.5) // return ( (value-750)/(900/M_PI) ); } // end-regValue2angle /*------------------------------------------------------------- setRegisterValue -------------------------------------------------------------*/ bool CServoeNeck::setRegisterValue(const uint16_t value, const uint8_t servo, bool fast) { try { if (!isOpen()) return false; mrpt::serialization::CMessage msg, msgRx; // Send cmd for setting the value of the register: // ------------------------------------------------ if (fast) msg.type = 0x15; else msg.type = 0x11; msg.content.resize(3); msg.content[2] = (uint8_t)value; // Low byte msg.content[1] = (uint8_t)(value >> 8); // High byte msg.content[0] = servo; // Servo number archiveFrom(*this).sendMessage(msg); if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error std::this_thread::sleep_for(200ms); return true; } catch (...) { // Error opening device: Close(); return false; } } // end-setRegisterValue /*------------------------------------------------------------- setRegisterValue -------------------------------------------------------------*/ bool CServoeNeck::setRegisterValueAndSpeed( const uint16_t value, const uint8_t servo, const uint16_t speed) { try { if (!isOpen()) return false; mrpt::serialization::CMessage msg, msgRx; // Send cmd for setting the value of the register: // ------------------------------------------------ msg.type = 0x16; msg.content.resize(5); msg.content[4] = (uint8_t)speed; // Low byte of the speed of the servo msg.content[3] = (uint8_t)(speed >> 8); // High byte of the speed of the servo msg.content[2] = (uint8_t)value; // Low byte msg.content[1] = (uint8_t)(value >> 8); // High byte msg.content[0] = servo; // Servo number archiveFrom(*this).sendMessage(msg); if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error std::this_thread::sleep_for(200ms); return true; } catch (...) { // Error opening device: Close(); return false; } } // end-setRegisterValue /*------------------------------------------------------------- getRegisterValue -------------------------------------------------------------*/ bool CServoeNeck::getRegisterValue(uint16_t& value, const uint8_t servo) { try { if (!isOpen()) return false; mrpt::serialization::CMessage msg, msgRx; // Send cmd for obtaining the value of the OCR1A register: // -------------------------------------------------------- msg.type = 0x12; msg.content.resize(1); msg.content[0] = servo; archiveFrom(*this).sendMessage(msg); if (archiveFrom(*this).receiveMessage(msgRx)) { if (msgRx.content.size() != 2) return false; value = (msgRx.content[0] << 8) + msgRx.content[1]; return true; } else return false; return true; } catch (...) { // Error opening device: Close(); return false; } } // end-getRegisterValue /*------------------------------------------------------------- getCurrentAngle -------------------------------------------------------------*/ bool CServoeNeck::getCurrentAngle(double& angle, const uint8_t servo) { uint16_t value; if (getRegisterValue(value, servo)) { angle = regValue2angle(value); return true; } else return false; } // end-getCurrentAngle /*------------------------------------------------------------- setAngle -------------------------------------------------------------*/ bool CServoeNeck::setAngle(double angle, const uint8_t servo, bool fast) { // double nangle = -angle; // if( nangle < -m_TruncateFactor*M_PI/2 ) nangle = // -m_TruncateFactor*M_PI/2; // if( nangle > m_TruncateFactor*M_PI/2 ) nangle = // m_TruncateFactor*M_PI/2; // unsigned int reg = angle2RegValue( nangle ); // std::cout << "Angle: " << RAD2DEG( nangle ) << " - Reg: " << reg << // std::endl; // return setRegisterValue( reg, servo ); if (angle < -m_TruncateFactor * M_PI / 2) angle = -m_TruncateFactor * M_PI / 2; if (angle > m_TruncateFactor * M_PI / 2) angle = m_TruncateFactor * M_PI / 2; unsigned int reg = angle2RegValue(m_offsets[servo] + angle); return setRegisterValue(reg, servo, fast); } // end-getCurrentAngle /*------------------------------------------------------------- setAngleAndSpeed -------------------------------------------------------------*/ bool CServoeNeck::setAngleAndSpeed(double angle, const uint8_t servo, const uint8_t speed) { // speed in the range 15/s-250/s if (angle < -m_TruncateFactor * M_PI / 2) angle = -m_TruncateFactor * M_PI / 2; if (angle > m_TruncateFactor * M_PI / 2) angle = m_TruncateFactor * M_PI / 2; unsigned int reg = angle2RegValue(m_offsets[servo] + angle); uint8_t thisSpeed = speed < 15 ? 15 : speed > 250 ? 250 : speed; auto delSpeed = uint16_t(0.25 * 1000000 / (500 + 1000 * (thisSpeed / 180.0f - 0.5))); // cout << "Speed: " << int(speed) << " -> " << delSpeed << endl; // std::cout << "Angle: " << RAD2DEG( angle ) << " - Reg: " << reg << // std::endl; return setRegisterValueAndSpeed(reg, servo, delSpeed); } // end-getCurrentAngle /*------------------------------------------------------------- setAngleWithFilter -------------------------------------------------------------*/ bool CServoeNeck::setAngleWithFilter(double angle, const uint8_t servo, bool fast) { double nangle = 0; if (m_PrevAngles.size() == m_NumPrevAngles && m_NumPrevAngles != 0) // If the deque is full populated m_PrevAngles.erase(m_PrevAngles.begin()); // Erase the first angle of the deque m_PrevAngles.push_back(angle); // Push back the new angle std::deque::iterator it; for (it = m_PrevAngles.begin(); it != m_PrevAngles.end(); ++it) // Sum up all the elements in the deque nangle += *it; nangle /= m_PrevAngles.size(); // Mean angle return (setAngle(nangle, servo, fast)); } /*------------------------------------------------------------- disableServo -------------------------------------------------------------*/ bool CServoeNeck::disableServo(const uint8_t servo) { try { if (!isOpen()) return false; mrpt::serialization::CMessage msg, msgRx; // Send cmd for disabling servo: // ---------------------------- msg.type = 0x13; msg.content.resize(1); msg.content[0] = servo; // Servo number archiveFrom(*this).sendMessage(msg); if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error return true; } catch (...) { // Error opening device: Close(); return false; } } // end-getCurrentAngle /*------------------------------------------------------------- enableServo -------------------------------------------------------------*/ bool CServoeNeck::enableServo(const uint8_t servo) { try { if (!isOpen()) return false; mrpt::serialization::CMessage msg, msgRx; // Send cmd for enabling the servo: // -------------------------------- msg.type = 0x14; msg.content.resize(1); msg.content[0] = servo; // Servo number archiveFrom(*this).sendMessage(msg); if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error return true; } catch (...) { // Error opening device: Close(); return false; } } // end-getCurrentAngle /*------------------------------------------------------------- Center -------------------------------------------------------------*/ bool CServoeNeck::center(const uint8_t servo) { unsigned int value = angle2RegValue(m_offsets[servo]); return setRegisterValue(value, servo); } // end-Center /*------------------------------------------------------------- checkConnectionAndConnect -------------------------------------------------------------*/ bool CServoeNeck::checkConnectionAndConnect() { if (isOpen()) return true; try { OpenBySerialNumber(m_usbSerialNumber); std::this_thread::sleep_for(10ms); Purge(); std::this_thread::sleep_for(10ms); SetLatencyTimer(1); SetTimeouts(300, 100); return true; } catch (...) { // Error opening device: Close(); return false; } } // end-checkConnectionAndConnect /*------------------------------------------------------------- setOffsets -------------------------------------------------------------*/ void CServoeNeck::setOffsets(float offset0, float offset1, float offset2) { m_offsets.resize(3); m_offsets[0] = offset0; m_offsets[1] = offset1; m_offsets[2] = offset2; }