From cc46ccd1f2cf36e67173cef12d9bec6f4929c20d Mon Sep 17 00:00:00 2001 From: drawal <2581478521@qq.com> Date: Thu, 31 Oct 2024 16:13:49 +0800 Subject: [PATCH 1/9] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=94=B5=E6=9C=BA?= =?UTF-8?q?=E9=80=89=E6=8B=A9=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RMDControl.c | 194 +++++++++++++++++++++++++++++++++------------------ RMDControl.h | 16 +++-- 2 files changed, 135 insertions(+), 75 deletions(-) diff --git a/RMDControl.c b/RMDControl.c index c07fb46..27ccc98 100644 --- a/RMDControl.c +++ b/RMDControl.c @@ -23,35 +23,40 @@ DWORD bytesRead, bytesWritten; * * @throws None */ -int RMD_Init(const char* serialPort) { +int RMD_Init(const char *serialPort) +{ hSerial = CreateFile(serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0); - if (hSerial == INVALID_HANDLE_VALUE) { + if (hSerial == INVALID_HANDLE_VALUE) + { return -1; } WINBOOL bSuccess = SetupComm(hSerial, 100, 100); - if (!bSuccess) { + if (!bSuccess) + { CloseHandle(hSerial); return -1; } COMMTIMEOUTS commTimeouts = {0}; - commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 - commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 - commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 - commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 - commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 + commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 + commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 + commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 + commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 + commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 bSuccess = SetCommTimeouts(hSerial, &commTimeouts); - if (!bSuccess) { + if (!bSuccess) + { CloseHandle(hSerial); return -1; } DCB dcbSerialParams = {0}; dcbSerialParams.DCBlength = sizeof(dcbSerialParams); - if (!GetCommState(hSerial, &dcbSerialParams)) { + if (!GetCommState(hSerial, &dcbSerialParams)) + { CloseHandle(hSerial); return -1; } @@ -59,7 +64,8 @@ int RMD_Init(const char* serialPort) { dcbSerialParams.ByteSize = 8; dcbSerialParams.StopBits = ONESTOPBIT; dcbSerialParams.Parity = NOPARITY; - if (!SetCommState(hSerial, &dcbSerialParams)) { + if (!SetCommState(hSerial, &dcbSerialParams)) + { CloseHandle(hSerial); return -1; } @@ -74,55 +80,65 @@ int RMD_Init(const char* serialPort) { * * @throws None */ -int RMD_DeInit() { +int RMD_DeInit() +{ CloseHandle(hSerial); 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)) { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) + { return -1; } - if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) { + if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) + { return -1; } // check received length - if (bytesRead != bytesToRead) { + if (bytesRead != bytesToRead) + { return -1; } // 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; } // check data sum uint8_t sum = 0; - for (int i = 5; i < 13; i++) { + for (int i = 5; i < 13; i++) + { sum += readBuf[i]; } - if (sum != readBuf[13]) { + if (sum != readBuf[13]) + { return -1; } // 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]; + *(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 0; @@ -137,23 +153,28 @@ int RMD_GetMultiAngle_S(int64_t* angle) { * * @throws None */ -int RMD_GoToAngle(int64_t angle) { +int RMD_GoToAngle(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[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); + command[2] = id; + command[4] = RMD_GetHeaderCheckSum(command); - for (int i = 5; i < 13; i++) { + 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; @@ -163,7 +184,8 @@ int RMD_GoToAngle(int64_t angle) { // printf("%02X ", command[i]); // } - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) + { return -1; } return 0; @@ -176,10 +198,15 @@ int RMD_GoToAngle(int64_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)) { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) + { return -1; } @@ -192,37 +219,47 @@ int RMD_Stop() { * @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]; - - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { +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); + DWORD bytesToRead = 12; + uint8_t readBuf[bytesToRead]; + + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) + { return -1; } - if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) { + if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) + { return -1; } - if (bytesRead != bytesToRead) { + if (bytesRead != bytesToRead) + { 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; } uint8_t sum = 0; - for (int i = 5; i < 11; i++) { + for (int i = 5; i < 11; i++) + { sum += readBuf[i]; } - if (sum != readBuf[11]) { + if (sum != readBuf[11]) + { return -1; } - for (int i = 0; i < 6; i++) { + for (int i = 0; i < 6; i++) + { arrPI[i] = (uint8_t)readBuf[5 + i]; } @@ -235,16 +272,21 @@ int RMD_GetPI(uint8_t* arrPI) { * @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++) { + for (int i = 0; i < 6; i++) + { command[5 + i] = (uint8_t)arrPI[i]; checksum += command[5 + i]; } command[11] = checksum; - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) + { return -1; } @@ -257,18 +299,34 @@ int RMD_WriteAnglePI_RAM(const uint8_t* arrPI) { * @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++) { + for (int i = 0; i < 6; i++) + { command[5 + i] = (uint8_t)arrPI[i]; checksum += command[5 + i]; } command[11] = checksum; - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) + { return -1; } 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 ae37d9a..564e075 100644 --- a/RMDControl.h +++ b/RMDControl.h @@ -23,14 +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_GoToAngle(int64_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_GetMultiAngle_S(int64_t *angle, const uint8_t id); +int RMD_GoToAngle(int64_t angle, const uint8_t id); +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 } From a62c78056a51a61dfec8ce5be92e1d1a56686739 Mon Sep 17 00:00:00 2001 From: worranhin Date: Thu, 31 Oct 2024 21:41:27 +0800 Subject: [PATCH 2/9] use clang format --- .clang-format | 246 ++++++++++++++++++++++++++++++++++++++++++++++++++ RMDControl.c | 118 +++++++++--------------- 2 files changed, 289 insertions(+), 75 deletions(-) create mode 100644 .clang-format 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/RMDControl.c b/RMDControl.c index 27ccc98..e69c7f6 100644 --- a/RMDControl.c +++ b/RMDControl.c @@ -23,18 +23,15 @@ DWORD bytesRead, bytesWritten; * * @throws None */ -int RMD_Init(const char *serialPort) -{ +int RMD_Init(const char *serialPort) { hSerial = CreateFile(serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0); - if (hSerial == INVALID_HANDLE_VALUE) - { + if (hSerial == INVALID_HANDLE_VALUE) { return -1; } - WINBOOL bSuccess = SetupComm(hSerial, 100, 100); - if (!bSuccess) - { + bool bSuccess = SetupComm(hSerial, 100, 100); + if (!bSuccess) { CloseHandle(hSerial); return -1; } @@ -47,16 +44,14 @@ int RMD_Init(const char *serialPort) commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 bSuccess = SetCommTimeouts(hSerial, &commTimeouts); - if (!bSuccess) - { + if (!bSuccess) { CloseHandle(hSerial); return -1; } DCB dcbSerialParams = {0}; dcbSerialParams.DCBlength = sizeof(dcbSerialParams); - if (!GetCommState(hSerial, &dcbSerialParams)) - { + if (!GetCommState(hSerial, &dcbSerialParams)) { CloseHandle(hSerial); return -1; } @@ -64,8 +59,7 @@ int RMD_Init(const char *serialPort) dcbSerialParams.ByteSize = 8; dcbSerialParams.StopBits = ONESTOPBIT; dcbSerialParams.Parity = NOPARITY; - if (!SetCommState(hSerial, &dcbSerialParams)) - { + if (!SetCommState(hSerial, &dcbSerialParams)) { CloseHandle(hSerial); return -1; } @@ -80,14 +74,12 @@ int RMD_Init(const char *serialPort) * * @throws None */ -int RMD_DeInit() -{ +int RMD_DeInit() { CloseHandle(hSerial); return 0; } -int RMD_GetMultiAngle_S(int64_t *angle, const uint8_t id) -{ +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); @@ -95,37 +87,31 @@ int RMD_GetMultiAngle_S(int64_t *angle, const uint8_t id) uint8_t readBuf[bytesToRead]; int64_t motorAngle = 0; - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) - { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; } - if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) - { + if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) { return -1; } // check received length - if (bytesRead != bytesToRead) - { + if (bytesRead != bytesToRead) { return -1; } // check received format if (readBuf[0] != 0x3E || readBuf[1] != 0x92 || readBuf[2] != id || - readBuf[3] != 0x08 || readBuf[4] != (0x3E + 0x92 + id + 0x08)) - { + readBuf[3] != 0x08 || readBuf[4] != (0x3E + 0x92 + id + 0x08)) { return -1; } // check data sum uint8_t sum = 0; - for (int i = 5; i < 13; i++) - { + for (int i = 5; i < 13; i++) { sum += readBuf[i]; } - if (sum != readBuf[13]) - { + if (sum != readBuf[13]) { return -1; } @@ -153,8 +139,7 @@ int RMD_GetMultiAngle_S(int64_t *angle, const uint8_t id) * * @throws None */ -int RMD_GoToAngle(int64_t angle, const uint8_t id) -{ +int RMD_GoToAngle(int64_t angle, const uint8_t id) { int64_t angleControl = angle; uint8_t checksum = 0; @@ -173,8 +158,7 @@ int RMD_GoToAngle(int64_t angle, const uint8_t id) command[11] = *((uint8_t *)(&angleControl) + 6); command[12] = *((uint8_t *)(&angleControl) + 7); - for (int i = 5; i < 13; i++) - { + for (int i = 5; i < 13; i++) { checksum += command[i]; } command[13] = checksum; @@ -184,8 +168,7 @@ int RMD_GoToAngle(int64_t angle, const uint8_t id) // printf("%02X ", command[i]); // } - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) - { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; } return 0; @@ -198,15 +181,13 @@ int RMD_GoToAngle(int64_t angle, const uint8_t id) * * @throws None */ -int RMD_Stop(const uint8_t id) -{ +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)) - { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; } @@ -216,50 +197,43 @@ int RMD_Stop(const uint8_t id) /** * 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, const uint8_t id) -{ +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); DWORD bytesToRead = 12; uint8_t readBuf[bytesToRead]; - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) - { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; } - if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) - { + if (!ReadFile(hSerial, readBuf, bytesToRead, &bytesRead, NULL)) { return -1; } - if (bytesRead != bytesToRead) - { + if (bytesRead != bytesToRead) { return -1; } if (readBuf[0] != 0x3E || readBuf[1] != 0x30 || readBuf[2] != id || - readBuf[3] != 0x06 || readBuf[4] != (0x3E + 0x30 + id + 0x06)) - { + readBuf[3] != 0x06 || readBuf[4] != (0x3E + 0x30 + id + 0x06)) { return -1; } uint8_t sum = 0; - for (int i = 5; i < 11; i++) - { + for (int i = 5; i < 11; i++) { sum += readBuf[i]; } - if (sum != readBuf[11]) - { + if (sum != readBuf[11]) { return -1; } - for (int i = 0; i < 6; i++) - { + for (int i = 0; i < 6; i++) { arrPI[i] = (uint8_t)readBuf[5 + i]; } @@ -269,24 +243,22 @@ int RMD_GetPI(uint8_t *arrPI, const uint8_t id) /** * 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, const uint8_t id) -{ +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++) - { + for (int i = 0; i < 6; i++) { command[5 + i] = (uint8_t)arrPI[i]; checksum += command[5 + i]; } command[11] = checksum; - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) - { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; } @@ -296,36 +268,32 @@ int RMD_WriteAnglePI_RAM(const uint8_t *arrPI, const uint8_t id) /** * 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, const uint8_t id) -{ +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++) - { + for (int i = 0; i < 6; i++) { command[5 + i] = (uint8_t)arrPI[i]; checksum += command[5 + i]; } command[11] = checksum; - if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) - { + if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { return -1; } return 0; } -uint8_t RMD_GetHeaderCheckSum(uint8_t *command) -{ +uint8_t RMD_GetHeaderCheckSum(uint8_t *command) { uint8_t sum = 0x00; - for (int i = 0; i < 4; ++i) - { + for (int i = 0; i < 4; ++i) { sum += command[i]; } return sum; From 8fe7f93f6f31f0e94710dcaa63edee68e9750943 Mon Sep 17 00:00:00 2001 From: worranhin Date: Mon, 4 Nov 2024 18:13:52 +0800 Subject: [PATCH 3/9] migrate to cpp --- .gitignore | 3 ++- CMakeLists.txt | 2 +- RMDControl.c => RMDControl.cpp | 6 +++--- 3 files changed, 6 insertions(+), 5 deletions(-) rename RMDControl.c => RMDControl.cpp (98%) 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 938bed2..a43b63c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED True) -add_library(RMDControl RMDControl.c) +add_library(RMDControl RMDControl.cpp) # # 添加一个可执行文件 # add_executable(RMDControl_main main.c) diff --git a/RMDControl.c b/RMDControl.cpp similarity index 98% rename from RMDControl.c rename to RMDControl.cpp index e69c7f6..22bba86 100644 --- a/RMDControl.c +++ b/RMDControl.cpp @@ -1,4 +1,4 @@ -/** +/** * @file RMDControl.c * @author worranhin (worranhin@foxmail.com) * @brief Source file of the RMD motor control library. @@ -30,7 +30,7 @@ int RMD_Init(const char *serialPort) { return -1; } - bool bSuccess = SetupComm(hSerial, 100, 100); + BOOL bSuccess = SetupComm(hSerial, 100, 100); if (!bSuccess) { CloseHandle(hSerial); return -1; @@ -205,7 +205,7 @@ 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); - DWORD bytesToRead = 12; + const DWORD bytesToRead = 12; uint8_t readBuf[bytesToRead]; if (!WriteFile(hSerial, command, sizeof(command), &bytesWritten, NULL)) { From fac62ef978ca6849fa5dfd7cd196e892223d618a Mon Sep 17 00:00:00 2001 From: drawal <2581478521@qq.com> Date: Tue, 5 Nov 2024 10:11:15 +0800 Subject: [PATCH 4/9] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E9=87=8D=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RMD.cpp | 294 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ RMD.h | 33 +++++++ 2 files changed, 327 insertions(+) create mode 100644 RMD.cpp create mode 100644 RMD.h diff --git a/RMD.cpp b/RMD.cpp new file mode 100644 index 0000000..22814da --- /dev/null +++ b/RMD.cpp @@ -0,0 +1,294 @@ +#include "RMD.h" + +// 日志输出----------------------------------------- +#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) + +// 构造析构------------------------------------------ +RMD::RMD(const char* serialPort) : _serialPort(serialPort) { + _isInit = Init(); +} +RMD::~RMD() { + CloseHandle(_handle); +} + +// 句柄初始化----------------------------------------- +bool RMD::Init() { + _handle = CreateFile(_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 RMD::isInit() { + return _isInit; +} + +// 设备重连------------------------------------------ +bool RMD::Reconnect() { + if (_handle != nullptr) { + CloseHandle(_handle); + } + return Init(); +} + +// 获取当前角度--------------------------------------- +bool RMD::GetMultiAngle_s(int64_t* angle, const uint8_t id) { + 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 RMD::GetHeaderCheckSum(uint8_t* command) { + uint8_t sum = 0x00; + for (int i = 0; i < 4; ++i) { + sum += command[i]; + } + return sum; +} + +// 旋转角度------------------------------------------ +bool RMD::GoToAngle(int64_t angle, const uint8_t id) { + 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 RMD::Stop(const uint8_t id) { + 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; +} + +//获取PI参数----------------------------------------- +bool RMD::GetPI(uint8_t* arrPI, const uint8_t id) { + 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; + } + + for (int i = 0; i < 6; i++) { + arrPI[i] = (uint8_t)readBuf[5 + i]; + } + + return true; +} + +// 改写PI参数---------------------------------------- +bool RMD::WriteAnglePI(const uint8_t* arrPI, const uint8_t id) { + 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; + } + + return true; +} + +//调试PI参数------------------------------------------- +bool RMD::DebugAnglePI(const uint8_t* arrPI, const uint8_t id){ + 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; + } + return true; +} \ No newline at end of file diff --git a/RMD.h b/RMD.h new file mode 100644 index 0000000..ab90f9c --- /dev/null +++ b/RMD.h @@ -0,0 +1,33 @@ +#pragma once +#include +#include +#include + +class RMD { + public: + RMD(const char* serialPort); + ~RMD(); + bool Init(); + bool isInit(); + bool Reconnect(); + bool GetMultiAngle_s(int64_t* angle, const uint8_t id); + uint8_t GetHeaderCheckSum(uint8_t* command); + bool GoToAngle(int64_t angle, const uint8_t id); + bool Stop(const uint8_t id); + bool GetPI(uint8_t* arrPI, const uint8_t id); + bool WriteAnglePI(const uint8_t* arrPI, const uint8_t id); + bool DebugAnglePI(const uint8_t* arrPI, const uint8_t id); + + private: + const char* _serialPort; + HANDLE _handle; + DWORD _bytesRead; + DWORD _bytesWritten; + bool _isInit; +}; + +typedef enum ID_ENTRY { + ID_01 = 0x01, + ID_02 = 0x02, +}; + From faff0768b8cb5f54f63243b1ca59a59e2faa83da Mon Sep 17 00:00:00 2001 From: worranhin Date: Tue, 5 Nov 2024 10:29:48 +0800 Subject: [PATCH 5/9] rename filename; update CmakeLists.txt In CMakeLists.txt: set cxx standard to 11, and add the newly added class implement as library --- CMakeLists.txt | 3 + RMD.cpp => RMDController.cpp | 112 +++++++++++++++++------------------ RMD.h => RMDController.h | 2 +- 3 files changed, 57 insertions(+), 60 deletions(-) rename RMD.cpp => RMDController.cpp (64%) rename RMD.h => RMDController.h (96%) diff --git a/CMakeLists.txt b/CMakeLists.txt index a43b63c..b72b57f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,9 +5,12 @@ 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.cpp) +add_library(RMDController RMDController.cpp) # # 添加一个可执行文件 # add_executable(RMDControl_main main.c) diff --git a/RMD.cpp b/RMDController.cpp similarity index 64% rename from RMD.cpp rename to RMDController.cpp index 22814da..b0684fd 100644 --- a/RMD.cpp +++ b/RMDController.cpp @@ -1,43 +1,39 @@ -#include "RMD.h" +#include "RMDController.h" // 日志输出----------------------------------------- -#define HIGHLIGHT_(...) \ - do { \ - printf("\033[35minfo - \033[0m" __VA_ARGS__); \ - printf("\n"); \ +#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"); \ +#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"); \ +#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"); \ +#define ERROR_(...) \ + do { \ + printf("\033[31m err - \033[0m" __VA_ARGS__); \ + printf("\n"); \ } while (false) -#define INFO_(...) \ - do { \ - printf("info - " __VA_ARGS__); \ - printf("\n"); \ +#define INFO_(...) \ + do { \ + printf("info - " __VA_ARGS__); \ + printf("\n"); \ } while (false) // 构造析构------------------------------------------ -RMD::RMD(const char* serialPort) : _serialPort(serialPort) { - _isInit = Init(); -} -RMD::~RMD() { - CloseHandle(_handle); -} +RMD::RMD(const char *serialPort) : _serialPort(serialPort) { _isInit = Init(); } +RMD::~RMD() { CloseHandle(_handle); } // 句柄初始化----------------------------------------- bool RMD::Init() { @@ -55,11 +51,11 @@ bool RMD::Init() { } COMMTIMEOUTS commTimeouts = {0}; - commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 - commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 - commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 - commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 - commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 + commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 + commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 + commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 + commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 + commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 bSuccess = SetCommTimeouts(_handle, &commTimeouts); if (!bSuccess) { @@ -86,9 +82,7 @@ bool RMD::Init() { } // 是否初始化----------------------------------------- -bool RMD::isInit() { - return _isInit; -} +bool RMD::isInit() { return _isInit; } // 设备重连------------------------------------------ bool RMD::Reconnect() { @@ -99,7 +93,7 @@ bool RMD::Reconnect() { } // 获取当前角度--------------------------------------- -bool RMD::GetMultiAngle_s(int64_t* angle, const uint8_t id) { +bool RMD::GetMultiAngle_s(int64_t *angle, const uint8_t id) { uint8_t command[] = {0x3E, 0x92, 0x00, 0x00, 0x00}; command[2] = id; command[4] = GetHeaderCheckSum(command); @@ -142,21 +136,21 @@ bool RMD::GetMultiAngle_s(int64_t* angle, const uint8_t id) { // 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]; + *(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 RMD::GetHeaderCheckSum(uint8_t* command) { +uint8_t RMD::GetHeaderCheckSum(uint8_t *command) { uint8_t sum = 0x00; for (int i = 0; i < 4; ++i) { sum += command[i]; @@ -175,14 +169,14 @@ bool RMD::GoToAngle(int64_t angle, const uint8_t id) { 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); + 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]; @@ -196,7 +190,7 @@ bool RMD::GoToAngle(int64_t angle, const uint8_t id) { return true; } -//急停---------------------------------------------- +// 急停---------------------------------------------- bool RMD::Stop(const uint8_t id) { uint8_t command[] = {0x3E, 0x81, 0x00, 0x00, 0x00}; command[2] = id; @@ -208,8 +202,8 @@ bool RMD::Stop(const uint8_t id) { return true; } -//获取PI参数----------------------------------------- -bool RMD::GetPI(uint8_t* arrPI, const uint8_t id) { +// 获取PI参数----------------------------------------- +bool RMD::GetPI(uint8_t *arrPI, const uint8_t id) { uint8_t command[] = {0x3E, 0X30, 0x00, 0x00, 0x00}; command[2] = id; command[4] = GetHeaderCheckSum(command); @@ -254,7 +248,7 @@ bool RMD::GetPI(uint8_t* arrPI, const uint8_t id) { } // 改写PI参数---------------------------------------- -bool RMD::WriteAnglePI(const uint8_t* arrPI, const uint8_t id) { +bool RMD::WriteAnglePI(const uint8_t *arrPI, const uint8_t id) { uint8_t command[12] = {0x3E, 0x32, 0x00, 0x06, 0x00}; command[2] = id; command[4] = GetHeaderCheckSum(command); @@ -274,8 +268,8 @@ bool RMD::WriteAnglePI(const uint8_t* arrPI, const uint8_t id) { return true; } -//调试PI参数------------------------------------------- -bool RMD::DebugAnglePI(const uint8_t* arrPI, const uint8_t id){ +// 调试PI参数------------------------------------------- +bool RMD::DebugAnglePI(const uint8_t *arrPI, const uint8_t id) { uint8_t command[12] = {0x3E, 0x31, 0x00, 0x06, 0x00}; command[2] = id; command[4] = GetHeaderCheckSum(command); diff --git a/RMD.h b/RMDController.h similarity index 96% rename from RMD.h rename to RMDController.h index ab90f9c..a6da04f 100644 --- a/RMD.h +++ b/RMDController.h @@ -26,7 +26,7 @@ class RMD { bool _isInit; }; -typedef enum ID_ENTRY { +enum ID_ENTRY { ID_01 = 0x01, ID_02 = 0x02, }; From a304a69a8d7529367c72f56010fa4182b13c052c Mon Sep 17 00:00:00 2001 From: drawal <2581478521@qq.com> Date: Tue, 5 Nov 2024 14:17:23 +0800 Subject: [PATCH 6/9] =?UTF-8?q?=E5=AE=8C=E5=96=84=E7=9B=B8=E5=85=B3?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=E7=9A=84=E5=AE=9E=E7=8E=B0=EF=BC=8C=E4=BC=98?= =?UTF-8?q?=E5=8C=96=E7=B1=BB=E7=9A=84=E7=BB=93=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RMDController.cpp | 199 +++++++++++++++++++++++++++++++--------------- RMDController.h | 53 ++++++++---- 2 files changed, 172 insertions(+), 80 deletions(-) diff --git a/RMDController.cpp b/RMDController.cpp index b0684fd..1e51997 100644 --- a/RMDController.cpp +++ b/RMDController.cpp @@ -1,39 +1,57 @@ +/** + * @file RMDController.cpp + * @author worranhin (worranhin@foxmail.com) + * @author drawal (2581478521@qq.com) + * @brief Implementation of RMD class + * @version 0.1 + * @date 2024-11-05 + * + * @copyright Copyright (c) 2024 + * + */ #include "RMDController.h" // 日志输出----------------------------------------- -#define HIGHLIGHT_(...) \ - do { \ - printf("\033[35minfo - \033[0m" __VA_ARGS__); \ - printf("\n"); \ +#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"); \ +#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"); \ +#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"); \ +#define ERROR_(...) \ + do { \ + printf("\033[31m err - \033[0m" __VA_ARGS__); \ + printf("\n"); \ } while (false) -#define INFO_(...) \ - do { \ - printf("info - " __VA_ARGS__); \ - printf("\n"); \ +#define INFO_(...) \ + do { \ + printf("info - " __VA_ARGS__); \ + printf("\n"); \ } while (false) // 构造析构------------------------------------------ -RMD::RMD(const char *serialPort) : _serialPort(serialPort) { _isInit = Init(); } -RMD::~RMD() { CloseHandle(_handle); } +RMD::RMD(const char* serialPort, uint8_t id) + : _serialPort(serialPort), _id(id) { + _isInit = Init(); + GetPI(); +} +RMD::RMD(HANDLE comHandle, uint8_t id) : _handle(comHandle), _id(id) {} +RMD::~RMD() { + CloseHandle(_handle); +} // 句柄初始化----------------------------------------- bool RMD::Init() { @@ -51,11 +69,11 @@ bool RMD::Init() { } COMMTIMEOUTS commTimeouts = {0}; - commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 - commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 - commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 - commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 - commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 + commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 + commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 + commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 + commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 + commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 bSuccess = SetCommTimeouts(_handle, &commTimeouts); if (!bSuccess) { @@ -82,7 +100,9 @@ bool RMD::Init() { } // 是否初始化----------------------------------------- -bool RMD::isInit() { return _isInit; } +bool RMD::isInit() { + return _isInit; +} // 设备重连------------------------------------------ bool RMD::Reconnect() { @@ -93,9 +113,9 @@ bool RMD::Reconnect() { } // 获取当前角度--------------------------------------- -bool RMD::GetMultiAngle_s(int64_t *angle, const uint8_t id) { +bool RMD::GetMultiAngle_s(int64_t* angle) { uint8_t command[] = {0x3E, 0x92, 0x00, 0x00, 0x00}; - command[2] = id; + command[2] = _id; command[4] = GetHeaderCheckSum(command); const DWORD bytesToRead = 14; uint8_t readBuf[bytesToRead]; @@ -118,8 +138,8 @@ bool RMD::GetMultiAngle_s(int64_t *angle, const uint8_t id) { } // check received format - if (readBuf[0] != 0x3E || readBuf[1] != 0x92 || readBuf[2] != id || - readBuf[3] != 0x08 || readBuf[4] != (0x3E + 0x92 + id + 0x08)) { + 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; } @@ -136,21 +156,21 @@ bool RMD::GetMultiAngle_s(int64_t *angle, const uint8_t id) { // 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]; + *(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 RMD::GetHeaderCheckSum(uint8_t *command) { +uint8_t RMD::GetHeaderCheckSum(uint8_t* command) { uint8_t sum = 0x00; for (int i = 0; i < 4; ++i) { sum += command[i]; @@ -158,25 +178,25 @@ uint8_t RMD::GetHeaderCheckSum(uint8_t *command) { return sum; } -// 旋转角度------------------------------------------ -bool RMD::GoToAngle(int64_t angle, const uint8_t id) { +// 旋转角度-绝对--------------------------------------- +bool RMD::GoToAngle_S(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[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); + 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]; @@ -190,10 +210,36 @@ bool RMD::GoToAngle(int64_t angle, const uint8_t id) { return true; } +// 旋转角度-相对-------------------------------------- +bool RMD::GoToAngle_R(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 RMD::Stop(const uint8_t id) { +bool RMD::Stop() { uint8_t command[] = {0x3E, 0x81, 0x00, 0x00, 0x00}; - command[2] = id; + command[2] = _id; command[4] = GetHeaderCheckSum(command); if (!WriteFile(_handle, command, sizeof(command), &_bytesWritten, NULL)) { ERROR_("Stop: Failed to send command to device"); @@ -202,10 +248,23 @@ bool RMD::Stop(const uint8_t id) { return true; } +// 将当前位置设置为电机零点----------------------------- +// 注意：该方法需要重新上电才能生效，且不建议频繁使用，会损害电机寿命。 +bool RMD::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 RMD::GetPI(uint8_t *arrPI, const uint8_t id) { +bool RMD::GetPI() { uint8_t command[] = {0x3E, 0X30, 0x00, 0x00, 0x00}; - command[2] = id; + command[2] = _id; command[4] = GetHeaderCheckSum(command); const DWORD bytesToRead = 12; uint8_t readBuf[bytesToRead]; @@ -225,8 +284,8 @@ bool RMD::GetPI(uint8_t *arrPI, const uint8_t id) { return false; } - if (readBuf[0] != 0x3E || readBuf[1] != 0x30 || readBuf[2] != id || - readBuf[3] != 0x06 || readBuf[4] != (0x3E + 0x30 + id + 0x06)) { + 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; } @@ -240,17 +299,20 @@ bool RMD::GetPI(uint8_t *arrPI, const uint8_t id) { return false; } - for (int i = 0; i < 6; i++) { - arrPI[i] = (uint8_t)readBuf[5 + i]; - } + _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 RMD::WriteAnglePI(const uint8_t *arrPI, const uint8_t id) { +bool RMD::WriteAnglePI(const uint8_t* arrPI) { uint8_t command[12] = {0x3E, 0x32, 0x00, 0x06, 0x00}; - command[2] = id; + command[2] = _id; command[4] = GetHeaderCheckSum(command); uint8_t checksum = 0; @@ -264,14 +326,16 @@ bool RMD::WriteAnglePI(const uint8_t *arrPI, const uint8_t id) { ERROR_("WriteAnglePI: Failed to send command to device"); return false; } - + if (!GetPI()) { + ERROR_("Failed to updata PI param"); + } return true; } // 调试PI参数------------------------------------------- -bool RMD::DebugAnglePI(const uint8_t *arrPI, const uint8_t id) { +bool RMD::DebugAnglePI(const uint8_t* arrPI) { uint8_t command[12] = {0x3E, 0x31, 0x00, 0x06, 0x00}; - command[2] = id; + command[2] = _id; command[4] = GetHeaderCheckSum(command); uint8_t checksum = 0; for (int i = 0; i < 6; i++) { @@ -284,5 +348,8 @@ bool RMD::DebugAnglePI(const uint8_t *arrPI, const uint8_t id) { ERROR_("DebugAnglePI: Failed to send command to device"); return false; } + if (!GetPI()) { + ERROR_("Failed to updata PI param"); + } return true; } \ No newline at end of file diff --git a/RMDController.h b/RMDController.h index a6da04f..aeba189 100644 --- a/RMDController.h +++ b/RMDController.h @@ -1,33 +1,58 @@ +/** + * @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 +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 = 0x01, + ID_02 = 0x02, +}; class RMD { public: - RMD(const char* serialPort); + RMD(const char* serialPort, uint8_t id); + RMD(HANDLE comHandle, uint8_t id); ~RMD(); bool Init(); bool isInit(); bool Reconnect(); - bool GetMultiAngle_s(int64_t* angle, const uint8_t id); + bool GetMultiAngle_s(int64_t* angle); uint8_t GetHeaderCheckSum(uint8_t* command); - bool GoToAngle(int64_t angle, const uint8_t id); - bool Stop(const uint8_t id); - bool GetPI(uint8_t* arrPI, const uint8_t id); - bool WriteAnglePI(const uint8_t* arrPI, const uint8_t id); - bool DebugAnglePI(const uint8_t* arrPI, const uint8_t id); + bool GoToAngle_S(int64_t angle); + bool GoToAngle_R(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; -}; - -enum ID_ENTRY { - ID_01 = 0x01, - ID_02 = 0x02, -}; - +}; \ No newline at end of file From 7bc3bb40de0f3d8b18f956615be4242bd4bf802f Mon Sep 17 00:00:00 2001 From: worranhin Date: Tue, 5 Nov 2024 15:55:42 +0800 Subject: [PATCH 7/9] =?UTF-8?q?=E9=87=8D=E6=9E=84=E8=A7=A3=E8=80=A6?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=20-=20=E5=B0=86=E6=89=93=E5=8D=B0=E8=BE=93?= =?UTF-8?q?=E5=87=BA=E7=9A=84=E5=AE=8F=E5=87=BD=E6=95=B0=E6=8F=90=E5=8F=96?= =?UTF-8?q?=E5=88=B0=20LogUtil.h=20=E4=B8=AD=20-=20=E5=B0=86=E5=BB=BA?= =?UTF-8?q?=E7=AB=8B=E4=B8=B2=E5=8F=A3=E7=9A=84=E6=93=8D=E4=BD=9C=E6=8F=90?= =?UTF-8?q?=E5=8F=96=E5=88=B0=20SerialPort=20=E7=B1=BB=E4=B8=AD=20-=20?= =?UTF-8?q?=E5=AF=B9=20RMD=20=E5=BA=93=E4=BD=BF=E7=94=A8=E5=91=BD=E5=90=8D?= =?UTF-8?q?=E7=A9=BA=E9=97=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 9 +- LogUtil.h | 33 +++++++ RMDControl.h | 1 - RMDController.cpp => RMDMotor.cpp | 159 +++++++++++++++--------------- RMDController.h => RMDMotor.h | 32 +++--- SerialPort.cpp | 51 ++++++++++ SerialPort.h | 15 +++ main.c | 72 -------------- main.cpp | 103 +++++++++++++++++++ 9 files changed, 304 insertions(+), 171 deletions(-) create mode 100644 LogUtil.h rename RMDController.cpp => RMDMotor.cpp (69%) rename RMDController.h => RMDMotor.h (68%) create mode 100644 SerialPort.cpp create mode 100644 SerialPort.h delete mode 100644 main.c create mode 100644 main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 311770f..8bd56dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,8 +10,11 @@ set(CMAKE_CXX_STANDARD_REQUIRED True) add_library(RMDControl RMDControl.cpp) -add_library(RMDController RMDController.cpp) +add_library(RMDMotor RMDMotor.cpp) +add_library(SerialPort SerialPort.cpp) # 添加一个可执行文件 -# 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) \ No newline at end of file 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.h b/RMDControl.h index 836eaf8..4872f08 100644 --- a/RMDControl.h +++ b/RMDControl.h @@ -23,7 +23,6 @@ extern "C" { #endif - int RMD_Init(const char *serialPort); int RMD_DeInit(); int RMD_GetMultiAngle_S(int64_t *angle, const uint8_t id); diff --git a/RMDController.cpp b/RMDMotor.cpp similarity index 69% rename from RMDController.cpp rename to RMDMotor.cpp index 1e51997..b12615c 100644 --- a/RMDController.cpp +++ b/RMDMotor.cpp @@ -2,60 +2,57 @@ * @file RMDController.cpp * @author worranhin (worranhin@foxmail.com) * @author drawal (2581478521@qq.com) - * @brief Implementation of RMD class + * @brief Implementation of RMDMotor class * @version 0.1 * @date 2024-11-05 * * @copyright Copyright (c) 2024 * */ -#include "RMDController.h" - -// 日志输出----------------------------------------- -#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) - -// 构造析构------------------------------------------ -RMD::RMD(const char* serialPort, uint8_t id) +#include "RMDMotor.h" +#include "LogUtil.h" + +namespace RMD { + +/** + * 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 + * + * 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(); } -RMD::RMD(HANDLE comHandle, uint8_t id) : _handle(comHandle), _id(id) {} -RMD::~RMD() { - CloseHandle(_handle); -} + +/** + * 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) {} + +/** + * Destructor of RMDMotor object. + * + * The destructor will close the handle of the serial port. + */ +RMDMotor::~RMDMotor() { CloseHandle(_handle); } // 句柄初始化----------------------------------------- -bool RMD::Init() { - _handle = CreateFile(_serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, +bool RMDMotor::Init() { + _handle = CreateFileA(_serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0); if (_handle == INVALID_HANDLE_VALUE) { ERROR_("Invalid serialport"); @@ -69,11 +66,11 @@ bool RMD::Init() { } COMMTIMEOUTS commTimeouts = {0}; - commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 - commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 - commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 - commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 - commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 + commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 + commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 + commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 + commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 + commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 bSuccess = SetCommTimeouts(_handle, &commTimeouts); if (!bSuccess) { @@ -100,12 +97,10 @@ bool RMD::Init() { } // 是否初始化----------------------------------------- -bool RMD::isInit() { - return _isInit; -} +bool RMDMotor::isInit() { return _isInit; } // 设备重连------------------------------------------ -bool RMD::Reconnect() { +bool RMDMotor::Reconnect() { if (_handle != nullptr) { CloseHandle(_handle); } @@ -113,7 +108,7 @@ bool RMD::Reconnect() { } // 获取当前角度--------------------------------------- -bool RMD::GetMultiAngle_s(int64_t* angle) { +bool RMDMotor::GetMultiAngle_s(int64_t *angle) { uint8_t command[] = {0x3E, 0x92, 0x00, 0x00, 0x00}; command[2] = _id; command[4] = GetHeaderCheckSum(command); @@ -156,21 +151,21 @@ bool RMD::GetMultiAngle_s(int64_t* angle) { // 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]; + *(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 RMD::GetHeaderCheckSum(uint8_t* command) { +uint8_t RMDMotor::GetHeaderCheckSum(uint8_t *command) { uint8_t sum = 0x00; for (int i = 0; i < 4; ++i) { sum += command[i]; @@ -179,7 +174,7 @@ uint8_t RMD::GetHeaderCheckSum(uint8_t* command) { } // 旋转角度-绝对--------------------------------------- -bool RMD::GoToAngle_S(int64_t angle) { +bool RMDMotor::GoAngleAbsolute(int64_t angle) { int64_t angleControl = angle; uint8_t checksum = 0; @@ -189,14 +184,14 @@ bool RMD::GoToAngle_S(int64_t angle) { 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); + 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]; @@ -211,7 +206,7 @@ bool RMD::GoToAngle_S(int64_t angle) { } // 旋转角度-相对-------------------------------------- -bool RMD::GoToAngle_R(int64_t angle) { +bool RMDMotor::GoAngleRelative(int64_t angle) { int64_t deltaAngle = angle; uint8_t checksum = 0; @@ -219,10 +214,10 @@ bool RMD::GoToAngle_R(int64_t angle) { 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); + 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]; @@ -237,7 +232,7 @@ bool RMD::GoToAngle_R(int64_t angle) { } // 急停---------------------------------------------- -bool RMD::Stop() { +bool RMDMotor::Stop() { uint8_t command[] = {0x3E, 0x81, 0x00, 0x00, 0x00}; command[2] = _id; command[4] = GetHeaderCheckSum(command); @@ -250,7 +245,7 @@ bool RMD::Stop() { // 将当前位置设置为电机零点----------------------------- // 注意：该方法需要重新上电才能生效，且不建议频繁使用，会损害电机寿命。 -bool RMD::SetZero() { +bool RMDMotor::SetZero() { uint8_t command[] = {0x3E, 0x19, 0x00, 0x00, 0x00}; command[2] = _id; command[4] = GetHeaderCheckSum(command); @@ -262,7 +257,7 @@ bool RMD::SetZero() { } // 获取PI参数----------------------------------------- -bool RMD::GetPI() { +bool RMDMotor::GetPI() { uint8_t command[] = {0x3E, 0X30, 0x00, 0x00, 0x00}; command[2] = _id; command[4] = GetHeaderCheckSum(command); @@ -310,7 +305,7 @@ bool RMD::GetPI() { } // 改写PI参数---------------------------------------- -bool RMD::WriteAnglePI(const uint8_t* arrPI) { +bool RMDMotor::WriteAnglePI(const uint8_t *arrPI) { uint8_t command[12] = {0x3E, 0x32, 0x00, 0x06, 0x00}; command[2] = _id; command[4] = GetHeaderCheckSum(command); @@ -333,7 +328,7 @@ bool RMD::WriteAnglePI(const uint8_t* arrPI) { } // 调试PI参数------------------------------------------- -bool RMD::DebugAnglePI(const uint8_t* arrPI) { +bool RMDMotor::DebugAnglePI(const uint8_t *arrPI) { uint8_t command[12] = {0x3E, 0x31, 0x00, 0x06, 0x00}; command[2] = _id; command[4] = GetHeaderCheckSum(command); @@ -352,4 +347,6 @@ bool RMD::DebugAnglePI(const uint8_t* arrPI) { ERROR_("Failed to updata PI param"); } return true; -} \ No newline at end of file +} + +} // namespace RMD \ No newline at end of file diff --git a/RMDController.h b/RMDMotor.h similarity index 68% rename from RMDController.h rename to RMDMotor.h index aeba189..5423f2f 100644 --- a/RMDController.h +++ b/RMDMotor.h @@ -15,6 +15,8 @@ #include #include +namespace RMD { + struct PIPARAM { uint8_t angleKp; uint8_t angleKi; @@ -28,18 +30,18 @@ enum ID_ENTRY { ID_01 = 0x01, ID_02 = 0x02, }; -class RMD { - public: - RMD(const char* serialPort, uint8_t id); - RMD(HANDLE comHandle, uint8_t id); - ~RMD(); +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 GoToAngle_S(int64_t angle); - bool GoToAngle_R(int64_t angle); + bool GoAngleAbsolute(int64_t angle); + bool GoAngleRelative(int64_t angle); bool Stop(); bool SetZero(); bool GetPI(); @@ -49,10 +51,12 @@ class RMD { PIPARAM _piParam; private: - const char* _serialPort; - uint8_t _id; - HANDLE _handle; - DWORD _bytesRead; - DWORD _bytesWritten; - bool _isInit; -}; \ No newline at end of file + const char *_serialPort; + uint8_t _id; + HANDLE _handle; + DWORD _bytesRead; + DWORD _bytesWritten; + bool _isInit; +}; + +} // namespace RMD \ No newline at end of file diff --git a/SerialPort.cpp b/SerialPort.cpp new file mode 100644 index 0000000..ace0a02 --- /dev/null +++ b/SerialPort.cpp @@ -0,0 +1,51 @@ +#include "SerialPort.h" + +namespace RMD { +SerialPort::SerialPort(const char *serialPort) { + _handle = CreateFile(serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, + OPEN_EXISTING, 0, 0); + if (_handle == INVALID_HANDLE_VALUE) { + // ERROR_("Invalid serialport"); + throw "Invalid serialport"; + } + + 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 RMD diff --git a/SerialPort.h b/SerialPort.h new file mode 100644 index 0000000..1a07655 --- /dev/null +++ b/SerialPort.h @@ -0,0 +1,15 @@ +#pragma once +#include + +namespace RMD { +class SerialPort { +public: + SerialPort(const char *serialPort); + ~SerialPort(); + HANDLE GetHandle(); + +private: + HANDLE _handle; +}; + +} // namespace RMD \ No newline at end of file diff --git a/main.c b/main.c deleted file mode 100644 index afa6901..0000000 --- a/main.c +++ /dev/null @@ -1,72 +0,0 @@ -#include "RMDControl.h" - -int main() { - char key = 0; - int num = 0; - bool isReadingNum = false; - int numSign = 1; - - RMD_Init("COM7"); - - int64_t angle; - if (RMD_GetMultiAngle_S(&angle) == 0) { - printf("Curent multi angle: %lld\n", angle); - } else { - printf("RMD_GetMultiAngle_S error\n"); - } - - 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_GoToAngle(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_GoToAngle(num); - num = 0; - numSign = 1; - } - } - - // if (num > 9000 || num < -9000) { - // printf("输入角度超出范围\n"); - // continue; - // } - - // GoToAngle(num); - } - - RMD_DeInit(); - return 0; -} \ No newline at end of file diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..39728ad --- /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 { + RMD::SerialPort sp("COM3"); + std::cout << "SerialPort: " << sp.GetHandle() << std::endl; + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + } +} + +int main() { + RMD::SerialPort port("COM3"); + RMD::RMDMotor motor(port.GetHandle(), 0x01); + RMD::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 From 328823996dab160a966f62b0be55254d2800c84f Mon Sep 17 00:00:00 2001 From: drawal <2581478521@qq.com> Date: Wed, 6 Nov 2024 14:37:19 +0800 Subject: [PATCH 8/9] change namespace --- CMakeLists.txt | 11 ++++++-- RMDControl.cpp | 69 +++++++++++++++++++++++++------------------------- RMDMotor.cpp | 7 ++--- RMDMotor.h | 30 +++++++++++----------- SerialPort.cpp | 4 +-- SerialPort.h | 4 +-- main.cpp | 8 +++--- 7 files changed, 71 insertions(+), 62 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8bd56dd..b45b5c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,13 +8,20 @@ set(CMAKE_C_STANDARD_REQUIRED True) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED True) - 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.cpp) target_link_libraries(RMDControl_main RMDControl) target_link_libraries(RMDControl_main RMDMotor) -target_link_libraries(RMDControl_main SerialPort) \ No newline at end of file +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/RMDControl.cpp b/RMDControl.cpp index 8781190..15d96d7 100644 --- a/RMDControl.cpp +++ 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 * @@ -23,7 +24,7 @@ DWORD bytesRead, bytesWritten; * * @throws None */ -int RMD_Init(const char *serialPort) { +int RMD_Init(const char* serialPort) { hSerial = CreateFile(serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0); if (hSerial == INVALID_HANDLE_VALUE) { @@ -37,11 +38,11 @@ int RMD_Init(const char *serialPort) { } COMMTIMEOUTS commTimeouts = {0}; - commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 - commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 - commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 - commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 - commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 + commTimeouts.ReadIntervalTimeout = 50; // 读取时间间隔超时 + commTimeouts.ReadTotalTimeoutConstant = 100; // 总读取超时 + commTimeouts.ReadTotalTimeoutMultiplier = 10; // 读取超时乘数 + commTimeouts.WriteTotalTimeoutConstant = 100; // 总写入超时 + commTimeouts.WriteTotalTimeoutMultiplier = 10; // 写入超时乘数 bSuccess = SetCommTimeouts(hSerial, &commTimeouts); if (!bSuccess) { @@ -79,7 +80,7 @@ int RMD_DeInit() { return 0; } -int RMD_GetMultiAngle_S(int64_t *angle, const uint8_t id) { +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); @@ -118,14 +119,14 @@ int RMD_GetMultiAngle_S(int64_t *angle, const uint8_t id) { // 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]; + *(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 0; @@ -150,14 +151,14 @@ int RMD_GoAngleAbsolute(int64_t angle, const uint8_t id) { command[2] = id; command[4] = RMD_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); + 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]; @@ -193,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]; @@ -236,7 +237,7 @@ int RMD_Stop(const uint8_t id) { * speedKi, torqueKp, torqueKi] * @return 0 if the command was successfully sent, -1 otherwise */ -int RMD_GetPI(uint8_t *arrPI, const uint8_t id) { +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); @@ -282,7 +283,7 @@ int RMD_GetPI(uint8_t *arrPI, const uint8_t id) { * speedKi, torqueKp, torqueKi] * @return 0 if the command was successfully sent, -1 otherwise */ -int RMD_WriteAnglePI_RAM(const uint8_t *arrPI, const uint8_t id) { +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); @@ -307,7 +308,7 @@ int RMD_WriteAnglePI_RAM(const uint8_t *arrPI, const uint8_t id) { * speedKi, torqueKp, torqueKi] * @return 0 if the command was successfully sent, -1 otherwise */ -int RMD_WriteAnglePI_ROM(const uint8_t *arrPI, const uint8_t id) { +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); @@ -326,7 +327,7 @@ int RMD_WriteAnglePI_ROM(const uint8_t *arrPI, const uint8_t id) { return 0; } -uint8_t RMD_GetHeaderCheckSum(uint8_t *command) { +uint8_t RMD_GetHeaderCheckSum(uint8_t* command) { uint8_t sum = 0x00; for (int i = 0; i < 4; ++i) { sum += command[i]; diff --git a/RMDMotor.cpp b/RMDMotor.cpp index b12615c..925d6e4 100644 --- a/RMDMotor.cpp +++ b/RMDMotor.cpp @@ -12,7 +12,7 @@ #include "RMDMotor.h" #include "LogUtil.h" -namespace RMD { +namespace D5R { /** * Construct a RMDMotor object. @@ -20,6 +20,7 @@ namespace RMD { * @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. @@ -53,7 +54,7 @@ RMDMotor::~RMDMotor() { CloseHandle(_handle); } // 句柄初始化----------------------------------------- bool RMDMotor::Init() { _handle = CreateFileA(_serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, - OPEN_EXISTING, 0, 0); + OPEN_EXISTING, 0, 0); if (_handle == INVALID_HANDLE_VALUE) { ERROR_("Invalid serialport"); return false; @@ -349,4 +350,4 @@ bool RMDMotor::DebugAnglePI(const uint8_t *arrPI) { return true; } -} // namespace RMD \ No newline at end of file +} // namespace D5R \ No newline at end of file diff --git a/RMDMotor.h b/RMDMotor.h index 5423f2f..7e0e45f 100644 --- a/RMDMotor.h +++ b/RMDMotor.h @@ -15,7 +15,7 @@ #include #include -namespace RMD { +namespace D5R { struct PIPARAM { uint8_t angleKp; @@ -27,8 +27,8 @@ struct PIPARAM { }; enum ID_ENTRY { - ID_01 = 0x01, - ID_02 = 0x02, + ID_01 = (uint8_t)0x01, + ID_02 = (uint8_t)0x02, }; class RMDMotor { public: @@ -38,25 +38,25 @@ class RMDMotor { bool Init(); bool isInit(); bool Reconnect(); - bool GetMultiAngle_s(int64_t* angle); - uint8_t GetHeaderCheckSum(uint8_t* command); + 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); + 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; +private: + const char *_serialPort; + uint8_t _id; + HANDLE _handle; + DWORD _bytesRead; + DWORD _bytesWritten; + bool _isInit; }; -} // namespace RMD \ No newline at end of file +} // namespace D5R \ No newline at end of file diff --git a/SerialPort.cpp b/SerialPort.cpp index ace0a02..422ab7e 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -1,6 +1,6 @@ #include "SerialPort.h" -namespace RMD { +namespace D5R { SerialPort::SerialPort(const char *serialPort) { _handle = CreateFile(serialPort, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0); @@ -48,4 +48,4 @@ SerialPort::~SerialPort() { CloseHandle(_handle); } HANDLE SerialPort::GetHandle() { return _handle; } -} // namespace RMD +} // namespace D5R diff --git a/SerialPort.h b/SerialPort.h index 1a07655..aecdd3a 100644 --- a/SerialPort.h +++ b/SerialPort.h @@ -1,7 +1,7 @@ #pragma once #include -namespace RMD { +namespace D5R { class SerialPort { public: SerialPort(const char *serialPort); @@ -12,4 +12,4 @@ class SerialPort { HANDLE _handle; }; -} // namespace RMD \ No newline at end of file +} // namespace D5R \ No newline at end of file diff --git a/main.cpp b/main.cpp index 39728ad..aaf2376 100644 --- a/main.cpp +++ b/main.cpp @@ -9,7 +9,7 @@ void TestSerialPort() { try { - RMD::SerialPort sp("COM3"); + D5R::SerialPort sp("COM3"); std::cout << "SerialPort: " << sp.GetHandle() << std::endl; } catch (const std::exception &e) { std::cerr << e.what() << '\n'; @@ -17,9 +17,9 @@ void TestSerialPort() { } int main() { - RMD::SerialPort port("COM3"); - RMD::RMDMotor motor(port.GetHandle(), 0x01); - RMD::RMDMotor motor2(port.GetHandle(), 0x02); + D5R::SerialPort port("COM3"); + D5R::RMDMotor motor(port.GetHandle(), 0x01); + D5R::RMDMotor motor2(port.GetHandle(), 0x02); return 0; } From 6dfd2c5082ffdf7cf05e2bcf3464ee4c80047198 Mon Sep 17 00:00:00 2001 From: worranhin Date: Wed, 6 Nov 2024 17:51:47 +0800 Subject: [PATCH 9/9] fix bugs --- CMakeLists.txt | 4 ++-- RMDMotor.cpp | 8 +++++++- SerialPort.cpp | 4 ++-- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b45b5c1..15f8e55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ 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_executable(RMDControl_test test.c) +# target_link_libraries(RMDControl_test RMDControl) # add_library(RMD RMD.cpp) diff --git a/RMDMotor.cpp b/RMDMotor.cpp index 925d6e4..b1915b8 100644 --- a/RMDMotor.cpp +++ b/RMDMotor.cpp @@ -29,6 +29,9 @@ RMDMotor::RMDMotor(const char *serialPort, uint8_t id) : _serialPort(serialPort), _id(id) { _isInit = Init(); GetPI(); + if (!_isInit) { + ERROR_("Fail to init RMDMotor"); + } } /** @@ -42,7 +45,10 @@ RMDMotor::RMDMotor(const char *serialPort, uint8_t id) * handle to the serial port. */ RMDMotor::RMDMotor(HANDLE comHandle, uint8_t id) - : _handle(comHandle), _id(id) {} + : _handle(comHandle), _id(id) { + GetPI(); + _isInit = true; + } /** * Destructor of RMDMotor object. diff --git a/SerialPort.cpp b/SerialPort.cpp index 422ab7e..6bc1db9 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -1,12 +1,12 @@ #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) { - // ERROR_("Invalid serialport"); - throw "Invalid serialport"; + throw std::runtime_error(std::string("Failed to open serial port")); } BOOL bSuccess = SetupComm(_handle, 100, 100);