580 lines
16 KiB
C
580 lines
16 KiB
C
/**
|
|
* @file sdl.c
|
|
*
|
|
* @copyright Nehemis SARL reserves all rights even in the event of industrial
|
|
* property rights. We reserve all rights of disposal such as
|
|
* copying and passing on to third parties.
|
|
*
|
|
* @brief This file contains the initialization and execution logic for
|
|
* the Valve Controller and CANopen stack. It defines the main
|
|
* scheduler loop for running these components within the firmware.
|
|
*
|
|
*/
|
|
|
|
/******************************************************************************
|
|
* Include Header Files
|
|
******************************************************************************/
|
|
/* CANopen includes */
|
|
#include <co_canopen.h>
|
|
#include <gen_define.h>
|
|
#include <co_canopen.h>
|
|
#include "co_sdo.h"
|
|
#include "co_odaccess.h"
|
|
|
|
/* User includes */
|
|
#include "sdl.h"
|
|
#include "processBoard.h"
|
|
#include "hal_system.h"
|
|
|
|
#include "stdlib.h"
|
|
/******************************************************************************
|
|
* Macro constant declarations
|
|
******************************************************************************/
|
|
#define SDL_CANOPEN_BITRATE 250u
|
|
#define SDL_PU_MASTER_NODE_ID 1u
|
|
#define SDL_LSS_TIMEOUT 20u /* Defined in stack examples */
|
|
#define SDL_LSS_CONFIG_TIMEOUT 1000u
|
|
#define SDL_LSS_CONFIG_INTERVAL 4u
|
|
#define SDL_APPL_TIMER_TIME 250000uL
|
|
#define SDL_LSS_NODE_COUNT 20u
|
|
#define SDL_LSS_NODE_HB_MS 1000uL
|
|
#define SDL_LSS_NODE_STATE_RESET_INTERVAL 3u
|
|
|
|
#define SDL_POSITION_SETPOINT_INDEX 0x6002
|
|
#define SDL_POSITION_SETPOINT_SUB_INDEX 0x0
|
|
#define SDL_POSITION_FEEDBACK_INDEX 0x6004
|
|
#define SDL_POSITION_FEEDBACK_SUB_INDEX 0x0
|
|
/******************************************************************************
|
|
* Type Declarations
|
|
******************************************************************************/
|
|
typedef struct
|
|
{
|
|
uint32 vendorId_u32;
|
|
uint32 productId_u32;
|
|
uint32 versionNbr_u32;
|
|
uint32 serialNbr_u32;
|
|
uint8 nodeId_u8;
|
|
} SdlLssNodeInfo_t;
|
|
/******************************************************************************
|
|
* Global Declarations
|
|
******************************************************************************/
|
|
SdlLssNodeInfo_t var_gst;
|
|
static CO_TIMER_T monitorTimer_gst; /**< application timer */
|
|
static CO_TIMER_T nodeResetTimer_gst; /**< node reset timer */
|
|
static bool monitorSleep_gb = CO_FALSE; /**< sleep flag */
|
|
static bool masterStarted_gb; /**< master started flag */
|
|
static const SdlLssNodeInfo_t nodeLookupTable_gast[SDL_LSS_NODE_COUNT] =
|
|
{
|
|
{0x319, 0x0, 0x0, 0x0, 0x12},
|
|
{0x319, 0x0, 0x3, 0x0, 0x6},
|
|
{0x319, 0x2, 0x0, 0x0, 0x7},
|
|
{0x319, 0x4d2, 0x1, 0x0, 0x18},
|
|
{0x31, 0x4d2, 0x5, 0x0, 0x9},
|
|
{0x3, 0x4d2, 0x6, 0x0, 0x10},
|
|
{0x1, 0x4d2, 0x7, 0x0, 0x11}
|
|
};
|
|
static CO_NMT_STATE_T nodeNMTState_gaen[SDL_LSS_NODE_COUNT];
|
|
/******************************************************************************
|
|
* Static function Declarations
|
|
******************************************************************************/
|
|
static void SdlInitCanopen(void);
|
|
static void SdlRunCanopen(void);
|
|
static RET_T SdlOverwriteLoadIndication(uint8 index_u8);
|
|
static void SdlLssIndCallback(CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32);
|
|
static void SdlLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en);
|
|
static void SdlLssResetNodesCallback(void *pData_pv); /**< callback of reset handler */
|
|
static uint8 SdlLssGetNodeIndex(uint8 nodeId_u8);
|
|
static void SdlLssToggleMonitorFlag(void *pData);
|
|
static void SdlLssNodeHandlerRun(void);
|
|
static uint32 SdlLssConfigureNode(uint8 arrIndex_u8);
|
|
static RET_T SdlLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8);
|
|
/* Supress warnings for implicit declaration.
|
|
* Need this function for overwriting the manual COB ids
|
|
* for the TPDO. */
|
|
extern void userOverwriteCobIdSettings(void);
|
|
|
|
static void SdlEnduranceTestBenchRun(void);
|
|
/******************************************************************************
|
|
* Public Function Definitions
|
|
******************************************************************************/
|
|
void SdlInit(void)
|
|
{
|
|
SdlInitCanopen();
|
|
}
|
|
|
|
|
|
void SdlRun(void)
|
|
{
|
|
SdlRunCanopen();
|
|
SdlLssNodeHandlerRun();
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
* Private Function Declarations
|
|
******************************************************************************/
|
|
/**
|
|
* @brief Init the CANopen stack.
|
|
*
|
|
*/
|
|
static void SdlInitCanopen(void)
|
|
{
|
|
HalSystemInit();
|
|
codrvHardwareInit();
|
|
codrvCanInit(SDL_CANOPEN_BITRATE);
|
|
codrvTimerSetup(CO_TIMER_INTERVAL);
|
|
|
|
/* CANopen Initialization */
|
|
coCanOpenStackInit(SdlOverwriteLoadIndication);
|
|
coEventRegister_LSS_MASTER(SdlLssIndCallback);
|
|
coEventRegister_ERRCTRL(SdlLssHbMonitorCallback);
|
|
|
|
/* enable CAN communication */
|
|
codrvCanEnable();
|
|
coTimerStart(&monitorTimer_gst, SDL_APPL_TIMER_TIME, SdlLssToggleMonitorFlag, NULL, CO_TIMER_ATTR_ROUNDUP_CYCLIC);
|
|
coTimerStart(&nodeResetTimer_gst, (SDL_LSS_NODE_STATE_RESET_INTERVAL * 1000000uLL), SdlLssResetNodesCallback, NULL, CO_TIMER_ATTR_ROUNDUP_CYCLIC);
|
|
coNmtStateReq(SDL_PU_MASTER_NODE_ID, CO_NMT_STATE_OPERATIONAL, CO_TRUE);
|
|
|
|
coLssIdentifyNonConfiguredSlaves(SDL_LSS_CONFIG_TIMEOUT, SDL_LSS_CONFIG_INTERVAL);
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Runs the CANopen stack.
|
|
*
|
|
*/
|
|
static void SdlRunCanopen(void)
|
|
{
|
|
coCommTask();
|
|
SdlLssNodeHandlerRun();
|
|
SdlEnduranceTestBenchRun();
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
* Private Function Definitions
|
|
******************************************************************************/
|
|
/**
|
|
* @brief Handler for the defined nodes in the network.
|
|
* This function starts the configuration of a node if needed
|
|
*
|
|
* @return void
|
|
*
|
|
*/
|
|
static void SdlLssNodeHandlerRun(void)
|
|
{
|
|
if (monitorSleep_gb != CO_TRUE)
|
|
{
|
|
for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_PREOP)
|
|
{
|
|
SdlLssConfigureNode(i_u8);
|
|
}
|
|
if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_OPERATIONAL)
|
|
{
|
|
/* Do nothing */
|
|
}
|
|
}
|
|
}
|
|
monitorSleep_gb = CO_TRUE;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Load function for overwriing the TPDO COB id values.
|
|
*
|
|
* @param index_u8 Subindex parameter to point parameter area(unused)
|
|
*
|
|
* @return Error Code
|
|
*/
|
|
static RET_T SdlOverwriteLoadIndication(uint8 index_u8)
|
|
{
|
|
userOverwriteCobIdSettings();
|
|
|
|
return RET_OK;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief This function is called if a new NMT state of a node,
|
|
* is recordnized by the CANopen stack.
|
|
*
|
|
* @param nodeID_u8 Node id of the node that has registered a change.
|
|
* state_en Error control state
|
|
* nmtState_en NMT state of the particular node.
|
|
*
|
|
* @return Array index of the node in the network.
|
|
*
|
|
*/
|
|
static uint8 SdlLssGetNodeIndex(uint8 nodeId_u8)
|
|
{
|
|
uint8 arrIndex_u8;
|
|
|
|
/* find node ID array arrIndex_u8 */
|
|
for (arrIndex_u8 = 0u; arrIndex_u8 < SDL_LSS_NODE_COUNT; arrIndex_u8++)
|
|
{
|
|
if (nodeId_u8 == nodeLookupTable_gast[arrIndex_u8].nodeId_u8)
|
|
{
|
|
return arrIndex_u8;
|
|
}
|
|
}
|
|
|
|
return NMS_UINT8_MAX;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief This function is called if a new NMT state of a node,
|
|
* is recordnized by the CANopen stack.
|
|
*
|
|
* @param nodeID_u8 Node id of the node that has registered a change.
|
|
* state_en Error control state
|
|
* nmtState_en NMT state of the particular node.
|
|
*
|
|
* @return Array index of the node in the network.
|
|
*
|
|
*/
|
|
static void SdlLssToggleMonitorFlag(void *pData)
|
|
{
|
|
(void)pData;
|
|
|
|
/* deactivate application sleep flag */
|
|
monitorSleep_gb = CO_FALSE;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Configure a specific node.
|
|
* This function adds the defined node to heartbeat consumers,
|
|
* and sets its heartbeat interval.
|
|
*
|
|
* @param arrIndex_u8 Index of the node in the array which is to be configured.
|
|
*
|
|
* @return Status of the operation.
|
|
*
|
|
*/
|
|
static uint32 SdlLssConfigureNode(uint8 arrIndex_u8)
|
|
{
|
|
uint32 error_u32 = SDL_DEFAULT_ERROR;
|
|
RET_T retVal_en = RET_INTERNAL_ERROR;
|
|
uint16 hb_u16 = 500u;
|
|
/* Get the node ID from the lookup table using arrIndex_u8 */
|
|
uint8 nodeId_u8 = nodeLookupTable_gast[arrIndex_u8].nodeId_u8;
|
|
|
|
/* Add node to hb consumers with +25% tolerance
|
|
* Rationale - Allows some flexibility in detecting timeouts due to minor clock drifts,
|
|
* bus delays, or transmission issues.*/
|
|
retVal_en = coHbConsumerSet(nodeId_u8,
|
|
((SDL_LSS_NODE_HB_MS / 100 * 25) + SDL_LSS_NODE_HB_MS));
|
|
retVal_en = coHbConsumerStart(nodeId_u8);
|
|
|
|
/* setup SDO channel */
|
|
retVal_en = SdlLssSetSdoCobID((arrIndex_u8 + 1), nodeLookupTable_gast[arrIndex_u8].nodeId_u8);
|
|
|
|
retVal_en = coSdoWrite((arrIndex_u8 + 1), 0x1017u, 0u, (uint8*)(&hb_u16), 2u, CO_FALSE, 1000u);
|
|
|
|
/* start node */
|
|
retVal_en = coNmtStateReq(nodeId_u8, CO_NMT_STATE_OPERATIONAL, CO_FALSE);
|
|
if (retVal_en != RET_OK)
|
|
{
|
|
error_u32 = SDL_LSS_NODE_CONFIG_ERROR;
|
|
}
|
|
/* set local state to operational, if not operational yet */
|
|
if (masterStarted_gb == CO_FALSE)
|
|
{
|
|
/* set local state */
|
|
coNmtLocalStateReq(CO_NMT_STATE_OPERATIONAL);
|
|
|
|
/* save started flag */
|
|
masterStarted_gb = CO_TRUE;
|
|
}
|
|
|
|
return error_u32;
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
* Callback Function Definitions
|
|
******************************************************************************/
|
|
/**
|
|
* @brief LSS indication function for handling the LSS api calls for dynamic
|
|
* setup of node ids.
|
|
*
|
|
* @param service_en LSS master services for indication functions
|
|
* errorCode_u16 Error code in the module
|
|
* errorSpec_u8 Specific error case that has occured
|
|
* pIdentity_pu32 LSS slave identity.
|
|
*
|
|
*/
|
|
static void SdlLssIndCallback(CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32)
|
|
{
|
|
static uint8 matchedIndex_u8 = NMS_UINT8_MAX;
|
|
|
|
if (errorCode_u16 != 0u)
|
|
{
|
|
if (errorCode_u16 == NMS_UINT16_MAX)
|
|
{
|
|
/* ERROR */
|
|
}
|
|
else
|
|
{
|
|
/* ERROR */
|
|
}
|
|
|
|
if (service_en == CO_LSS_MASTER_SERVICE_STORE)
|
|
{
|
|
/* DEBUG INFO */
|
|
coLssSwitchGlobal(CO_LSS_STATE_WAITING);
|
|
}
|
|
return;
|
|
}
|
|
|
|
switch (service_en)
|
|
{
|
|
case CO_LSS_MASTER_SERVICE_NON_CONFIG_SLAVE:
|
|
/* DEBUG INFO */
|
|
coLssFastScan(SDL_LSS_TIMEOUT);
|
|
break;
|
|
|
|
case CO_LSS_MASTER_SERVICE_FASTSCAN:
|
|
/* Match detected node with lookup table */
|
|
for (uint8 i_u8 = 0; i_u8 < SDL_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
if (pIdentity_pu32[0] == nodeLookupTable_gast[i_u8].vendorId_u32 &&
|
|
pIdentity_pu32[1] == nodeLookupTable_gast[i_u8].productId_u32 &&
|
|
pIdentity_pu32[2] == nodeLookupTable_gast[i_u8].versionNbr_u32 &&
|
|
pIdentity_pu32[3] == nodeLookupTable_gast[i_u8].serialNbr_u32)
|
|
{
|
|
coLssSwitchSelective(pIdentity_pu32[0], pIdentity_pu32[1],
|
|
pIdentity_pu32[2], pIdentity_pu32[3], 20);
|
|
matchedIndex_u8 = i_u8;
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case CO_LSS_MASTER_SERVICE_SWITCH_SELECTIVE:
|
|
if (matchedIndex_u8 < SDL_LSS_NODE_COUNT)
|
|
{
|
|
coLssSetNodeId(nodeLookupTable_gast[matchedIndex_u8].nodeId_u8, SDL_LSS_TIMEOUT);
|
|
matchedIndex_u8 = NMS_UINT8_MAX;
|
|
}
|
|
else
|
|
{
|
|
/* ERROR */
|
|
}
|
|
break;
|
|
|
|
case CO_LSS_MASTER_SERVICE_SET_NODEID:
|
|
/* DEBUG INFO */
|
|
coLssInquireNodeId(SDL_LSS_TIMEOUT);
|
|
break;
|
|
|
|
case CO_LSS_MASTER_SERVICE_INQUIRE_NODEID:
|
|
/* DEBUG INFO */
|
|
coLssStoreConfig(200);
|
|
break;
|
|
|
|
case CO_LSS_MASTER_SERVICE_STORE:
|
|
/* DEBUG INFO */
|
|
coLssSwitchGlobal(CO_LSS_STATE_WAITING);
|
|
break;
|
|
|
|
default:
|
|
/* ERROR */
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief This function is called if a new NMT state of a node,
|
|
* is recordnized by the CANopen stack.
|
|
*
|
|
* @param nodeID_u8 Node id of the node that has registered a change.
|
|
* state_en Error control state
|
|
* nmtState_en NMT state of the particular node.
|
|
*
|
|
* @return void
|
|
*
|
|
*/
|
|
static void SdlLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en)
|
|
{
|
|
uint8 arrIndex_u8;
|
|
|
|
/* look if node is monitored */
|
|
arrIndex_u8 = SdlLssGetNodeIndex(nodeID_u8);
|
|
|
|
/* handle monitored node */
|
|
if (arrIndex_u8 != NMS_UINT16_MAX)
|
|
{
|
|
/* save states */
|
|
nodeNMTState_gaen[arrIndex_u8] = nmtState_en;
|
|
|
|
/* indicate if monitored node lost heartbeat */
|
|
if (nmtState_en == CO_NMT_STATE_UNKNOWN)
|
|
{
|
|
/* To be transmitted via CAN */
|
|
/* ERROR */
|
|
}
|
|
|
|
/* indicate if monitored node sent a bootup message */
|
|
if (state_en == CO_ERRCTRL_BOOTUP)
|
|
{
|
|
/* INFO */
|
|
}
|
|
|
|
/* handle unmonitored node */
|
|
}
|
|
else
|
|
{
|
|
/* ERROR */
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief This function tries to reset nodes with unknown NMT state.
|
|
*
|
|
* @param pData_pv Data.
|
|
*/
|
|
static void SdlLssResetNodesCallback(void *pData_pv)
|
|
{
|
|
uint8 arrIndex_u8;
|
|
|
|
(void)pData_pv;
|
|
|
|
/* reset defined nodes without known state */
|
|
for (arrIndex_u8 = 0u; arrIndex_u8 < 3; arrIndex_u8++)
|
|
{
|
|
if ((nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_PREOP) &&
|
|
(nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_OPERATIONAL))
|
|
{
|
|
/* reset node */
|
|
coNmtStateReq(nodeLookupTable_gast[arrIndex_u8].nodeId_u8, CO_NMT_STATE_RESET_NODE, CO_FALSE);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief This function configures a SDO client for the
|
|
* given node ID and SDO number.
|
|
*
|
|
* @param sdoNr_u8 Sdo number
|
|
* nodeId_u8 Node id of the node.
|
|
*
|
|
* @return Error state.
|
|
*
|
|
*/
|
|
static RET_T SdlLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8)
|
|
{
|
|
uint16 cobIndex_u16;
|
|
uint32 newCobId_u32;
|
|
RET_T retVal = RET_INTERNAL_ERROR;
|
|
|
|
/* get od index of sdoNr */
|
|
cobIndex_u16 = 0x1280u + sdoNr_u8 - 1u;
|
|
|
|
/* set cobID for client to server direction (request) */
|
|
newCobId_u32 = 0x600u + nodeId_u8;
|
|
retVal = coOdSetCobid(cobIndex_u16, 1u, newCobId_u32);
|
|
|
|
if (retVal == RET_OK)
|
|
{
|
|
/* set cobID for server to client direction (response) */
|
|
newCobId_u32 = 0x580u + nodeId_u8;
|
|
retVal = coOdSetCobid(cobIndex_u16, 2u, newCobId_u32);
|
|
}
|
|
|
|
/* print failed setup details */
|
|
if (retVal != RET_OK)
|
|
{
|
|
/* ERROR */
|
|
}
|
|
|
|
return retVal;
|
|
}
|
|
|
|
|
|
static void SdlEnduranceTestBenchRun(void)
|
|
{
|
|
RET_T retVal_en = RET_INTERNAL_ERROR;
|
|
uint16 max_u16 = NMS_UINT8_MAX;
|
|
uint16 min_u16 = 0u;
|
|
static uint64 startTime_u64 = 0uLL;
|
|
uint8 alternate_u8 = 0u;
|
|
uint64 currentTime_u64;
|
|
|
|
|
|
if(startTime_u64 == 0uLL)
|
|
{
|
|
HalSystemGetRunTimeMs(&startTime_u64);
|
|
srand(startTime_u64);
|
|
}
|
|
HalSystemGetRunTimeMs(¤tTime_u64);
|
|
if(currentTime_u64 - startTime_u64 <= 10000uLL)
|
|
{
|
|
if(alternate_u8 == 0u)
|
|
{
|
|
for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT/2; i_u8++)
|
|
{
|
|
retVal_en = coSdoWrite((i_u8 + 1), SDL_POSITION_SETPOINT_INDEX, SDL_POSITION_SETPOINT_SUB_INDEX, (uint8*)(&max_u16), 2u, CO_FALSE, 1000u);
|
|
}
|
|
for (uint8 i_u8 = 10u; i_u8 < SDL_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
retVal_en = coSdoWrite((i_u8 + 1), SDL_POSITION_SETPOINT_INDEX, SDL_POSITION_SETPOINT_SUB_INDEX, (uint8*)(&min_u16), 2u, CO_FALSE, 1000u);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT/2; i_u8++)
|
|
{
|
|
retVal_en = coSdoWrite((i_u8 + 1), SDL_POSITION_SETPOINT_INDEX, SDL_POSITION_SETPOINT_SUB_INDEX, (uint8*)(&min_u16), 2u, CO_FALSE, 1000u);
|
|
}
|
|
for (uint8 i_u8 = 10u; i_u8 < SDL_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
retVal_en = coSdoWrite((i_u8 + 1), SDL_POSITION_SETPOINT_INDEX, SDL_POSITION_SETPOINT_SUB_INDEX, (uint8*)(&max_u16), 2u, CO_FALSE, 1000u);
|
|
}
|
|
}
|
|
uint16 readPos_u16 = 0u;
|
|
bool allMotorsReached_b = false;
|
|
|
|
while (!allMotorsReached_b)
|
|
{
|
|
allMotorsReached_b = true; // Assume all motors have reached the position
|
|
for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
retVal_en = coSdoRead((i_u8 + 1), SDL_POSITION_SETPOINT_INDEX, SDL_POSITION_SETPOINT_SUB_INDEX, (uint8*)(&readPos_u16), 2u, CO_FALSE, 1000u);
|
|
if (alternate_u8 == 0u)
|
|
{
|
|
if ((i_u8 < SDL_LSS_NODE_COUNT / 2 && readPos_u16 != max_u16) ||
|
|
(i_u8 >= SDL_LSS_NODE_COUNT / 2 && readPos_u16 != min_u16))
|
|
{
|
|
allMotorsReached_b = false; // Keep waiting
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ((i_u8 < SDL_LSS_NODE_COUNT / 2 && readPos_u16 != min_u16) ||
|
|
(i_u8 >= SDL_LSS_NODE_COUNT / 2 && readPos_u16 != max_u16))
|
|
{
|
|
allMotorsReached_b = false; // Keep waiting
|
|
}
|
|
}
|
|
}
|
|
}
|
|
alternate_u8 = !alternate_u8;
|
|
}
|
|
else
|
|
{
|
|
for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
uint16 randomPos_u16 = (uint16)(rand() % (NMS_UINT8_MAX + 1));
|
|
retVal_en = coSdoWrite((i_u8 + 1), SDL_POSITION_SETPOINT_INDEX, SDL_POSITION_SETPOINT_SUB_INDEX, (uint8*)(&randomPos_u16), 2u, CO_FALSE, 1000u);
|
|
}
|
|
}
|
|
|
|
}
|