diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..580a096 --- /dev/null +++ b/.clang-format @@ -0,0 +1,246 @@ +--- +Language: Cpp +BasedOnStyle: LLVM +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseColons: false +AlignEscapedNewlines: Right +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowBreakBeforeNoexceptSpecifier: Never +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: MultiLine +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: true +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterExternBlock: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: Always +BreakBeforeBraces: Attach +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 1 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: false +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +KeepEmptyLinesAtEOF: false +LambdaBodyIndentation: Signature +LineEnding: DeriveLF +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PackConstructorInitializers: BinPack +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Right +PPIndentWidth: -1 +QualifierAlignment: Leave +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + InCStyleCasts: false + InConditionalStatements: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: Latest +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE +... + diff --git a/.gitignore b/.gitignore index d167b8b..dcf731e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ build -*.exe \ No newline at end of file +*.exe +.vscode \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b24809..15f8e55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,10 +5,23 @@ project(RMDControl VERSION 0.1) # 定义项目名称 # 设置C标准 set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED True) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED True) - -add_library(RMDControl RMDControl.c) +add_library(RMDControl RMDControl.cpp) +add_library(RMDMotor RMDMotor.cpp) +add_library(SerialPort SerialPort.cpp) +target_link_libraries(RMDMotor SerialPort) # 添加一个可执行文件 -add_executable(RMDControl_main main.c) -target_link_libraries(RMDControl_main RMDControl) \ No newline at end of file +add_executable(RMDControl_main main.cpp) +target_link_libraries(RMDControl_main RMDControl) +target_link_libraries(RMDControl_main RMDMotor) +target_link_libraries(RMDControl_main SerialPort) + +# add_executable(RMDControl_main main.c) +# target_link_libraries(RMDControl_main RMDControl) +# add_executable(RMDControl_test test.c) +# target_link_libraries(RMDControl_test RMDControl) + +# add_library(RMD RMD.cpp) diff --git a/LogUtil.h b/LogUtil.h new file mode 100644 index 0000000..6912d32 --- /dev/null +++ b/LogUtil.h @@ -0,0 +1,33 @@ +#pragma once +#include + +// 日志输出----------------------------------------- +#define HIGHLIGHT_(...) \ + do { \ + printf("\033[35minfo - \033[0m" __VA_ARGS__); \ + printf("\n"); \ + } while (false) + +#define WARNING_(...) \ + do { \ + printf("\033[33mwarn - \033[0m" __VA_ARGS__); \ + printf("\n"); \ + } while (false) + +#define PASS_(...) \ + do { \ + printf("\033[32minfo - \033[0m" __VA_ARGS__); \ + printf("\n"); \ + } while (false) + +#define ERROR_(...) \ + do { \ + printf("\033[31m err - \033[0m" __VA_ARGS__); \ + printf("\n"); \ + } while (false) + +#define INFO_(...) \ + do { \ + printf("info - " __VA_ARGS__); \ + printf("\n"); \ + } while (false) diff --git a/RMDControl.c b/RMDControl.cpp similarity index 76% rename from RMDControl.c rename to RMDControl.cpp index 6251ea1..15d96d7 100644 --- a/RMDControl.c +++ b/RMDControl.cpp @@ -1,9 +1,10 @@ -/** - * @file RMDControl.c +/** + * @file RMDControl.cpp * @author worranhin (worranhin@foxmail.com) + * @author drawal * @brief Source file of the RMD motor control library. - * @version 0.1 - * @date 2024-07-26 + * @version 0.2 + * @date 2024-11-5 * * @copyright Copyright (c) 2024 * @@ -30,7 +31,7 @@ int RMD_Init(const char* serialPort) { return -1; } - WINBOOL bSuccess = SetupComm(hSerial, 100, 100); + BOOL bSuccess = SetupComm(hSerial, 100, 100); if (!bSuccess) { CloseHandle(hSerial); return -1; @@ -79,10 +80,12 @@ int RMD_DeInit() { return 0; } -int RMD_GetMultiAngle_S(int64_t* angle) { - static const uint8_t command[] = {0x3E, 0x92, 0x01, 0x00, 0xD1}; - static const DWORD bytesToRead = 14; - static uint8_t readBuf[14]; +int RMD_GetMultiAngle_S(int64_t* angle, const uint8_t id) { + uint8_t command[] = {0x3E, 0x92, 0x00, 0x00, 0x00}; + command[2] = id; + command[4] = RMD_GetHeaderCheckSum(command); + const DWORD bytesToRead = 14; + uint8_t readBuf[bytesToRead]; int64_t motorAngle = 0; if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { @@ -99,8 +102,8 @@ int RMD_GetMultiAngle_S(int64_t* angle) { } // check received format - if (readBuf[0] != 0x3E || readBuf[1] != 0x92 || readBuf[2] != 0x01 || - readBuf[3] != 0x08 || readBuf[4] != 0xD9) { + if (readBuf[0] != 0x3E || readBuf[1] != 0x92 || readBuf[2] != id || + readBuf[3] != 0x08 || readBuf[4] != (0x3E + 0x92 + id + 0x08)) { return -1; } @@ -138,12 +141,15 @@ int RMD_GetMultiAngle_S(int64_t* angle) { * * @throws None */ -int RMD_GoAngleAbsolute(int64_t angle) { +int RMD_GoAngleAbsolute(int64_t angle, const uint8_t id) { int64_t angleControl = angle; uint8_t checksum = 0; - static uint8_t command[] = {0x3E, 0xA3, 0x01, 0x08, 0xEA, 0xA0, 0x0F, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAF}; + uint8_t command[] = {0x3E, 0xA3, 0x00, 0x08, 0x00, 0xA0, 0x0F, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAF}; + + command[2] = id; + command[4] = RMD_GetHeaderCheckSum(command); command[5] = *(uint8_t*)(&angleControl); command[6] = *((uint8_t*)(&angleControl) + 1); @@ -188,10 +194,10 @@ int RMD_GoAngleRelative(int32_t angle) { uint8_t checksum = 0; static uint8_t command[10] = {0x3E, 0xA7, 0x01, 0x04, 0xEA, 0x00}; - command[5] = *(uint8_t *)(&deltaAngle); - command[6] = *((uint8_t *)(&deltaAngle) + 1); - command[7] = *((uint8_t *)(&deltaAngle) + 2); - command[8] = *((uint8_t *)(&deltaAngle) + 3); + command[5] = *(uint8_t*)(&deltaAngle); + command[6] = *((uint8_t*)(&deltaAngle) + 1); + command[7] = *((uint8_t*)(&deltaAngle) + 2); + command[8] = *((uint8_t*)(&deltaAngle) + 3); for (int i = 5; i < 9; i++) { checksum += command[i]; @@ -211,8 +217,11 @@ int RMD_GoAngleRelative(int32_t angle) { * * @throws None */ -int RMD_Stop() { - static uint8_t command[] = {0x3E, 0x81, 0x01, 0x00, 0xc0}; +int RMD_Stop(const uint8_t id) { + uint8_t command[] = {0x3E, 0x81, 0x00, 0x00, 0x00}; + + command[2] = id; + command[4] = RMD_GetHeaderCheckSum(command); if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; @@ -224,13 +233,16 @@ int RMD_Stop() { /** * Sends a command to get motor's PI parameters * - * @param arrPI a array to obtain motor's PI, [angleKp, angleKi, speedKp, speedKi, torqueKp, torqueKi] + * @param arrPI a array to obtain motor's PI, [angleKp, angleKi, speedKp, + * speedKi, torqueKp, torqueKi] * @return 0 if the command was successfully sent, -1 otherwise */ -int RMD_GetPI(uint8_t* arrPI) { - static uint8_t command[] = {0x3E, 0X30, 0x01, 0x00, 0x6F}; - static const DWORD bytesToRead = 12; - static uint8_t readBuf[12]; +int RMD_GetPI(uint8_t* arrPI, const uint8_t id) { + uint8_t command[] = {0x3E, 0X30, 0x00, 0x00, 0x00}; + command[2] = id; + command[4] = RMD_GetHeaderCheckSum(command); + const DWORD bytesToRead = 12; + uint8_t readBuf[bytesToRead]; if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; @@ -244,8 +256,8 @@ int RMD_GetPI(uint8_t* arrPI) { return -1; } - if (readBuf[0] != 0x3E || readBuf[1] != 0x30 || readBuf[2] != 0x01 || - readBuf[3] != 0x06 || readBuf[4] != 0x75) { + if (readBuf[0] != 0x3E || readBuf[1] != 0x30 || readBuf[2] != id || + readBuf[3] != 0x06 || readBuf[4] != (0x3E + 0x30 + id + 0x06)) { return -1; } @@ -267,11 +279,14 @@ int RMD_GetPI(uint8_t* arrPI) { /** * Sends a command to debug PI parameters * - * @param arrPI a array to config target PI, [angleKp, angleKi, speedKp, speedKi, torqueKp, torqueKi] + * @param arrPI a array to config target PI, [angleKp, angleKi, speedKp, + * speedKi, torqueKp, torqueKi] * @return 0 if the command was successfully sent, -1 otherwise */ -int RMD_WriteAnglePI_RAM(const uint8_t* arrPI) { - static uint8_t command[12] = {0x3E, 0x31, 0x01, 0x06, 0x76}; +int RMD_WriteAnglePI_RAM(const uint8_t* arrPI, const uint8_t id) { + uint8_t command[12] = {0x3E, 0x31, 0x00, 0x06, 0x00}; + command[2] = id; + command[4] = RMD_GetHeaderCheckSum(command); uint8_t checksum = 0; for (int i = 0; i < 6; i++) { command[5 + i] = (uint8_t)arrPI[i]; @@ -289,11 +304,15 @@ int RMD_WriteAnglePI_RAM(const uint8_t* arrPI) { /** * Sends a command to change PI parameters * - * @param arrPI a array to config target PI, [angleKp, angleKi, speedKp, speedKi, torqueKp, torqueKi] + * @param arrPI a array to config target PI, [angleKp, angleKi, speedKp, + * speedKi, torqueKp, torqueKi] * @return 0 if the command was successfully sent, -1 otherwise */ -int RMD_WriteAnglePI_ROM(const uint8_t* arrPI) { - static uint8_t command[12] = {0x3E, 0x32, 0x01, 0x06, 0x77}; +int RMD_WriteAnglePI_ROM(const uint8_t* arrPI, const uint8_t id) { + uint8_t command[12] = {0x3E, 0x32, 0x00, 0x06, 0x00}; + command[2] = id; + command[4] = RMD_GetHeaderCheckSum(command); + uint8_t checksum = 0; for (int i = 0; i < 6; i++) { command[5 + i] = (uint8_t)arrPI[i]; @@ -307,3 +326,11 @@ int RMD_WriteAnglePI_ROM(const uint8_t* arrPI) { return 0; } + +uint8_t RMD_GetHeaderCheckSum(uint8_t* command) { + uint8_t sum = 0x00; + for (int i = 0; i < 4; ++i) { + sum += command[i]; + } + return sum; +} diff --git a/RMDControl.h b/RMDControl.h index 614ae5d..4872f08 100644 --- a/RMDControl.h +++ b/RMDControl.h @@ -23,15 +23,16 @@ extern "C" { #endif -int RMD_Init(const char* serialPort); +int RMD_Init(const char *serialPort); int RMD_DeInit(); -int RMD_GetMultiAngle_S(int64_t *angle); -int RMD_GoAngleAbsolute(int64_t angle); +int RMD_GetMultiAngle_S(int64_t *angle, const uint8_t id); +int RMD_GoAngleAbsolute(int64_t angle, const uint8_t id); int RMD_GoAngleRelative(int32_t angle); -int RMD_Stop(); -int RMD_GetPI(uint8_t *arrPI); -int RMD_WriteAnglePI_ROM(const uint8_t *arrPI); -int RMD_WriteAnglePI_RAM(const uint8_t *arrPI); +int RMD_Stop(const uint8_t id); +int RMD_GetPI(uint8_t *arrPI, const uint8_t id); +int RMD_WriteAnglePI_ROM(const uint8_t *arrPI, const uint8_t id); +int RMD_WriteAnglePI_RAM(const uint8_t *arrPI, const uint8_t id); +uint8_t RMD_GetHeaderCheckSum(uint8_t *command); #ifdef __cplusplus } diff --git a/RMDMotor.cpp b/RMDMotor.cpp new file mode 100644 index 0000000..b1915b8 --- /dev/null +++ b/RMDMotor.cpp @@ -0,0 +1,359 @@ +/** + * @file RMDController.cpp + * @author worranhin (worranhin@foxmail.com) + * @author drawal (2581478521@qq.com) + * @brief Implementation of RMDMotor class + * @version 0.1 + * @date 2024-11-05 + * + * @copyright Copyright (c) 2024 + * + */ +#include "RMDMotor.h" +#include "LogUtil.h" + +namespace D5R { + +/** + * Construct a RMDMotor object. + * + * @param serialPort The name of the serial port. e.g. "COM1" + * @param id The ID of the motor. e.g. 0x01 + * + * !! Deprecate !! + * The constructor will try to initialize the serial port and get the PI + * parameters of the motor. If the serial port is invalid, the constructor + * will print an error message and set the _isInit flag to false. + */ +RMDMotor::RMDMotor(const char *serialPort, uint8_t id) + : _serialPort(serialPort), _id(id) { + _isInit = Init(); + GetPI(); + if (!_isInit) { + ERROR_("Fail to init RMDMotor"); + } +} + +/** + * Construct a RMDMotor object using a handle to a serial port. + * + * @param comHandle The handle of the serial port. + * @param id The ID of the motor. e.g. 0x01 + * + * The constructor will not try to initialize the serial port. The caller + * should ensure that the serial port is valid and the handle is a valid + * handle to the serial port. + */ +RMDMotor::RMDMotor(HANDLE comHandle, uint8_t id) + : _handle(comHandle), _id(id) { + GetPI(); + _isInit = true; + } + +/** + * Destructor of RMDMotor object. + * + * The destructor will close the handle of the serial port. + */ +RMDMotor::~RMDMotor() { CloseHandle(_handle); } + +// 句柄初始化----------------------------------------- +bool RMDMotor::Init() { + _handle = CreateFileA(_serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, + OPEN_EXISTING, 0, 0); + if (_handle == INVALID_HANDLE_VALUE) { + ERROR_("Invalid serialport"); + return false; + } + + BOOL bSuccess = SetupComm(_handle, 100, 100); + if (!bSuccess) { + ERROR_("Failed to init serial device buffer"); + return false; + } + + COMMTIMEOUTS commTimeouts = {0}; + commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 + commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 + commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 + commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 + commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 + + bSuccess = SetCommTimeouts(_handle, &commTimeouts); + if (!bSuccess) { + ERROR_("Failed to config Timeouts value"); + return false; + } + + DCB dcbSerialParams = {0}; + dcbSerialParams.DCBlength = sizeof(dcbSerialParams); + if (!GetCommState(_handle, &dcbSerialParams)) { + ERROR_("Failed to obtain device comm status"); + return false; + } + dcbSerialParams.BaudRate = CBR_115200; + dcbSerialParams.ByteSize = 8; + dcbSerialParams.StopBits = ONESTOPBIT; + dcbSerialParams.Parity = NOPARITY; + if (!SetCommState(_handle, &dcbSerialParams)) { + ERROR_("Failed to config DCB"); + return false; + } + + return true; +} + +// 是否初始化----------------------------------------- +bool RMDMotor::isInit() { return _isInit; } + +// 设备重连------------------------------------------ +bool RMDMotor::Reconnect() { + if (_handle != nullptr) { + CloseHandle(_handle); + } + return Init(); +} + +// 获取当前角度--------------------------------------- +bool RMDMotor::GetMultiAngle_s(int64_t *angle) { + uint8_t command[] = {0x3E, 0x92, 0x00, 0x00, 0x00}; + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + const DWORD bytesToRead = 14; + uint8_t readBuf[bytesToRead]; + int64_t motorAngle = 0; + + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("GetMultiAngle_s: Failed to send command to device"); + return false; + } + + if (!ReadFile(_handle, readBuf, bytesToRead, &_bytesRead, NULL)) { + ERROR_("GetMultiAngle_s: Failed to revice data from device"); + return false; + } + + // check received length + if (_bytesRead != bytesToRead) { + ERROR_("GetMultiAngle_s: Abnormal received data - byte count"); + return false; + } + + // check received format + if (readBuf[0] != 0x3E || readBuf[1] != 0x92 || readBuf[2] != _id || + readBuf[3] != 0x08 || readBuf[4] != (0x3E + 0x92 + _id + 0x08)) { + ERROR_("GetMultiAngle_s: Abnormal received data - frame header"); + return false; + } + + // check data sum + uint8_t sum = 0; + for (int i = 5; i < 13; i++) { + sum += readBuf[i]; + } + if (sum != readBuf[13]) { + ERROR_("GetMultiAngle_s: Abnormal received data - data"); + return false; + } + + // motorAngle = readBuf[5] | (readBuf[6] << 8) | (readBuf[7] << 16) | + // (readBuf[8] << 24); + *(uint8_t *)(&motorAngle) = readBuf[5]; + *((uint8_t *)(&motorAngle) + 1) = readBuf[6]; + *((uint8_t *)(&motorAngle) + 2) = readBuf[7]; + *((uint8_t *)(&motorAngle) + 3) = readBuf[8]; + *((uint8_t *)(&motorAngle) + 4) = readBuf[9]; + *((uint8_t *)(&motorAngle) + 5) = readBuf[10]; + *((uint8_t *)(&motorAngle) + 6) = readBuf[11]; + *((uint8_t *)(&motorAngle) + 7) = readBuf[12]; + + *angle = motorAngle; + return true; +} + +// 帧头计算------------------------------------------ +uint8_t RMDMotor::GetHeaderCheckSum(uint8_t *command) { + uint8_t sum = 0x00; + for (int i = 0; i < 4; ++i) { + sum += command[i]; + } + return sum; +} + +// 旋转角度-绝对--------------------------------------- +bool RMDMotor::GoAngleAbsolute(int64_t angle) { + int64_t angleControl = angle; + uint8_t checksum = 0; + + uint8_t command[] = {0x3E, 0xA3, 0x00, 0x08, 0x00, 0xA0, 0x0F, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAF}; + + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + + command[5] = *(uint8_t *)(&angleControl); + command[6] = *((uint8_t *)(&angleControl) + 1); + command[7] = *((uint8_t *)(&angleControl) + 2); + command[8] = *((uint8_t *)(&angleControl) + 3); + command[9] = *((uint8_t *)(&angleControl) + 4); + command[10] = *((uint8_t *)(&angleControl) + 5); + command[11] = *((uint8_t *)(&angleControl) + 6); + command[12] = *((uint8_t *)(&angleControl) + 7); + + for (int i = 5; i < 13; i++) { + checksum += command[i]; + } + command[13] = checksum; + + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("GoToAngle: Failed to send command to device"); + return false; + } + return true; +} + +// 旋转角度-相对-------------------------------------- +bool RMDMotor::GoAngleRelative(int64_t angle) { + int64_t deltaAngle = angle; + uint8_t checksum = 0; + + static uint8_t command[10] = {0x3E, 0xA7, 0x00, 0x04, 0x00, 0x00}; + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + + command[5] = *(uint8_t *)(&deltaAngle); + command[6] = *((uint8_t *)(&deltaAngle) + 1); + command[7] = *((uint8_t *)(&deltaAngle) + 2); + command[8] = *((uint8_t *)(&deltaAngle) + 3); + + for (int i = 5; i < 9; i++) { + checksum += command[i]; + } + command[9] = checksum; + + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("GoToAngle_R: Failed to send command to device"); + return false; + } + return true; +} + +// 急停---------------------------------------------- +bool RMDMotor::Stop() { + uint8_t command[] = {0x3E, 0x81, 0x00, 0x00, 0x00}; + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("Stop: Failed to send command to device"); + return false; + } + return true; +} + +// 将当前位置设置为电机零点----------------------------- +// 注意：该方法需要重新上电才能生效，且不建议频繁使用，会损害电机寿命。 +bool RMDMotor::SetZero() { + uint8_t command[] = {0x3E, 0x19, 0x00, 0x00, 0x00}; + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("SetZero: Failed to send command to device"); + return false; + } + return true; +} + +// 获取PI参数----------------------------------------- +bool RMDMotor::GetPI() { + uint8_t command[] = {0x3E, 0X30, 0x00, 0x00, 0x00}; + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + const DWORD bytesToRead = 12; + uint8_t readBuf[bytesToRead]; + + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("GetPI: Failed to send command to device"); + return false; + } + + if (!ReadFile(_handle, readBuf, bytesToRead, &_bytesRead, NULL)) { + ERROR_("GetPI: Failed to revice data from device"); + return false; + } + + if (_bytesRead != bytesToRead) { + ERROR_("GetPI: Abnormal received data - byte count"); + return false; + } + + if (readBuf[0] != 0x3E || readBuf[1] != 0x30 || readBuf[2] != _id || + readBuf[3] != 0x06 || readBuf[4] != (0x3E + 0x30 + _id + 0x06)) { + ERROR_("GetPI: Abnormal received data - frame header"); + return false; + } + + uint8_t sum = 0; + for (int i = 5; i < 11; i++) { + sum += readBuf[i]; + } + if (sum != readBuf[11]) { + ERROR_("GetPI: Abnormal received data - data"); + return false; + } + + _piParam.angleKp = (uint8_t)readBuf[5]; + _piParam.angleKi = (uint8_t)readBuf[6]; + _piParam.speedKp = (uint8_t)readBuf[7]; + _piParam.speedKi = (uint8_t)readBuf[8]; + _piParam.torqueKp = (uint8_t)readBuf[9]; + _piParam.torqueKi = (uint8_t)readBuf[10]; + + return true; +} + +// 改写PI参数---------------------------------------- +bool RMDMotor::WriteAnglePI(const uint8_t *arrPI) { + uint8_t command[12] = {0x3E, 0x32, 0x00, 0x06, 0x00}; + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + + uint8_t checksum = 0; + for (int i = 0; i < 6; i++) { + command[5 + i] = (uint8_t)arrPI[i]; + checksum += command[5 + i]; + } + command[11] = checksum; + + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("WriteAnglePI: Failed to send command to device"); + return false; + } + if (!GetPI()) { + ERROR_("Failed to updata PI param"); + } + return true; +} + +// 调试PI参数------------------------------------------- +bool RMDMotor::DebugAnglePI(const uint8_t *arrPI) { + uint8_t command[12] = {0x3E, 0x31, 0x00, 0x06, 0x00}; + command[2] = _id; + command[4] = GetHeaderCheckSum(command); + uint8_t checksum = 0; + for (int i = 0; i < 6; i++) { + command[5 + i] = (uint8_t)arrPI[i]; + checksum += command[5 + i]; + } + command[11] = checksum; + + if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { + ERROR_("DebugAnglePI: Failed to send command to device"); + return false; + } + if (!GetPI()) { + ERROR_("Failed to updata PI param"); + } + return true; +} + +} // namespace D5R \ No newline at end of file diff --git a/RMDMotor.h b/RMDMotor.h new file mode 100644 index 0000000..7e0e45f --- /dev/null +++ b/RMDMotor.h @@ -0,0 +1,62 @@ +/** + * @file RMDController.h + * @author worranhin (worranhin@foxmail.com) + * @author drawal (2581478521@qq.com) + * @brief RMD motor class + * @version 0.1 + * @date 2024-11-05 + * + * @copyright Copyright (c) 2024 + * + */ +#pragma once +#include +#include +#include +#include + +namespace D5R { + +struct PIPARAM { + uint8_t angleKp; + uint8_t angleKi; + uint8_t speedKp; + uint8_t speedKi; + uint8_t torqueKp; + uint8_t torqueKi; +}; + +enum ID_ENTRY { + ID_01 = (uint8_t)0x01, + ID_02 = (uint8_t)0x02, +}; +class RMDMotor { +public: + RMDMotor(const char *serialPort, uint8_t id); + RMDMotor(HANDLE comHandle, uint8_t id); + ~RMDMotor(); + bool Init(); + bool isInit(); + bool Reconnect(); + bool GetMultiAngle_s(int64_t *angle); + uint8_t GetHeaderCheckSum(uint8_t *command); + bool GoAngleAbsolute(int64_t angle); + bool GoAngleRelative(int64_t angle); + bool Stop(); + bool SetZero(); + bool GetPI(); + bool WriteAnglePI(const uint8_t *arrPI); + bool DebugAnglePI(const uint8_t *arrPI); + + PIPARAM _piParam; + +private: + const char *_serialPort; + uint8_t _id; + HANDLE _handle; + DWORD _bytesRead; + DWORD _bytesWritten; + bool _isInit; +}; + +} // namespace D5R \ No newline at end of file diff --git a/SerialPort.cpp b/SerialPort.cpp new file mode 100644 index 0000000..6bc1db9 --- /dev/null +++ b/SerialPort.cpp @@ -0,0 +1,51 @@ +#include "SerialPort.h" +#include + +namespace D5R { +SerialPort::SerialPort(const char *serialPort) { + _handle = CreateFile(serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, + OPEN_EXISTING, 0, 0); + if (_handle == INVALID_HANDLE_VALUE) { + throw std::runtime_error(std::string("Failed to open serial port")); + } + + BOOL bSuccess = SetupComm(_handle, 100, 100); + if (!bSuccess) { + // ERROR_("Failed to init serial device buffer"); + throw "Failed to init serial device buffer"; + } + + COMMTIMEOUTS commTimeouts = {0}; + commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 + commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 + commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 + commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 + commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 + + bSuccess = SetCommTimeouts(_handle, &commTimeouts); + if (!bSuccess) { + // ERROR_("Failed to config Timeouts value"); + throw "Failed to config Timeouts value"; + } + + DCB dcbSerialParams = {0}; + dcbSerialParams.DCBlength = sizeof(dcbSerialParams); + if (!GetCommState(_handle, &dcbSerialParams)) { + // ERROR_("Failed to obtain device comm status"); + throw "Failed to obtain device comm status"; + } + dcbSerialParams.BaudRate = CBR_115200; + dcbSerialParams.ByteSize = 8; + dcbSerialParams.StopBits = ONESTOPBIT; + dcbSerialParams.Parity = NOPARITY; + if (!SetCommState(_handle, &dcbSerialParams)) { + // ERROR_("Failed to config DCB"); + throw "Failed to config DCB"; + } +} + +SerialPort::~SerialPort() { CloseHandle(_handle); } + +HANDLE SerialPort::GetHandle() { return _handle; } + +} // namespace D5R diff --git a/SerialPort.h b/SerialPort.h new file mode 100644 index 0000000..aecdd3a --- /dev/null +++ b/SerialPort.h @@ -0,0 +1,15 @@ +#pragma once +#include + +namespace D5R { +class SerialPort { +public: + SerialPort(const char *serialPort); + ~SerialPort(); + HANDLE GetHandle(); + +private: + HANDLE _handle; +}; + +} // namespace D5R \ No newline at end of file diff --git a/main.c b/main.c deleted file mode 100644 index 76b22fc..0000000 --- a/main.c +++ /dev/null @@ -1,96 +0,0 @@ -#include "RMDControl.h" - -void TestGoAngle(); -void TestGetAngle(); -void TestGetPI(); - -int main() { - RMD_Init("COM7"); - - TestGetAngle(); - TestGetPI(); - RMD_GoAngleRelative(1000); - Sleep(1000); - RMD_GoAngleRelative(-1000); - Sleep(1000); - - RMD_DeInit(); - return 0; -} - -void TestGetAngle() { - int64_t angle; - if (RMD_GetMultiAngle_S(&angle) == 0) { - printf("Curent multi angle: %lld\n", angle); - } else { - printf("RMD_GetMultiAngle_S error\n"); - } -} - -void TestGoAngle() { - char key = 0; - int num = 0; - bool isReadingNum = false; - int numSign = 1; - - printf("Enter an angle to go to(unit: 0.01 degree)\n"); - printf("Enter \'s\' to stop.\n"); - printf("Enter \'q\' to quit.\n"); - - while (1) { - key = getchar(); - - if (isReadingNum && !isdigit(key)) { - RMD_GoAngleAbsolute(num); - } - - if (key == 'q') { - break; - } else if (key == 's') { - RMD_Stop(); - continue; - } else if (key == '+') { - isReadingNum = true; - numSign = 1; - num = 0; - } else if (key == '-') { - isReadingNum = true; - numSign = -1; - num = 0; - } else if (isdigit(key)) { - isReadingNum = true; - num = num * 10 + key - '0'; - - while ((key = getchar()) != '\n') { - num = num * 10 + key - '0'; - } - - num *= numSign; - - if (num > 9000 || num < -9000) { - printf("输入角度超出范围\n"); - continue; - } else { - RMD_GoAngleAbsolute(num); - num = 0; - numSign = 1; - } - } - - // if (num > 9000 || num < -9000) { - // printf("输入角度超出范围\n"); - // continue; - // } - - // GoToAngle(num); - } -} - -void TestGetPI() { - uint8_t arrPI[6]; - if (RMD_GetPI(arrPI) == 0) { - printf("Curent PI: %d, %d, %d, %d, %d, %d\n", arrPI[0], arrPI[1], arrPI[2], arrPI[3], arrPI[4], arrPI[5]); - } else { - printf("RMD_GetPI error\n"); - } -} \ No newline at end of file diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..aaf2376 --- /dev/null +++ b/main.cpp @@ -0,0 +1,103 @@ +#include "RMDControl.h" +#include "RMDMotor.h" +#include "SerialPort.h" +#include + +// void TestGoAngle(); +// void TestGetAngle(); +// void TestGetPI(); + +void TestSerialPort() { + try { + D5R::SerialPort sp("COM3"); + std::cout << "SerialPort: " << sp.GetHandle() << std::endl; + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + } +} + +int main() { + D5R::SerialPort port("COM3"); + D5R::RMDMotor motor(port.GetHandle(), 0x01); + D5R::RMDMotor motor2(port.GetHandle(), 0x02); + + return 0; +} + +// void TestGetAngle() { +// int64_t angle; +// if (RMD_GetMultiAngle_S(&angle) == 0) { +// printf("Curent multi angle: %lld\n", angle); +// } else { +// printf("RMD_GetMultiAngle_S error\n"); +// } +// } + +// void TestGoAngle() { +// char key = 0; +// int num = 0; +// bool isReadingNum = false; +// int numSign = 1; + +// printf("Enter an angle to go to(unit: 0.01 degree)\n"); +// printf("Enter \'s\' to stop.\n"); +// printf("Enter \'q\' to quit.\n"); + +// while (1) { +// key = getchar(); + +// if (isReadingNum && !isdigit(key)) { +// RMD_GoAngleAbsolute(num); +// } + +// if (key == 'q') { +// break; +// } else if (key == 's') { +// RMD_Stop(); +// continue; +// } else if (key == '+') { +// isReadingNum = true; +// numSign = 1; +// num = 0; +// } else if (key == '-') { +// isReadingNum = true; +// numSign = -1; +// num = 0; +// } else if (isdigit(key)) { +// isReadingNum = true; +// num = num * 10 + key - '0'; + +// while ((key = getchar()) != '\n') { +// num = num * 10 + key - '0'; +// } + +// num *= numSign; + +// if (num > 9000 || num < -9000) { +// printf("输入角度超出范围\n"); +// continue; +// } else { +// RMD_GoAngleAbsolute(num); +// num = 0; +// numSign = 1; +// } +// } + +// // if (num > 9000 || num < -9000) { +// // printf("输入角度超出范围\n"); +// // continue; +// // } + +// // GoToAngle(num); +// } +// } + +// void TestGetPI() { +// uint8_t arrPI[6]; +// if (RMD_GetPI(arrPI) == 0) { +// printf("Curent PI: %d, %d, %d, %d, %d, %d\n", arrPI[0], arrPI[1], arrPI[2], +// arrPI[3], arrPI[4], arrPI[5]); +// } else { +// printf("RMD_GetPI error\n"); +// } +// } \ No newline at end of file