diff --git a/puma_motor_driver/.clang-format b/puma_motor_driver/.clang-format new file mode 100644 index 0000000..dbd4efb --- /dev/null +++ b/puma_motor_driver/.clang-format @@ -0,0 +1,60 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +AlignArrayOfStructures: Right +AlignConsecutiveAssignments: + Enabled: true + AlignCompound: true +AlignConsecutiveDeclarations: + Enabled: true + PadOperators: true +AlignConsecutiveMacros: + Enabled: true + AcrossEmptyLines: true + PadOperators: true +AlignTrailingComments: + OverEmptyLines: 1 +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: Empty +AllowShortFunctionsOnASingleLine: InlineOnly +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterCaseLabel: false + AfterClass: "true" + AfterControlStatement: Always + AfterEnum: "true" + AfterFunction: "true" + AfterNamespace: "true" + AfterObjCDeclaration: false + AfterStruct: "true" + AfterUnion: "true" + AfterExternBlock: false + BeforeCatch: "true" + BeforeElse: "true" + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: "false" + SplitEmptyFunction: false + SplitEmptyRecord: false + SplitEmptyNamespace: false +BreakAfterReturnType: Automatic +BreakBeforeBinaryOperators: false +BreakBeforeBraces: Custom +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: BeforeComma +BreakTemplateDeclarations: MultiLine +ColumnLimit: 120 +ConstructorInitializerIndentWidth: 2 +Cpp11BracedListStyle: false +DerivePointerAlignment: false +InsertNewlineAtEOF: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakFirstLessLess: 1000 +PenaltyBreakString: 1 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SortIncludes: false +SpacesInAngles: false +TabWidth: 2 diff --git a/puma_motor_driver/CMakeLists.txt b/puma_motor_driver/CMakeLists.txt index 90af4c4..1e7d77c 100644 --- a/puma_motor_driver/CMakeLists.txt +++ b/puma_motor_driver/CMakeLists.txt @@ -42,13 +42,12 @@ install(TARGETS multi_puma_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_copyright + ament_cmake_cpplint + ament_cmake_uncrustify + ) + set(ament_cmake_clang_format_CONFIG_FILE .clang-format) ament_lint_auto_find_test_dependencies() endif() diff --git a/puma_motor_driver/include/puma_motor_driver/can_proto.hpp b/puma_motor_driver/include/puma_motor_driver/can_proto.hpp index 9a14fe2..aa07124 100644 --- a/puma_motor_driver/include/puma_motor_driver/can_proto.hpp +++ b/puma_motor_driver/include/puma_motor_driver/can_proto.hpp @@ -31,22 +31,22 @@ // The masks of the fields that are used in the message identifier. // //***************************************************************************** -#define CAN_MSGID_FULL_M 0x1fffffff -#define CAN_MSGID_DEVNO_M 0x0000003f -#define CAN_MSGID_API_M 0x0000ffc0 -#define CAN_MSGID_MFR_M 0x00ff0000 -#define CAN_MSGID_DTYPE_M 0x1f000000 -#define CAN_MSGID_DEVNO_S 0 -#define CAN_MSGID_API_S 6 -#define CAN_MSGID_MFR_S 16 -#define CAN_MSGID_DTYPE_S 24 +#define CAN_MSGID_FULL_M 0x1fffffff +#define CAN_MSGID_DEVNO_M 0x0000003f +#define CAN_MSGID_API_M 0x0000ffc0 +#define CAN_MSGID_MFR_M 0x00ff0000 +#define CAN_MSGID_DTYPE_M 0x1f000000 +#define CAN_MSGID_DEVNO_S 0 +#define CAN_MSGID_API_S 6 +#define CAN_MSGID_MFR_S 16 +#define CAN_MSGID_DTYPE_S 24 //***************************************************************************** // // The Reserved device number values in the Message Id. // //***************************************************************************** -#define CAN_MSGID_DEVNO_BCAST 0x00000000 +#define CAN_MSGID_DEVNO_BCAST 0x00000000 //***************************************************************************** // @@ -69,44 +69,44 @@ // The 32 bit values associated with the CAN_MSGID_API_STATUS request. // //***************************************************************************** -#define CAN_STATUS_CODE_M 0x0000ffff -#define CAN_STATUS_MFG_M 0x00ff0000 -#define CAN_STATUS_DTYPE_M 0x1f000000 -#define CAN_STATUS_CODE_S 0 -#define CAN_STATUS_MFG_S 16 -#define CAN_STATUS_DTYPE_S 24 +#define CAN_STATUS_CODE_M 0x0000ffff +#define CAN_STATUS_MFG_M 0x00ff0000 +#define CAN_STATUS_DTYPE_M 0x1f000000 +#define CAN_STATUS_CODE_S 0 +#define CAN_STATUS_MFG_S 16 +#define CAN_STATUS_DTYPE_S 24 //***************************************************************************** // // The Reserved manufacturer identifiers in the Message Id. // //***************************************************************************** -#define CAN_MSGID_MFR_NI 0x00010000 -#define CAN_MSGID_MFR_LM 0x00020000 -#define CAN_MSGID_MFR_DEKA 0x00030000 +#define CAN_MSGID_MFR_NI 0x00010000 +#define CAN_MSGID_MFR_LM 0x00020000 +#define CAN_MSGID_MFR_DEKA 0x00030000 //***************************************************************************** // // The Reserved device type identifiers in the Message Id. // //***************************************************************************** -#define CAN_MSGID_DTYPE_BCAST 0x00000000 -#define CAN_MSGID_DTYPE_ROBOT 0x01000000 -#define CAN_MSGID_DTYPE_MOTOR 0x02000000 -#define CAN_MSGID_DTYPE_RELAY 0x03000000 -#define CAN_MSGID_DTYPE_GYRO 0x04000000 -#define CAN_MSGID_DTYPE_ACCEL 0x05000000 -#define CAN_MSGID_DTYPE_USONIC 0x06000000 -#define CAN_MSGID_DTYPE_GEART 0x07000000 -#define CAN_MSGID_DTYPE_UPDATE 0x1f000000 +#define CAN_MSGID_DTYPE_BCAST 0x00000000 +#define CAN_MSGID_DTYPE_ROBOT 0x01000000 +#define CAN_MSGID_DTYPE_MOTOR 0x02000000 +#define CAN_MSGID_DTYPE_RELAY 0x03000000 +#define CAN_MSGID_DTYPE_GYRO 0x04000000 +#define CAN_MSGID_DTYPE_ACCEL 0x05000000 +#define CAN_MSGID_DTYPE_USONIC 0x06000000 +#define CAN_MSGID_DTYPE_GEART 0x07000000 +#define CAN_MSGID_DTYPE_UPDATE 0x1f000000 //***************************************************************************** // // LM Motor Control API Classes API Class and ID masks. // //***************************************************************************** -#define CAN_MSGID_API_CLASS_M 0x0000fc00 -#define CAN_MSGID_API_ID_M 0x000003c0 +#define CAN_MSGID_API_CLASS_M 0x0000fc00 +#define CAN_MSGID_API_ID_M 0x000003c0 //***************************************************************************** // @@ -115,56 +115,56 @@ // the APIId. // //***************************************************************************** -#define CAN_API_MC_VOLTAGE 0x00000000 -#define CAN_API_MC_SPD 0x00000400 -#define CAN_API_MC_VCOMP 0x00000800 -#define CAN_API_MC_POS 0x00000c00 -#define CAN_API_MC_ICTRL 0x00001000 -#define CAN_API_MC_STATUS 0x00001400 -#define CAN_API_MC_PSTAT 0x00001800 -#define CAN_API_MC_CFG 0x00001c00 -#define CAN_API_MC_ACK 0x00002000 +#define CAN_API_MC_VOLTAGE 0x00000000 +#define CAN_API_MC_SPD 0x00000400 +#define CAN_API_MC_VCOMP 0x00000800 +#define CAN_API_MC_POS 0x00000c00 +#define CAN_API_MC_ICTRL 0x00001000 +#define CAN_API_MC_STATUS 0x00001400 +#define CAN_API_MC_PSTAT 0x00001800 +#define CAN_API_MC_CFG 0x00001c00 +#define CAN_API_MC_ACK 0x00002000 //***************************************************************************** // // The Stellaris Motor Class Control Voltage API definitions. // //***************************************************************************** -#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE) -#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S)) -#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET_RAMP (LM_API_VOLT | (3 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET_NO_ACK (LM_API_VOLT | (8 << CAN_MSGID_API_S)) +#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE) +#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S)) +#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET_RAMP (LM_API_VOLT | (3 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET_NO_ACK (LM_API_VOLT | (8 << CAN_MSGID_API_S)) //***************************************************************************** // // The Stellaris Motor Class Control API definitions for LM_API_VOLT_SET_RAMP. // //***************************************************************************** -#define LM_API_VOLT_RAMP_DIS 0 +#define LM_API_VOLT_RAMP_DIS 0 //***************************************************************************** // // The Stellaris Motor Class Control API definitions for CAN_MSGID_API_SYNC. // //***************************************************************************** -#define LM_API_SYNC_PEND_NOW 0 +#define LM_API_SYNC_PEND_NOW 0 //***************************************************************************** // // The Stellaris Motor Class Speed Control API definitions. // //***************************************************************************** -#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD) -#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S)) -#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S)) -#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S)) -#define LM_API_SPD_PC (LM_API_SPD | (3 << CAN_MSGID_API_S)) -#define LM_API_SPD_IC (LM_API_SPD | (4 << CAN_MSGID_API_S)) -#define LM_API_SPD_DC (LM_API_SPD | (5 << CAN_MSGID_API_S)) -#define LM_API_SPD_REF (LM_API_SPD | (6 << CAN_MSGID_API_S)) -#define LM_API_SPD_SET_NO_ACK (LM_API_SPD | (11 << CAN_MSGID_API_S)) +#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD) +#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S)) +#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S)) +#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S)) +#define LM_API_SPD_PC (LM_API_SPD | (3 << CAN_MSGID_API_S)) +#define LM_API_SPD_IC (LM_API_SPD | (4 << CAN_MSGID_API_S)) +#define LM_API_SPD_DC (LM_API_SPD | (5 << CAN_MSGID_API_S)) +#define LM_API_SPD_REF (LM_API_SPD | (6 << CAN_MSGID_API_S)) +#define LM_API_SPD_SET_NO_ACK (LM_API_SPD | (11 << CAN_MSGID_API_S)) //***************************************************************************** // @@ -184,15 +184,15 @@ // The Stellaris Motor Class Position Control API definitions. // //***************************************************************************** -#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS) -#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S)) -#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S)) -#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S)) -#define LM_API_POS_PC (LM_API_POS | (3 << CAN_MSGID_API_S)) -#define LM_API_POS_IC (LM_API_POS | (4 << CAN_MSGID_API_S)) -#define LM_API_POS_DC (LM_API_POS | (5 << CAN_MSGID_API_S)) -#define LM_API_POS_REF (LM_API_POS | (6 << CAN_MSGID_API_S)) -#define LM_API_POS_SET_NO_ACK (LM_API_POS | (11 << CAN_MSGID_API_S)) +#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS) +#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S)) +#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S)) +#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S)) +#define LM_API_POS_PC (LM_API_POS | (3 << CAN_MSGID_API_S)) +#define LM_API_POS_IC (LM_API_POS | (4 << CAN_MSGID_API_S)) +#define LM_API_POS_DC (LM_API_POS | (5 << CAN_MSGID_API_S)) +#define LM_API_POS_REF (LM_API_POS | (6 << CAN_MSGID_API_S)) +#define LM_API_POS_SET_NO_ACK (LM_API_POS | (11 << CAN_MSGID_API_S)) //***************************************************************************** // @@ -213,14 +213,14 @@ // The Stellaris Motor Class Firmware Update API definitions. // //***************************************************************************** -#define LM_API_UPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_UPDATE) -#define LM_API_UPD_PING (LM_API_UPD | (0 << CAN_MSGID_API_S)) -#define LM_API_UPD_DOWNLOAD (LM_API_UPD | (1 << CAN_MSGID_API_S)) -#define LM_API_UPD_SEND_DATA (LM_API_UPD | (2 << CAN_MSGID_API_S)) -#define LM_API_UPD_RESET (LM_API_UPD | (3 << CAN_MSGID_API_S)) -#define LM_API_UPD_ACK (LM_API_UPD | (4 << CAN_MSGID_API_S)) -#define LM_API_HWVER (LM_API_UPD | (5 << CAN_MSGID_API_S)) -#define LM_API_UPD_REQUEST (LM_API_UPD | (6 << CAN_MSGID_API_S)) +#define LM_API_UPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_UPDATE) +#define LM_API_UPD_PING (LM_API_UPD | (0 << CAN_MSGID_API_S)) +#define LM_API_UPD_DOWNLOAD (LM_API_UPD | (1 << CAN_MSGID_API_S)) +#define LM_API_UPD_SEND_DATA (LM_API_UPD | (2 << CAN_MSGID_API_S)) +#define LM_API_UPD_RESET (LM_API_UPD | (3 << CAN_MSGID_API_S)) +#define LM_API_UPD_ACK (LM_API_UPD | (4 << CAN_MSGID_API_S)) +#define LM_API_HWVER (LM_API_UPD | (5 << CAN_MSGID_API_S)) +#define LM_API_UPD_REQUEST (LM_API_UPD | (6 << CAN_MSGID_API_S)) //***************************************************************************** // @@ -240,44 +240,43 @@ #define LM_API_STATUS_CMODE (LM_API_STATUS | (9 << CAN_MSGID_API_S)) #define LM_API_STATUS_VOUT (LM_API_STATUS | (10 << CAN_MSGID_API_S)) -#define CPR_API_STATUS_ANALOG (LM_API_STATUS | (15 << CAN_MSGID_API_S)) +#define CPR_API_STATUS_ANALOG (LM_API_STATUS | (15 << CAN_MSGID_API_S)) #define LM_API_STATUS_STKY_FLT (LM_API_STATUS | (11 << CAN_MSGID_API_S)) #define LM_API_STATUS_FLT_COUNT (LM_API_STATUS | (12 << CAN_MSGID_API_S)) - //***************************************************************************** // // These definitions are used with the byte that is returned from // the status request for LM_API_STATUS_LIMIT. // //***************************************************************************** -#define LM_STATUS_LIMIT_FWD 0x01 -#define LM_STATUS_LIMIT_REV 0x02 +#define LM_STATUS_LIMIT_FWD 0x01 +#define LM_STATUS_LIMIT_REV 0x02 //***************************************************************************** // // LM Motor Control status codes returned due to the CAN_STATUS_CODE_M field. // //***************************************************************************** -#define LM_STATUS_FAULT_ILIMIT 0x01 -#define LM_STATUS_FAULT_TLIMIT 0x02 -#define LM_STATUS_FAULT_VLIMIT 0x04 +#define LM_STATUS_FAULT_ILIMIT 0x01 +#define LM_STATUS_FAULT_TLIMIT 0x02 +#define LM_STATUS_FAULT_VLIMIT 0x04 //***************************************************************************** // // The Stellaris Motor Class Configuration API definitions. // //***************************************************************************** -#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG) -#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S)) -#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S)) -#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S)) -#define LM_API_CFG_BRAKE_COAST (LM_API_CFG | (3 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_MODE (LM_API_CFG | (4 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_FWD (LM_API_CFG | (5 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_REV (LM_API_CFG | (6 << CAN_MSGID_API_S)) -#define LM_API_CFG_MAX_VOUT (LM_API_CFG | (7 << CAN_MSGID_API_S)) -#define LM_API_CFG_FAULT_TIME (LM_API_CFG | (8 << CAN_MSGID_API_S)) +#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG) +#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S)) +#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S)) +#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S)) +#define LM_API_CFG_BRAKE_COAST (LM_API_CFG | (3 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_MODE (LM_API_CFG | (4 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_FWD (LM_API_CFG | (5 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_REV (LM_API_CFG | (6 << CAN_MSGID_API_S)) +#define LM_API_CFG_MAX_VOUT (LM_API_CFG | (7 << CAN_MSGID_API_S)) +#define LM_API_CFG_FAULT_TIME (LM_API_CFG | (8 << CAN_MSGID_API_S)) #define CPR_API_CFG_SHUTDOWN_TEMP (LM_API_CFG | (11 << CAN_MSGID_API_S)) #define CPR_API_CFG_MINIMUM_LEVEL (LM_API_CFG | (12 << CAN_MSGID_API_S)) @@ -285,22 +284,21 @@ #define CPR_API_CFG_SHUTOFF_LEVEL (LM_API_CFG | (14 << CAN_MSGID_API_S)) #define CPR_API_CFG_SHUTOFF_TIME (LM_API_CFG | (15 << CAN_MSGID_API_S)) - //***************************************************************************** // // The Stellaris ACK API definition. // //***************************************************************************** -#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK) +#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK) //***************************************************************************** // // The 8 bit values that can be returned by a call to LM_API_STATUS_HWVER. // //***************************************************************************** -#define LM_HWVER_UNKNOWN 0x00 -#define LM_HWVER_JAG_1_0 0x01 -#define LM_HWVER_JAG_2_0 0x02 +#define LM_HWVER_UNKNOWN 0x00 +#define LM_HWVER_JAG_1_0 0x01 +#define LM_HWVER_JAG_2_0 0x02 //***************************************************************************** // @@ -320,41 +318,41 @@ // none will be selected. // //***************************************************************************** -#define LM_REF_ENCODER 0x00 -#define LM_REF_POT 0x01 -#define LM_REF_INV_ENCODER 0x02 -#define LM_REF_QUAD_ENCODER 0x03 -#define LM_REF_NONE 0xff +#define LM_REF_ENCODER 0x00 +#define LM_REF_POT 0x01 +#define LM_REF_INV_ENCODER 0x02 +#define LM_REF_QUAD_ENCODER 0x03 +#define LM_REF_NONE 0xff //***************************************************************************** // // The flags that are used to indicate the currently active fault sources. // //***************************************************************************** -#define LM_FAULT_CURRENT 0x01 -#define LM_FAULT_TEMP 0x02 -#define LM_FAULT_VBUS 0x04 -#define LM_FAULT_GATE_DRIVE 0x08 -#define LM_FAULT_COMM 0x10 +#define LM_FAULT_CURRENT 0x01 +#define LM_FAULT_TEMP 0x02 +#define LM_FAULT_VBUS 0x04 +#define LM_FAULT_GATE_DRIVE 0x08 +#define LM_FAULT_COMM 0x10 //***************************************************************************** // // The Stellaris Motor Class Periodic Status API definitions. // //***************************************************************************** -#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT) -#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S3 (LM_API_PSTAT | (3 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S0 (LM_API_PSTAT | (4 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S1 (LM_API_PSTAT | (5 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S2 (LM_API_PSTAT | (6 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S3 (LM_API_PSTAT | (7 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S0 (LM_API_PSTAT | (8 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S1 (LM_API_PSTAT | (9 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S2 (LM_API_PSTAT | (10 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S3 (LM_API_PSTAT | (11 << CAN_MSGID_API_S)) +#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT) +#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S3 (LM_API_PSTAT | (3 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S0 (LM_API_PSTAT | (4 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S1 (LM_API_PSTAT | (5 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S2 (LM_API_PSTAT | (6 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S3 (LM_API_PSTAT | (7 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S0 (LM_API_PSTAT | (8 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S1 (LM_API_PSTAT | (9 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S2 (LM_API_PSTAT | (10 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S3 (LM_API_PSTAT | (11 << CAN_MSGID_API_S)) //***************************************************************************** // @@ -363,37 +361,37 @@ // little-endian, therefore B0 is the least significant byte. // //***************************************************************************** -#define LM_PSTAT_END 0 -#define LM_PSTAT_VOLTOUT_B0 1 -#define LM_PSTAT_VOLTOUT_B1 2 -#define LM_PSTAT_VOLTBUS_B0 3 -#define LM_PSTAT_VOLTBUS_B1 4 -#define LM_PSTAT_CURRENT_B0 5 -#define LM_PSTAT_CURRENT_B1 6 -#define LM_PSTAT_TEMP_B0 7 -#define LM_PSTAT_TEMP_B1 8 -#define LM_PSTAT_POS_B0 9 -#define LM_PSTAT_POS_B1 10 -#define LM_PSTAT_POS_B2 11 -#define LM_PSTAT_POS_B3 12 -#define LM_PSTAT_SPD_B0 13 -#define LM_PSTAT_SPD_B1 14 -#define LM_PSTAT_SPD_B2 15 -#define LM_PSTAT_SPD_B3 16 -#define LM_PSTAT_LIMIT_NCLR 17 -#define LM_PSTAT_LIMIT_CLR 18 -#define LM_PSTAT_FAULT 19 -#define LM_PSTAT_STKY_FLT_NCLR 20 -#define LM_PSTAT_STKY_FLT_CLR 21 -#define LM_PSTAT_VOUT_B0 22 -#define LM_PSTAT_VOUT_B1 23 -#define LM_PSTAT_FLT_COUNT_CURRENT 24 -#define LM_PSTAT_FLT_COUNT_TEMP 25 -#define LM_PSTAT_FLT_COUNT_VOLTBUS 26 -#define LM_PSTAT_FLT_COUNT_GATE 27 -#define LM_PSTAT_FLT_COUNT_COMM 28 -#define LM_PSTAT_CANSTS 29 -#define LM_PSTAT_CANERR_B0 30 -#define LM_PSTAT_CANERR_B1 31 - -#endif // __CAN_PROTO_H__ +#define LM_PSTAT_END 0 +#define LM_PSTAT_VOLTOUT_B0 1 +#define LM_PSTAT_VOLTOUT_B1 2 +#define LM_PSTAT_VOLTBUS_B0 3 +#define LM_PSTAT_VOLTBUS_B1 4 +#define LM_PSTAT_CURRENT_B0 5 +#define LM_PSTAT_CURRENT_B1 6 +#define LM_PSTAT_TEMP_B0 7 +#define LM_PSTAT_TEMP_B1 8 +#define LM_PSTAT_POS_B0 9 +#define LM_PSTAT_POS_B1 10 +#define LM_PSTAT_POS_B2 11 +#define LM_PSTAT_POS_B3 12 +#define LM_PSTAT_SPD_B0 13 +#define LM_PSTAT_SPD_B1 14 +#define LM_PSTAT_SPD_B2 15 +#define LM_PSTAT_SPD_B3 16 +#define LM_PSTAT_LIMIT_NCLR 17 +#define LM_PSTAT_LIMIT_CLR 18 +#define LM_PSTAT_FAULT 19 +#define LM_PSTAT_STKY_FLT_NCLR 20 +#define LM_PSTAT_STKY_FLT_CLR 21 +#define LM_PSTAT_VOUT_B0 22 +#define LM_PSTAT_VOUT_B1 23 +#define LM_PSTAT_FLT_COUNT_CURRENT 24 +#define LM_PSTAT_FLT_COUNT_TEMP 25 +#define LM_PSTAT_FLT_COUNT_VOLTBUS 26 +#define LM_PSTAT_FLT_COUNT_GATE 27 +#define LM_PSTAT_FLT_COUNT_COMM 28 +#define LM_PSTAT_CANSTS 29 +#define LM_PSTAT_CANERR_B0 30 +#define LM_PSTAT_CANERR_B1 31 + +#endif // __CAN_PROTO_H__ diff --git a/puma_motor_driver/include/puma_motor_driver/driver.hpp b/puma_motor_driver/include/puma_motor_driver/driver.hpp index 0a237e4..6571473 100644 --- a/puma_motor_driver/include/puma_motor_driver/driver.hpp +++ b/puma_motor_driver/include/puma_motor_driver/driver.hpp @@ -25,13 +25,12 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #define PUMA_MOTOR_DRIVER_DRIVER_H #include + #include #include "can_msgs/msg/frame.hpp" -#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" - #include "clearpath_motor_msgs/msg/puma_status.hpp" - +#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" #include "puma_motor_driver/can_proto.hpp" namespace puma_motor_driver @@ -40,11 +39,10 @@ namespace puma_motor_driver class Driver { public: - Driver( - const std::shared_ptr interface, - std::shared_ptr nh, - const uint8_t & device_number, - const std::string & device_name); + Driver(const std::shared_ptr interface, + std::shared_ptr nh, + const uint8_t& device_number, + const std::string& device_name); void processMessage(const can_msgs::msg::Frame::SharedPtr received_msg); @@ -154,73 +152,73 @@ class Driver * Check fault response field was received. * * @return received flag - */ + */ bool receivedFault(); /** * Check power field was received. * * @return received flag - */ + */ bool receivedPower(); /** * Check mode field was received. * * @return received flag - */ + */ bool receivedMode(); /** * Check duty cycle field was received. * * @return received flag - */ + */ bool receivedDutyCycle(); /** * Check bus voltage field was received. * * @return received flag - */ + */ bool receivedBusVoltage(); /** * Check current field was received. * * @return received flag - */ + */ bool receivedCurrent(); /** * Check out voltage field was received. * * @return received flag - */ + */ bool receivedOutVoltage(); /** * Check teperature field was received. * * @return received flag - */ + */ bool receivedTemperature(); /** * Check analog input field was received. * * @return received flag - */ + */ bool receivedAnalogInput(); /** * Check position field was received. * * @return received flag - */ + */ bool receivedPosition(); /** * Check speed field was received. * * @return received flag - */ + */ bool receivedSpeed(); /** * Check setpoint field was received. * * @return received flag - */ + */ bool receivedSetpoint(); /** * Check the set-point response in voltage @@ -395,21 +393,21 @@ class Driver * * @return pointer to raw 4 bytes of the P gain response. */ - uint8_t * getRawP(); + uint8_t* getRawP(); /** * Process the last received I gain * for the current control mode. * * @return pointer to raw 4 bytes of the I gain response. */ - uint8_t * getRawI(); + uint8_t* getRawI(); /** * Process the last received I gain * for the current control mode. * * @return pointer to raw 4 bytes of the I gain response. */ - uint8_t * getRawD(); + uint8_t* getRawD(); /** * Process the last received set-point response * in voltage open-loop control. @@ -439,71 +437,65 @@ class Driver */ double statusPositionGet(); - std::string deviceName() const {return device_name_;} + std::string deviceName() const { return device_name_; } - uint8_t deviceNumber() const {return device_number_;} + uint8_t deviceNumber() const { return device_number_; } // Only used internally but is used for testing. struct Field { uint8_t data[4]; - bool received; + bool received; - float interpretFixed8x8() - { - return *(reinterpret_cast(data)) / static_cast(1 << 8); - } + float interpretFixed8x8() { return *(reinterpret_cast(data)) / static_cast(1 << 8); } - double interpretFixed16x16() - { - return *(reinterpret_cast(data)) / static_cast(1 << 16); - } + double interpretFixed16x16() { return *(reinterpret_cast(data)) / static_cast(1 << 16); } }; private: std::shared_ptr interface_; - std::shared_ptr nh_; - uint8_t device_number_; - std::string device_name_; + std::shared_ptr nh_; + uint8_t device_number_; + std::string device_name_; - bool configured_; + bool configured_; uint8_t state_; - uint8_t control_mode_; - double gain_p_; - double gain_i_; - double gain_d_; + uint8_t control_mode_; + double gain_p_; + double gain_i_; + double gain_d_; uint16_t encoder_cpr_; - float gear_ratio_; + float gear_ratio_; /** * Helpers to generate data for CAN messages. */ can_msgs::msg::Frame::SharedPtr can_msg_; - void sendId(const uint32_t id); - void sendUint8(const uint32_t id, const uint8_t value); - void sendUint16(const uint32_t id, const uint16_t value); - void sendFixed8x8(const uint32_t id, const float value); - void sendFixed16x16(const uint32_t id, const double value); - can_msgs::msg::Frame getMsg(const uint32_t id); - uint32_t getApi(const can_msgs::msg::Frame msg); - uint32_t getDeviceNumber(const can_msgs::msg::Frame msg); + void sendId(const uint32_t id); + void sendUint8(const uint32_t id, const uint8_t value); + void sendUint16(const uint32_t id, const uint16_t value); + void sendFixed8x8(const uint32_t id, const float value); + void sendFixed16x16(const uint32_t id, const double value); + can_msgs::msg::Frame getMsg(const uint32_t id); + uint32_t getApi(const can_msgs::msg::Frame msg); + uint32_t getDeviceNumber(const can_msgs::msg::Frame msg); /** * Comparing the raw bytes of the 16x16 fixed-point numbers - * to avoid comparing the floating point values. + * to avoid comparing the floating point values. * * @return boolean if received is equal to expected. */ - bool verifyRaw16x16(const uint8_t * received, const double expected); + bool verifyRaw16x16(const uint8_t* received, const double expected); /** * Comparing the raw bytes of the 8x8 fixed-point numbers - * to avoid comparing the floating point values. + * to avoid comparing the floating point values. * * @return boolean if received is equal to expected. */ - bool verifyRaw8x8(const uint8_t * received, const float expected); + bool verifyRaw8x8(const uint8_t* received, const float expected); Field voltage_fields_[4]; Field spd_fields_[7]; @@ -513,13 +505,13 @@ class Driver Field status_fields_[15]; Field cfg_fields_[15]; - Field * voltageFieldForMessage(uint32_t api); - Field * spdFieldForMessage(uint32_t api); - Field * vcompFieldForMessage(uint32_t api); - Field * posFieldForMessage(uint32_t api); - Field * ictrlFieldForMessage(uint32_t api); - Field * statusFieldForMessage(uint32_t api); - Field * cfgFieldForMessage(uint32_t api); + Field* voltageFieldForMessage(uint32_t api); + Field* spdFieldForMessage(uint32_t api); + Field* vcompFieldForMessage(uint32_t api); + Field* posFieldForMessage(uint32_t api); + Field* ictrlFieldForMessage(uint32_t api); + Field* statusFieldForMessage(uint32_t api); + Field* cfgFieldForMessage(uint32_t api); }; } // namespace puma_motor_driver diff --git a/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp b/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp index 14c10de..80c8b33 100644 --- a/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp +++ b/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp @@ -24,18 +24,15 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #ifndef PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H #define PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float64.hpp" -#include "sensor_msgs/msg/joint_state.hpp" - +#include "clearpath_motor_msgs/msg/puma_feedback.hpp" +#include "clearpath_motor_msgs/msg/puma_multi_feedback.hpp" #include "clearpath_motor_msgs/msg/puma_multi_status.hpp" #include "clearpath_motor_msgs/msg/puma_status.hpp" -#include "clearpath_motor_msgs/msg/puma_multi_feedback.hpp" -#include "clearpath_motor_msgs/msg/puma_feedback.hpp" - #include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" - #include "puma_motor_driver/driver.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_msgs/msg/float64.hpp" // #include "puma_motor_driver/diagnostic_updater.hpp" namespace FeedbackBit @@ -65,8 +62,7 @@ enum }; } -class MultiPumaNode - : public rclcpp::Node +class MultiPumaNode : public rclcpp::Node { public: MultiPumaNode(const std::string node_name); @@ -74,42 +70,42 @@ class MultiPumaNode /** * Receives desired motor speeds in sensor_msgs::JointState format and * sends commands to each motor over CAN. - */ + */ void cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg); /** * Checks if feedback fields have been received from each motor driver. * If feedback is avaiable, creates the feedback message and returns * true. Otherwise, returns false. - */ + */ bool getFeedback(); /** * Checks if status fields have been received from each motor driver. * If status data is available, creates the status message and returns * true. Otherwise, returns false. - */ + */ bool getStatus(); /** * If feedback message was created, publishes feedback message. - */ + */ void publishFeedback(); /** * If status message was created, publishes status message. - */ + */ void publishStatus(); /** * Checks that all motor drivers have been configured and are active. - */ + */ bool areAllActive(); /** * Checks if socket connection is active. If not, attempts to establish * a connection. - */ + */ bool connectIfNotConnected(); /** @@ -117,38 +113,37 @@ class MultiPumaNode * and reconfigures drivers that have disconnected, verifies parameters * are set appropriately, receives motor data, and publishes the feedback * and status messages. - */ + */ void run(); private: std::shared_ptr interface_; - std::vector drivers_; - - bool active_; - double gear_ratio_; - int encoder_cpr_; - int freq_; - uint8_t status_count_; - uint8_t desired_mode_; - std::string canbus_dev_; + std::vector drivers_; + + bool active_; + double gear_ratio_; + int encoder_cpr_; + int freq_; + uint8_t status_count_; + uint8_t desired_mode_; + std::string canbus_dev_; std::vector joint_names_; - std::vector joint_can_ids_; - std::vector joint_directions_; + std::vector joint_can_ids_; + std::vector joint_directions_; - can_msgs::msg::Frame::SharedPtr recv_msg_; - clearpath_motor_msgs::msg::PumaMultiStatus status_msg_; + can_msgs::msg::Frame::SharedPtr recv_msg_; + clearpath_motor_msgs::msg::PumaMultiStatus status_msg_; clearpath_motor_msgs::msg::PumaMultiFeedback feedback_msg_; double gain_p_; double gain_i_; double gain_d_; - rclcpp::Node::SharedPtr node_handle_; - rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Node::SharedPtr node_handle_; + rclcpp::Publisher::SharedPtr status_pub_; rclcpp::Publisher::SharedPtr feedback_pub_; - rclcpp::Subscription::SharedPtr cmd_sub_; - rclcpp::TimerBase::SharedPtr run_timer_; - + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::TimerBase::SharedPtr run_timer_; }; -#endif // PUMA_MOTOR_DRIVER_PUMA_NODE_H +#endif // PUMA_MOTOR_DRIVER_PUMA_NODE_H diff --git a/puma_motor_driver/src/driver.cpp b/puma_motor_driver/src/driver.cpp index 8245130..257afca 100644 --- a/puma_motor_driver/src/driver.cpp +++ b/puma_motor_driver/src/driver.cpp @@ -21,14 +21,15 @@ OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTE ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "clearpath_motor_msgs/msg/puma_status.hpp" -#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" - #include "puma_motor_driver/driver.hpp" -#include -#include #include + +#include +#include + +#include "clearpath_motor_msgs/msg/puma_status.hpp" +#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" #include "rclcpp/rclcpp.hpp" namespace puma_motor_driver @@ -55,57 +56,71 @@ enum ConfigurationState } // namespace ConfigurationStates typedef ConfigurationStates::ConfigurationState ConfigurationState; -Driver::Driver( - const std::shared_ptr interface, - std::shared_ptr nh, - const uint8_t & device_number, - const std::string & device_name) -: interface_(interface), - nh_(nh), - device_number_(device_number), - device_name_(device_name), - configured_(false), - state_(ConfigurationState::Initializing), - control_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), - gain_p_(1), - gain_i_(0), - gain_d_(0), - encoder_cpr_(1), - gear_ratio_(1) -{ -} +Driver::Driver(const std::shared_ptr interface, + std::shared_ptr nh, + const uint8_t& device_number, + const std::string& device_name) + : interface_(interface) + , nh_(nh) + , device_number_(device_number) + , device_name_(device_name) + , configured_(false) + , state_(ConfigurationState::Initializing) + , control_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) + , gain_p_(1) + , gain_i_(0) + , gain_d_(0) + , encoder_cpr_(1) + , gear_ratio_(1) +{} void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) { // If it's not our message, jump out. - if (getDeviceNumber(*received_msg) != device_number_) { + if (getDeviceNumber(*received_msg) != device_number_) + { return; } // If there's no data then this is a request message, jump out. - if (received_msg->dlc == 0) { + if (received_msg->dlc == 0) + { return; } - Field * field = nullptr; + Field* field = nullptr; uint32_t received_api = getApi(*received_msg); - if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) { + if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) + { field = cfgFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) { + } + else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) + { field = statusFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) { + } + else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) + { field = ictrlFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) { + } + else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) + { field = posFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) { + } + else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) + { field = vcompFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) { + } + else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) + { field = spdFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) { + } + else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) + { field = voltageFieldForMessage(received_api); } - if (!field) { + if (!field) + { return; } @@ -128,8 +143,8 @@ void Driver::sendId(const uint32_t id) void Driver::sendUint8(const uint32_t id, const uint8_t value) { can_msgs::msg::Frame msg = getMsg(id); - msg.dlc = sizeof(uint8_t); - uint8_t data[8] = {0}; + msg.dlc = sizeof(uint8_t); + uint8_t data[8] = { 0 }; std::memcpy(data, &value, sizeof(uint8_t)); std::copy(std::begin(data), std::end(data), std::begin(msg.data)); @@ -139,8 +154,8 @@ void Driver::sendUint8(const uint32_t id, const uint8_t value) void Driver::sendUint16(const uint32_t id, const uint16_t value) { can_msgs::msg::Frame msg = getMsg(id); - msg.dlc = sizeof(uint16_t); - uint8_t data[8] = {0}; + msg.dlc = sizeof(uint16_t); + uint8_t data[8] = { 0 }; std::memcpy(data, &value, sizeof(uint16_t)); std::copy(std::begin(data), std::end(data), std::begin(msg.data)); @@ -150,10 +165,10 @@ void Driver::sendUint16(const uint32_t id, const uint16_t value) void Driver::sendFixed8x8(const uint32_t id, const float value) { can_msgs::msg::Frame msg = getMsg(id); - msg.dlc = sizeof(int16_t); - int16_t output_value = static_cast(static_cast(1 << 8) * value); + msg.dlc = sizeof(int16_t); + int16_t output_value = static_cast(static_cast(1 << 8) * value); - uint8_t data[8] = {0}; + uint8_t data[8] = { 0 }; std::memcpy(data, &output_value, sizeof(int16_t)); std::copy(std::begin(data), std::end(data), std::begin(msg.data)); @@ -163,10 +178,10 @@ void Driver::sendFixed8x8(const uint32_t id, const float value) void Driver::sendFixed16x16(const uint32_t id, const double value) { can_msgs::msg::Frame msg = getMsg(id); - msg.dlc = sizeof(int32_t); - int32_t output_value = static_cast(static_cast((1 << 16) * value)); + msg.dlc = sizeof(int32_t); + int32_t output_value = static_cast(static_cast((1 << 16) * value)); - uint8_t data[8] = {0}; + uint8_t data[8] = { 0 }; std::memcpy(data, &output_value, sizeof(int32_t)); std::copy(std::begin(data), std::end(data), std::begin(msg.data)); @@ -176,10 +191,10 @@ void Driver::sendFixed16x16(const uint32_t id, const double value) can_msgs::msg::Frame Driver::getMsg(const uint32_t id) { can_msgs::msg::Frame msg; - msg.id = id; - msg.dlc = 0; - msg.is_extended = true; - msg.header.stamp = nh_->get_clock()->now(); + msg.id = id; + msg.dlc = 0; + msg.is_extended = true; + msg.header.stamp = nh_->get_clock()->now(); msg.header.frame_id = "base_link"; return msg; } @@ -194,13 +209,15 @@ uint32_t Driver::getDeviceNumber(const can_msgs::msg::Frame msg) return msg.id & CAN_MSGID_DEVNO_M; } -bool Driver::verifyRaw16x16(const uint8_t * received, const double expected) +bool Driver::verifyRaw16x16(const uint8_t* received, const double expected) { uint8_t data[4]; int32_t output_value = static_cast(static_cast((1 << 16) * expected)); std::memcpy(data, &output_value, 4); - for (uint8_t i = 0; i < 4; i++) { - if (*received != data[i]) { + for (uint8_t i = 0; i < 4; i++) + { + if (*received != data[i]) + { return false; } received++; @@ -208,13 +225,15 @@ bool Driver::verifyRaw16x16(const uint8_t * received, const double expected) return true; } -bool Driver::verifyRaw8x8(const uint8_t * received, const float expected) +bool Driver::verifyRaw8x8(const uint8_t* received, const float expected) { uint8_t data[2]; int32_t output_value = static_cast(static_cast((1 << 8) * expected)); std::memcpy(data, &output_value, 2); - for (uint8_t i = 0; i < 2; i++) { - if (*received != data[i]) { + for (uint8_t i = 0; i < 2; i++) + { + if (*received != data[i]) + { return false; } received++; @@ -245,89 +264,128 @@ void Driver::commandSpeed(const double cmd) void Driver::verifyParams() { - switch (state_) { + switch (state_) + { case ConfigurationState::Initializing: RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): starting to verify parameters.", - device_name_.c_str(), device_number_); + "Puma Motor Controller on %s (%i): starting to verify parameters.", + device_name_.c_str(), + device_number_); state_ = ConfigurationState::PowerFlag; break; case ConfigurationState::PowerFlag: - if (lastPower() == 0) { + if (lastPower() == 0) + { state_ = ConfigurationState::EncoderPosRef; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): cleared power flag.", - device_name_.c_str(), device_number_); - } else { + "Puma Motor Controller on %s (%i): cleared power flag.", + device_name_.c_str(), + device_number_); + } + else + { sendId(LM_API_STATUS_POWER | device_number_); } break; case ConfigurationState::EncoderPosRef: - if (posEncoderRef() == LM_REF_ENCODER) { + if (posEncoderRef() == LM_REF_ENCODER) + { state_ = ConfigurationState::EncoderSpdRef; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): set position encoder reference.", - device_name_.c_str(), device_number_); - } else { + "Puma Motor Controller on %s (%i): set position encoder reference.", + device_name_.c_str(), + device_number_); + } + else + { sendId(LM_API_POS_REF | device_number_); } break; case ConfigurationState::EncoderSpdRef: - if (spdEncoderRef() == LM_REF_QUAD_ENCODER) { + if (spdEncoderRef() == LM_REF_QUAD_ENCODER) + { state_ = ConfigurationState::EncoderCounts; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): set speed encoder reference.", - device_name_.c_str(), device_number_); - } else { + "Puma Motor Controller on %s (%i): set speed encoder reference.", + device_name_.c_str(), + device_number_); + } + else + { sendId(LM_API_SPD_REF | device_number_); } break; case ConfigurationState::EncoderCounts: - if (encoderCounts() == encoder_cpr_) { + if (encoderCounts() == encoder_cpr_) + { state_ = ConfigurationState::ClosedLoop; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): set encoder counts to %i.", - device_name_.c_str(), device_number_, encoder_cpr_); - } else { + "Puma Motor Controller on %s (%i): set encoder counts to %i.", + device_name_.c_str(), + device_number_, + encoder_cpr_); + } + else + { sendId(LM_API_CFG_ENC_LINES | device_number_); } break; case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. - if (lastMode() == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { + if (lastMode() == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) + { state_ = ConfigurationState::ControlMode; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): entered a close-loop control mode.", - device_name_.c_str(), device_number_); - } else { + "Puma Motor Controller on %s (%i): entered a close-loop control mode.", + device_name_.c_str(), + device_number_); + } + else + { sendId(LM_API_STATUS_CMODE | device_number_); } break; case ConfigurationState::ControlMode: - if (lastMode() == control_mode_) { - if (control_mode_ != clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + if (lastMode() == control_mode_) + { + if (control_mode_ != clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) + { state_ = ConfigurationState::PGain; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): was set to a close loop control mode.", - device_name_.c_str(), device_number_); - } else { + "Puma Motor Controller on %s (%i): was set to a close loop control mode.", + device_name_.c_str(), + device_number_); + } + else + { state_ = ConfigurationState::VerifiedParameters; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): was set to voltage control mode.", - device_name_.c_str(), device_number_); + "Puma Motor Controller on %s (%i): was set to voltage control mode.", + device_name_.c_str(), + device_number_); } } break; case ConfigurationState::PGain: - if (verifyRaw16x16(getRawP(), gain_p_)) { + if (verifyRaw16x16(getRawP(), gain_p_)) + { state_ = ConfigurationState::IGain; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getP(), gain_p_); - } else { + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), + device_number_, + getP(), + gain_p_); + } + else + { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getP(), gain_p_); - switch (control_mode_) { + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), + device_number_, + getP(), + gain_p_); + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_PC | device_number_); break; @@ -341,16 +399,26 @@ void Driver::verifyParams() } break; case ConfigurationState::IGain: - if (verifyRaw16x16(getRawI(), gain_i_)) { + if (verifyRaw16x16(getRawI(), gain_i_)) + { state_ = ConfigurationState::DGain; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getI(), gain_i_); - } else { + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), + device_number_, + getI(), + gain_i_); + } + else + { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getI(), gain_i_); - switch (control_mode_) { + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), + device_number_, + getI(), + gain_i_); + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_IC | device_number_); break; @@ -364,16 +432,26 @@ void Driver::verifyParams() } break; case ConfigurationState::DGain: - if (verifyRaw16x16(getRawD(), gain_d_)) { + if (verifyRaw16x16(getRawD(), gain_d_)) + { state_ = ConfigurationState::VerifiedParameters; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getD(), gain_d_); - } else { + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), + device_number_, + getD(), + gain_d_); + } + else + { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getD(), gain_d_); - switch (control_mode_) { + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), + device_number_, + getD(), + gain_d_); + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_DC | device_number_); break; @@ -387,18 +465,21 @@ void Driver::verifyParams() } break; } - if (state_ == ConfigurationState::VerifiedParameters) { + if (state_ == ConfigurationState::VerifiedParameters) + { RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): all parameters verified.", - device_name_.c_str(), device_number_); + "Puma Motor Controller on %s (%i): all parameters verified.", + device_name_.c_str(), + device_number_); configured_ = true; - state_ = ConfigurationState::Configured; + state_ = ConfigurationState::Configured; } } void Driver::configureParams() { - switch (state_) { + switch (state_) + { case ConfigurationState::PowerFlag: sendUint8((LM_API_STATUS_POWER | device_number_), 1); break; @@ -416,7 +497,8 @@ void Driver::configureParams() sendId(LM_API_SPD_EN | device_number_); break; case ConfigurationState::ControlMode: - switch (control_mode_) { + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: sendId(LM_API_VOLT_EN | device_number_); break; @@ -433,7 +515,8 @@ void Driver::configureParams() break; case ConfigurationState::PGain: // Set P - switch (control_mode_) { + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendFixed16x16((LM_API_ICTRL_PC | device_number_), gain_p_); break; @@ -447,7 +530,8 @@ void Driver::configureParams() break; case ConfigurationState::IGain: // Set I - switch (control_mode_) { + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendFixed16x16((LM_API_ICTRL_IC | device_number_), gain_i_); break; @@ -461,7 +545,8 @@ void Driver::configureParams() break; case ConfigurationState::DGain: // Set D - switch (control_mode_) { + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendFixed16x16((LM_API_ICTRL_DC | device_number_), gain_d_); break; @@ -487,47 +572,65 @@ void Driver::setGains(const double p, const double i, const double d) gain_i_ = i; gain_d_ = d; - if (configured_) { + if (configured_) + { updateGains(); } } void Driver::setMode(const uint8_t mode) { - if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) + { control_mode_ = mode; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): mode set to voltage control.", - device_name_.c_str(), device_number_); - if (configured_) { + "Puma Motor Controller on %s (%i): mode set to voltage control.", + device_name_.c_str(), + device_number_); + if (configured_) + { resetConfiguration(); } - } else { + } + else + { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): Close loop modes need PID gains.", - device_name_.c_str(), device_number_); + "Puma Motor Controller on %s (%i): Close loop modes need PID gains.", + device_name_.c_str(), + device_number_); } } void Driver::setMode(const uint8_t mode, const double p, const double i, const double d) { - if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) + { control_mode_ = mode; RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): mode set to voltage control but PID gains are not needed.", - device_name_.c_str(), device_number_); - if (configured_) { + "Puma Motor Controller on %s (%i): mode set to voltage control but PID gains are not needed.", + device_name_.c_str(), + device_number_); + if (configured_) + { resetConfiguration(); } - } else { + } + else + { control_mode_ = mode; - if (configured_) { + if (configured_) + { resetConfiguration(); } setGains(p, i, d); RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): mode set to a closed-loop control with PID gains of P:%f, I:%f and D:%f.", - device_name_.c_str(), device_number_, gain_p_, gain_i_, gain_d_); + "Puma Motor Controller on %s (%i): mode set to a closed-loop control with PID gains of P:%f, I:%f and " + "D:%f.", + device_name_.c_str(), + device_number_, + gain_p_, + gain_i_, + gain_d_); } } @@ -584,7 +687,8 @@ void Driver::requestFeedbackPowerState() void Driver::requestFeedbackSetpoint() { - switch (control_mode_) { + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_SET | device_number_); break; @@ -603,84 +707,85 @@ void Driver::requestFeedbackSetpoint() void Driver::resetConfiguration() { configured_ = false; - state_ = ConfigurationState::Initializing; + state_ = ConfigurationState::Initializing; } void Driver::updateGains() { configured_ = false; - state_ = ConfigurationState::PGain; + state_ = ConfigurationState::PGain; } bool Driver::receivedDutyCycle() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); return field->received; } bool Driver::receivedBusVoltage() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); return field->received; } bool Driver::receivedCurrent() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); return field->received; } bool Driver::receivedPosition() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); return field->received; } bool Driver::receivedSpeed() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); return field->received; } bool Driver::receivedFault() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); return field->received; } bool Driver::receivedPower() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); return field->received; } bool Driver::receivedMode() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); return field->received; } bool Driver::receivedOutVoltage() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); return field->received; } bool Driver::receivedTemperature() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); return field->received; } bool Driver::receivedAnalogInput() { - Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + Field* field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); return field->received; } bool Driver::receivedSetpoint() { - switch (control_mode_) { + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: return receivedCurrentSetpoint(); break; @@ -701,108 +806,109 @@ bool Driver::receivedSetpoint() bool Driver::receivedSpeedSetpoint() { - Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + Field* field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); return field->received; } bool Driver::receivedDutyCycleSetpoint() { - Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + Field* field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); return field->received; } bool Driver::receivedCurrentSetpoint() { - Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + Field* field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); return field->received; } bool Driver::receivedPositionSetpoint() { - Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + Field* field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); return field->received; } float Driver::lastDutyCycle() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); field->received = false; return field->interpretFixed8x8() / 128.0; } float Driver::lastBusVoltage() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); field->received = false; return field->interpretFixed8x8(); } float Driver::lastCurrent() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); field->received = false; return field->interpretFixed8x8(); } double Driver::lastPosition() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); field->received = false; return field->interpretFixed16x16() * ((2 * M_PI) / gear_ratio_); // Convert rev to rad } double Driver::lastSpeed() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); field->received = false; return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s } uint8_t Driver::lastFault() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); field->received = false; return field->data[0]; } uint8_t Driver::lastPower() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); field->received = false; return field->data[0]; } uint8_t Driver::lastMode() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); field->received = false; return field->data[0]; } float Driver::lastOutVoltage() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); field->received = false; return field->interpretFixed8x8(); } float Driver::lastTemperature() { - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); field->received = false; return field->interpretFixed8x8(); } float Driver::lastAnalogInput() { - Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + Field* field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); field->received = false; return field->interpretFixed8x8(); } double Driver::lastSetpoint() { - switch (control_mode_) { + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: return statusCurrentGet(); break; @@ -822,54 +928,55 @@ double Driver::lastSetpoint() } double Driver::statusSpeedGet() { - Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + Field* field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); field->received = false; return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s } float Driver::statusDutyCycleGet() { - Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + Field* field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); field->received = false; return field->interpretFixed8x8() / 128.0; } float Driver::statusCurrentGet() { - Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + Field* field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); field->received = false; return field->interpretFixed8x8(); } double Driver::statusPositionGet() { - Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + Field* field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); field->received = false; - return field->interpretFixed16x16() * (( 2 * M_PI) / gear_ratio_); // Convert rev to rad + return field->interpretFixed16x16() * ((2 * M_PI) / gear_ratio_); // Convert rev to rad } uint8_t Driver::posEncoderRef() { - Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_REF))); + Field* field = posFieldForMessage(getApi(getMsg(LM_API_POS_REF))); return field->data[0]; } uint8_t Driver::spdEncoderRef() { - Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_REF))); + Field* field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_REF))); return field->data[0]; } uint16_t Driver::encoderCounts() { - Field * field = cfgFieldForMessage(getApi(getMsg(LM_API_CFG_ENC_LINES))); + Field* field = cfgFieldForMessage(getApi(getMsg(LM_API_CFG_ENC_LINES))); return static_cast(field->data[0]) | static_cast(field->data[1] << 8); } double Driver::getP() { - Field * field; - switch (control_mode_) { + Field* field; + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); break; @@ -885,8 +992,9 @@ double Driver::getP() double Driver::getI() { - Field * field; - switch (control_mode_) { + Field* field; + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); break; @@ -902,8 +1010,9 @@ double Driver::getI() double Driver::getD() { - Field * field; - switch (control_mode_) { + Field* field; + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); break; @@ -917,10 +1026,11 @@ double Driver::getD() return field->interpretFixed16x16(); } -uint8_t * Driver::getRawP() +uint8_t* Driver::getRawP() { - Field * field; - switch (control_mode_) { + Field* field; + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); break; @@ -934,10 +1044,11 @@ uint8_t * Driver::getRawP() return field->data; } -uint8_t * Driver::getRawI() +uint8_t* Driver::getRawI() { - Field * field; - switch (control_mode_) { + Field* field; + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); break; @@ -951,10 +1062,11 @@ uint8_t * Driver::getRawI() return field->data; } -uint8_t * Driver::getRawD() +uint8_t* Driver::getRawD() { - Field * field; - switch (control_mode_) { + Field* field; + switch (control_mode_) + { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); break; @@ -968,43 +1080,43 @@ uint8_t * Driver::getRawD() return field->data; } -Driver::Field * Driver::voltageFieldForMessage(uint32_t api) +Driver::Field* Driver::voltageFieldForMessage(uint32_t api) { uint32_t voltage_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &voltage_fields_[voltage_field_index]; } -Driver::Field * Driver::spdFieldForMessage(uint32_t api) +Driver::Field* Driver::spdFieldForMessage(uint32_t api) { uint32_t spd_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &spd_fields_[spd_field_index]; } -Driver::Field * Driver::vcompFieldForMessage(uint32_t api) +Driver::Field* Driver::vcompFieldForMessage(uint32_t api) { uint32_t vcomp_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &vcomp_fields_[vcomp_field_index]; } -Driver::Field * Driver::posFieldForMessage(uint32_t api) +Driver::Field* Driver::posFieldForMessage(uint32_t api) { uint32_t pos_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &pos_fields_[pos_field_index]; } -Driver::Field * Driver::ictrlFieldForMessage(uint32_t api) +Driver::Field* Driver::ictrlFieldForMessage(uint32_t api) { uint32_t ictrl_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &ictrl_fields_[ictrl_field_index]; } -Driver::Field * Driver::statusFieldForMessage(uint32_t api) +Driver::Field* Driver::statusFieldForMessage(uint32_t api) { uint32_t status_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &status_fields_[status_field_index]; } -Driver::Field * Driver::cfgFieldForMessage(uint32_t api) +Driver::Field* Driver::cfgFieldForMessage(uint32_t api) { uint32_t cfg_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &cfg_fields_[cfg_field_index]; diff --git a/puma_motor_driver/src/multi_puma_node.cpp b/puma_motor_driver/src/multi_puma_node.cpp index 4a48c15..32bdae2 100644 --- a/puma_motor_driver/src/multi_puma_node.cpp +++ b/puma_motor_driver/src/multi_puma_node.cpp @@ -24,10 +24,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "puma_motor_driver/multi_puma_node.hpp" MultiPumaNode::MultiPumaNode(const std::string node_name) -:Node(node_name), - active_(false), - status_count_(0), - desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) + : Node(node_name), active_(false), status_count_(0), desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { // Parameters this->declare_parameter("canbus_dev", "vcan0"); @@ -48,59 +45,48 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) this->get_parameter("gain.i", gain_i_); this->get_parameter("gain.d", gain_d_); this->get_parameter("gear_ratio", gear_ratio_); - joint_can_ids_ = this->get_parameter("joint_can_ids").as_integer_array(); + joint_can_ids_ = this->get_parameter("joint_can_ids").as_integer_array(); joint_directions_ = this->get_parameter("joint_directions").as_integer_array(); - joint_names_ = this->get_parameter("joint_names").as_string_array(); - - RCLCPP_INFO( - this->get_logger(), - "Gear Ratio %f\nEncoder CPR %d\nFrequency %d\nGain PID %f,%f,%f\nCANBus Device %s", - gear_ratio_, - encoder_cpr_, - freq_, - gain_p_, - gain_i_, - gain_d_, - canbus_dev_.c_str() - ); + joint_names_ = this->get_parameter("joint_names").as_string_array(); + + RCLCPP_INFO(this->get_logger(), + "Gear Ratio %f\nEncoder CPR %d\nFrequency %d\nGain PID %f,%f,%f\nCANBus Device %s", + gear_ratio_, + encoder_cpr_, + freq_, + gain_p_, + gain_i_, + gain_d_, + canbus_dev_.c_str()); // Validate Parameters - if (joint_names_.size() != joint_can_ids_.size() || - joint_names_.size() != joint_directions_.size()) + if (joint_names_.size() != joint_can_ids_.size() || joint_names_.size() != joint_directions_.size()) { - RCLCPP_ERROR( - this->get_logger(), - "Length of joint_name list must match length of joint_can_ids list and joint_directions list."); + RCLCPP_ERROR(this->get_logger(), + "Length of joint_name list must match length of joint_can_ids list and joint_directions list."); return; } // Subsciber cmd_sub_ = this->create_subscription( - "platform/puma/cmd", - rclcpp::SensorDataQoS(), - std::bind(&MultiPumaNode::cmdCallback, this, std::placeholders::_1)); + "platform/puma/cmd", + rclcpp::SensorDataQoS(), + std::bind(&MultiPumaNode::cmdCallback, this, std::placeholders::_1)); // Publishers - feedback_pub_ = this->create_publisher( - "platform/puma/feedback", - rclcpp::SensorDataQoS()); - status_pub_ = this->create_publisher( - "platform/puma/status", - rclcpp::SensorDataQoS()); + feedback_pub_ = this->create_publisher("platform/puma/feedback", + rclcpp::SensorDataQoS()); + status_pub_ = this->create_publisher("platform/puma/status", + rclcpp::SensorDataQoS()); - node_handle_ = std::shared_ptr(this, [](rclcpp::Node *){}); + node_handle_ = std::shared_ptr(this, [](rclcpp::Node*) {}); // Socket - interface_.reset(new clearpath_ros2_socketcan_interface::SocketCANInterface( - canbus_dev_, node_handle_)); - - for (uint8_t i = 0; i < joint_names_.size(); i++) { - drivers_.push_back(puma_motor_driver::Driver( - interface_, - node_handle_, - joint_can_ids_[i], - joint_names_[i] - )); + interface_.reset(new clearpath_ros2_socketcan_interface::SocketCANInterface(canbus_dev_, node_handle_)); + + for (uint8_t i = 0; i < joint_names_.size(); i++) + { + drivers_.push_back(puma_motor_driver::Driver(interface_, node_handle_, joint_can_ids_[i], joint_names_[i])); } recv_msg_.reset(new can_msgs::msg::Frame()); @@ -108,7 +94,8 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) status_msg_.drivers.resize(drivers_.size()); uint8_t i = 0; - for (auto & driver : drivers_) { + for (auto& driver : drivers_) + { driver.clearMsgCache(); driver.setEncoderCPR(encoder_cpr_); driver.setGearRatio(gear_ratio_ * joint_directions_[i]); @@ -116,15 +103,15 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) i++; } - run_timer_ = this->create_wall_timer( - std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); + run_timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); } bool MultiPumaNode::getFeedback() { // Check All Fields Received uint8_t received = 0; - for (auto & driver : drivers_) { + for (auto& driver : drivers_) + { received |= driver.receivedDutyCycle() << FeedbackBit::DutyCycle; received |= driver.receivedCurrent() << FeedbackBit::Current; received |= driver.receivedPosition() << FeedbackBit::Position; @@ -132,20 +119,22 @@ bool MultiPumaNode::getFeedback() received |= driver.receivedSetpoint() << FeedbackBit::Setpoint; } - if (received != (1 << FeedbackBit::Count) - 1) { + if (received != (1 << FeedbackBit::Count) - 1) + { return false; } uint8_t feedback_index = 0; - for (auto & driver : drivers_) { - clearpath_motor_msgs::msg::PumaFeedback * f = &feedback_msg_.drivers_feedback[feedback_index]; - f->device_number = driver.deviceNumber(); - f->device_name = driver.deviceName(); - f->duty_cycle = driver.lastDutyCycle(); - f->current = driver.lastCurrent(); - f->travel = driver.lastPosition(); - f->speed = driver.lastSpeed(); - f->setpoint = driver.lastSetpoint(); + for (auto& driver : drivers_) + { + clearpath_motor_msgs::msg::PumaFeedback* f = &feedback_msg_.drivers_feedback[feedback_index]; + f->device_number = driver.deviceNumber(); + f->device_name = driver.deviceName(); + f->duty_cycle = driver.lastDutyCycle(); + f->current = driver.lastCurrent(); + f->travel = driver.lastPosition(); + f->speed = driver.lastSpeed(); + f->setpoint = driver.lastSetpoint(); feedback_index++; } @@ -156,10 +145,11 @@ bool MultiPumaNode::getFeedback() bool MultiPumaNode::getStatus() { // Check All Fields Received - uint8_t status_index = 0; + uint8_t status_index = 0; uint8_t received_fields = 0; uint8_t received_status = 0; - for (auto & driver : drivers_) { + for (auto& driver : drivers_) + { received_fields |= driver.receivedBusVoltage() << StatusBit::BusVoltage; received_fields |= driver.receivedOutVoltage() << StatusBit::OutVoltage; received_fields |= driver.receivedAnalogInput() << StatusBit::AnalogInput; @@ -167,31 +157,36 @@ bool MultiPumaNode::getStatus() received_fields |= driver.receivedTemperature() << StatusBit::Temperature; received_fields |= driver.receivedMode() << StatusBit::Mode; received_fields |= driver.receivedFault() << StatusBit::Fault; - if (received_fields != (1 << StatusBit::Count) - 1) { + if (received_fields != (1 << StatusBit::Count) - 1) + { RCLCPP_DEBUG(this->get_logger(), "Received Status Fields %x", received_fields); - } else { + } + else + { received_status |= 1 << status_index; } status_index++; } - if (received_status != (1 << status_index) - 1) { + if (received_status != (1 << status_index) - 1) + { RCLCPP_DEBUG(this->get_logger(), "Received Status %x", received_status); return false; } // Prepare output status message to ROS. status_index = 0; - for (auto & driver : drivers_) { - clearpath_motor_msgs::msg::PumaStatus * s = &status_msg_.drivers[status_index]; - s->device_number = driver.deviceNumber(); - s->device_name = driver.deviceName(); - s->bus_voltage = driver.lastBusVoltage(); - s->output_voltage = driver.lastOutVoltage(); - s->analog_input = driver.lastAnalogInput(); - s->temperature = driver.lastTemperature(); - s->mode = driver.lastMode(); - s->fault = driver.lastFault(); + for (auto& driver : drivers_) + { + clearpath_motor_msgs::msg::PumaStatus* s = &status_msg_.drivers[status_index]; + s->device_number = driver.deviceNumber(); + s->device_name = driver.deviceName(); + s->bus_voltage = driver.lastBusVoltage(); + s->output_voltage = driver.lastOutVoltage(); + s->analog_input = driver.lastAnalogInput(); + s->temperature = driver.lastTemperature(); + s->mode = driver.lastMode(); + s->fault = driver.lastFault(); status_index++; } @@ -201,27 +196,36 @@ bool MultiPumaNode::getStatus() void MultiPumaNode::publishFeedback() { - if (getFeedback()) { + if (getFeedback()) + { feedback_pub_->publish(feedback_msg_); } } void MultiPumaNode::publishStatus() { - if (getStatus()) { + if (getStatus()) + { status_pub_->publish(status_msg_); } } void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { - if (active_) { - for (auto & driver : drivers_) { - for (int i = 0; i < static_cast(msg->name.size()); i++) { - if (driver.deviceName() == msg->name[i]) { - if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + if (active_) + { + for (auto& driver : drivers_) + { + for (int i = 0; i < static_cast(msg->name.size()); i++) + { + if (driver.deviceName() == msg->name[i]) + { + if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) + { driver.commandDutyCycle(msg->velocity[i]); - } else if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { + } + else if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) + { driver.commandSpeed(msg->velocity[i]); } } @@ -232,8 +236,10 @@ void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr ms bool MultiPumaNode::areAllActive() { - for (auto & driver : drivers_) { - if (!driver.isConfigured()) { + for (auto& driver : drivers_) + { + if (!driver.isConfigured()) + { return false; } } @@ -242,66 +248,79 @@ bool MultiPumaNode::areAllActive() void MultiPumaNode::run() { - if (active_) { + if (active_) + { // Checks to see if power flag has been reset for each driver - for (auto & driver : drivers_) { - if (driver.lastPower() != 0) { + for (auto& driver : drivers_) + { + if (driver.lastPower() != 0) + { active_ = false; RCLCPP_WARN(this->get_logger(), - "Power reset detected on device ID %d, will reconfigure all drivers.", - driver.deviceNumber()); - for (auto & driver : drivers_) { + "Power reset detected on device ID %d, will reconfigure all drivers.", + driver.deviceNumber()); + for (auto& driver : drivers_) + { driver.resetConfiguration(); } } } // Queue data requests for the drivers in order to assemble an amalgamated status message. - for (auto & driver : drivers_) { + for (auto& driver : drivers_) + { driver.requestStatusMessages(); driver.requestFeedbackSetpoint(); } - } else { + } + else + { // Set parameters for each driver. - for (auto & driver : drivers_) { + for (auto& driver : drivers_) + { driver.configureParams(); } } // Process all received messages through the connected driver instances. - while (interface_->recv(recv_msg_)) { - for (auto & driver : drivers_) { + while (interface_->recv(recv_msg_)) + { + for (auto& driver : drivers_) + { driver.processMessage(recv_msg_); } } // Check parameters of each driver instance. - if (!active_) { - for (auto & driver : drivers_) { + if (!active_) + { + for (auto& driver : drivers_) + { driver.verifyParams(); } } // Verify that the all drivers are configured. - if (areAllActive() == true && active_ == false) { + if (areAllActive() == true && active_ == false) + { active_ = true; RCLCPP_INFO(this->get_logger(), "All controllers active."); } // Broadcast feedback and status messages - if (active_) { + if (active_) + { publishFeedback(); publishStatus(); } status_count_++; } -int main(int argc, char * argv[]) +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::executors::MultiThreadedExecutor exe; - std::shared_ptr multi_puma_node = - std::make_shared("multi_puma_node"); + std::shared_ptr multi_puma_node = std::make_shared("multi_puma_node"); exe.add_node(multi_puma_node); exe.spin();