...
 
......@@ -8,18 +8,12 @@
#ifndef EMROS_EMROS_INC_BASECONTROLLER_H_
#define EMROS_EMROS_INC_BASECONTROLLER_H_
#include "../../emros_common/include/emros_common/wheel_drive_config.h"
#include "../../emros_common/include/emros_common/emros_commands.h"
#include "../../../nanotec/inc/nanoj.h"
#include <cmath>
#include "../../utility_ofc/include/utility_ofc/leaky_bucket_timeout.h"
#include "../../emros_common/include/emros_common/kinematics_kind_configuration.h"
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include <cstring>
#include "../../motors/inc/can_helper.h"
#include "../../utility_ofc/include/utility_ofc/ethprotocol.h"
#include "ramp_generator.h"
#include "../../system/inc/factories.h"
#include "../../system/inc/msg.h"
#include "ramp_generator.h"
#include "basecontroller_types.h"
#ifdef DEF_4WD
#include "../../motors/inc/pdo_mapping_4WD.h"
......@@ -28,32 +22,20 @@
#elif defined DEF_2WD_PLUS_AUX
#include "../../motors/inc/pdo_mapping_2WD_plus_auxiliary.h"
#endif
#include <vector>
#include "../../system/inc/msg.h"
using namespace EMROSCmdType::Basecontroller;
class Basecontroller
{
public:
// struct Status
// {
// static const uint8_t Operational_enabled = 0x01;
// static const uint8_t Fault = 0x02;
// static const uint8_t Sync = 0x04;
// static const uint8_t Quick_stop = 0x08;
// };
Basecontroller();
~Basecontroller();
Error CheckButton(os_msg_t& msg);
private:
Error MsgBoxCallback(const MsgBoxHandler::Dataframe& dataframe);
Error MsgBoxCallbackBroadcast(const MsgBoxHandler::Dataframe& dataframe);
......@@ -66,7 +48,6 @@ private:
InstanceFactory& mInstanceFactory;
CanHelper& mCanHelper;
EthHandler::DeviceId mDevice_id;
bool mRos_link_enabled;
RampGenerator<TargetVelType>::RampConfig mBase_config;
......@@ -74,7 +55,7 @@ private:
bool mCSV_mode_enabled;
bool mHearbeat;
bool mHeartbeat;
bool mCritical_fault;
......@@ -83,10 +64,9 @@ private:
//---------------CANopen objects---------------
//read
MotorStatus* mStatus_word; // 6041
int8_t* mCAN_modes_of_operation_display; // 6061
//---------------------------------------------
std::vector<MotorPdoMapping*>* mMotors;
std::vector<Motor*>* mMotors;
TargetVelocitiesInterface* mTargetVelocities;
TargetVelocitiesInterface* mRampedVelocities;
ActualVelocitiesInterface* mActualVelocities;
......
......@@ -14,7 +14,6 @@
#include <cmath>
#include "../../system/inc/factories.h"
template<typename VEL_T>
class RampGenerator
{
......
This diff is collapsed.
#pragma once
#include "../../emros_common/include/emros_common/emros_commands.h"
#include "../../emros_common/include/emros_common/kinematics_kind_configuration.h"
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include "../../system/inc/msg.h"
#include "../../system/inc/factories.h"
#ifdef DEF_4WD
#include "../../motors/inc/pdo_mapping_4WD.h"
#elif defined DEF_2WD
#include "../../motors/inc/pdo_mapping_2WD.h"
#elif defined DEF_2WD_PLUS_AUX
#include "../../motors/inc/pdo_mapping_2WD_plus_auxiliary.h"
#endif
using namespace EMROSCmdType::Basecontroller;
class AuxiliaryController
{
public:
AuxiliaryController();
~AuxiliaryController();
Error CheckButton(os_msg_t& msg);
private:
Error MsgBoxCallback(const MsgBoxHandler::Dataframe& dataframe);
Error MsgBoxCallbackBroadcast(const MsgBoxHandler::Dataframe& dataframe);
Error EthMsgCallback(const EthHandler::Dataframe& dataframe);
Error FilterAndSendActualVelocitiesCallback();
Error CyclicCANSyncCallback();
Error CyclicTimerCallback();
void UpdateVelocities();
InstanceFactory& mInstanceFactory;
EthHandler::DeviceId mDevice_id;
bool mRos_link_enabled;
bool mCSV_mode_enabled;
bool mHeartbeat;
bool mCritical_fault;
//---------------CANopen objects---------------
//read
MotorStatus* mStatus_word; // 6041
//---------------------------------------------
std::vector<Motor*>* mMotors;
TargetVelocitiesInterface* mTargetVelocities;
TargetVelocitiesInterface* mRampedVelocities;
ActualVelocitiesInterface* mActualVelocities;
ActualVelocitiesInterface* mAverageVelocities;
EthHandler::ObserverEthHandler mObserver_eth;
MsgBoxHandler::ObserverMsgBox mObserver_msg_box;
MsgBoxHandler::ObserverMsgBox mObserver_msg_box_broadcast;
CANSyncHandler::ObserverSyncHandler mObserver_delayed_can_sync;
CANSyncHandler::ObserverSyncHandler mObserver_cyclic_can_sync;
};
Subproject commit 0325cd0a0ca961f9228f99de8050dfea98ec1f65
Subproject commit fa6fd21ca38fa63730aeb2010c4386015f5a827b
......@@ -17,15 +17,11 @@
#include "../../NanoOFC/logger/inc/ofc_logger_server.h"
#include "../../basecontroller/inc/basecontroller.h"
#include "../../devices/inc/service.h"
//#include "devices.h"
#include <cstring>
#include "../../../nanotec/inc/tools.h"
#include "../../system/inc/msg.h"
#include "../../utility_ofc/include/utility_ofc/singleton.h"
#include "../../motors/inc/can_helper.h"
#include "../../emros_common/include/emros_common/emros_commands.h"
#include "../../devices/inc/powermanagement.h"
constexpr float RADS_TO_UMIN = noexcept(60 / (2 * M_TWOPI));
......@@ -57,7 +53,7 @@ private:
protected:
Interface() :
mButton_names(), mButton_handler(), mBoxes(), mFileHandler(), mPage(), mLoggerServer(*OFCLoggerServer::instance()),mCanHelper(*CanHelper::instance())
mButton_names(), mButton_handler(), mBoxes(), mFileHandler(), mPage(), mLoggerServer(*OFCLoggerServer::instance())
{
}
......@@ -112,9 +108,6 @@ protected:
guipage_t *mPage;
OFCLoggerServer& mLoggerServer;
//LoggerClient *mLoggerClient;
CanHelper& mCanHelper;
//PowerManagemant::power_data_t mPower_data;
};
......@@ -122,10 +115,10 @@ class MecanumGUI final : public Interface
{
public:
MecanumGUI():
mMotor_front_left(*MotorManagerWD::instance()->getMotor("MotorFrontLeft")),
mMotor_front_right(*MotorManagerWD::instance()->getMotor("MotorFrontRight")),
mMotor_rear_left(*MotorManagerWD::instance()->getMotor("MotorRearLeft")),
mMotor_rear_right(*MotorManagerWD::instance()->getMotor("MotorRearRight"))
mMotor_front_left(*MotorManagerWD::instance()->GetMotor("MotorFrontLeft")),
mMotor_front_right(*MotorManagerWD::instance()->GetMotor("MotorFrontRight")),
mMotor_rear_left(*MotorManagerWD::instance()->GetMotor("MotorRearLeft")),
mMotor_rear_right(*MotorManagerWD::instance()->GetMotor("MotorRearRight"))
{
Graphics::SelectLayer(LAYER_TOP);
Graphics::ClearScreen(COLOR_BLACK);
......@@ -156,8 +149,8 @@ class DifferentialGUI final : public Interface
public:
DifferentialGUI():
mMotor_left(*MotorManagerWD::instance()->getMotor("MotorLeft")),
mMotor_right(*MotorManagerWD::instance()->getMotor("MotorRight")),
mMotor_left(*MotorManagerWD::instance()->GetMotor("MotorLeft")),
mMotor_right(*MotorManagerWD::instance()->GetMotor("MotorRight")),
mPowermanagement(*PowerManagement::instance())
{
Graphics::SelectLayer(LAYER_TOP);
......
......@@ -13,7 +13,7 @@
#include "../../utility_ofc/include/utility_ofc/timeout.h"
//#include "../../devices/inc/powermanagement.h"
#include "sequential_sdo_helper.h"
#include "../../emros_common/include/emros_common/wheel_drive_config.h"
#include "../../emros_common/include/emros_common/kinematics_kind_configuration.h"
class BringupHelper {
public:
......@@ -26,6 +26,7 @@ public:
Error CanSetup(void);
void TriggerDisplayRefresh(void);
void PrintFirmwareVersion(const SdoHandler::NodeId node_id);
void EnterAndStayInEndlessErrorHandlerLoop(void);
private:
OFCLoggerRTClient& myLogger { *OFCLoggerRTClient::instance() };
Timeout<uint32_t, uint32_t, uint32_t (*)()> timeout_PowerboardBringup;
......
......@@ -13,7 +13,9 @@
#include "../../utility_ofc/include/utility_ofc/timeout.h"
//#include "../../devices/inc/powermanagement.h"
class SequentialSdoHelper {
class SequentialSdoHelper final: public Singleton<SequentialSdoHelper>
{
friend class Singleton<SequentialSdoHelper> ;
public:
SequentialSdoHelper();
......
......@@ -8,9 +8,9 @@
#include "../inc/bringup_helper.h"
namespace {
const uint32_t timeout_BootupPowerboard_us = 3E6;
const uint32_t timeout_BootupPowerboard_us = 1E6;
const uint32_t timeout_can_reset_us = 3E6;
const uint32_t assumed_duration_node_bootup_ms = 2E3;
const uint32_t assumed_duration_node_bootup_ms = 5E2;
const SdoHandler::NodeId node_id_powerboard = 0xA;
const SdoHandler::NodeId node_id_Motor_1 = 0x1;
const SdoHandler::NodeId node_id_Motor_2 = 0x2;
......@@ -73,7 +73,8 @@ Error BringupHelper::BringupPowerboard(void){
TriggerDisplayRefresh();
return Error::NO_ERROR;
}else{
myLogger.InsertError("Powerboard communication boot-up failed.");
myLogger.InsertError("Start communication with Powerboard failed.");
myLogger.InsertCriticalError("Bring-up of Powerboard failed.");
TriggerDisplayRefresh();
return Error::ERROR;
}
......@@ -101,6 +102,8 @@ Error BringupHelper::BootMotors(void) {
return Error::NO_ERROR;
} else {
myLogger.InsertError("There is something wrong, not all motors booted");
TriggerDisplayRefresh();
return Error::ERROR;
}
}
......@@ -196,3 +199,12 @@ void BringupHelper::PrintFirmwareVersion(const SdoHandler::NodeId node_id){
}
void BringupHelper::EnterAndStayInEndlessErrorHandlerLoop(void){
myLogger.InsertWarning("Entering error handler loop");
while(true)
{
TriggerDisplayRefresh();
System::WaitMilliSeconds(100);
}
}
......@@ -21,9 +21,9 @@
#include "devices/inc/security.h"
#include "gui/inc/emros_gui.h"
#include "utility_ofc/include/utility_ofc/map_extractor.h"
#include "emros_common/include/emros_common/wheel_drive_config.h"
#include "helper/inc/bringup_helper.h"
#include "devices/inc/powermanagement.h"
#include "emros_common/include/emros_common/kinematics_kind_configuration.h"
namespace {
const char ip_ros_pc[] ="192.168.60.55"; // <-- PLEASE SET THE IP OF YOUR MACHINE HERE!
......@@ -33,11 +33,10 @@ const uint16_t port_ros_pc = 65000;
const uint32_t timeout_can_bootup_us = 1E6;
const uint32_t timeout_rt_task_mainloop_ms = 10;
const uint32_t timeout_user_task_mainloop_ms = 50;
const uint32_t timeout_user_task_initialloop_ms = 200;
}
//-----------------------------CHECK DRIVE CONFIG------------------------------//
//-----------------------------CHECK DRIVE CONFIG-----------------------------//
#if !defined (DEF_4WD) && !defined (DEF_2WD) && !defined (DEF_2WD_PLUS_AUX)
#error "DEF_2WD, DEF_2WD_PLUS_AUX and DEF_4WD are not defined. Choose one!!!!!"
#endif
......@@ -65,45 +64,36 @@ void RealtimeTask(void) {
OFCLoggerRTClient& myLogger { *OFCLoggerRTClient::instance() };
EthHandler::instance()->Init(ip_ros_pc, port_ros_pc);// Configure communication with ROS-PC
BringupHelper bringupHelper = BringupHelper();
PowerManagement powerboard = *(PowerManagement::instance());
//-----------------------------POWERMANAGEMENT SETUP----------------------//
//-----------------------------BOOTUP CAN-NETWORK-------------------------//
bringupHelper.CanSetup();
if(bringupHelper.BringupPowerboard() == Error::ERROR){
myLogger.InsertError("Bring-up of Powerboard failed.");
}
System::WaitMilliSeconds(2000); //TODO: Bugfix Powerboard still needs some time after bootup
if (bringupHelper.BootMotors() == Error::ERROR) {
myLogger.InsertError("There is something wrong, not all motors booted");
bringupHelper.EnterAndStayInEndlessErrorHandlerLoop();
}
bringupHelper.BootMotors();
//-----------------------------EMROS SETUP--------------------------------//
#ifdef DEF_4WD
InstanceFactory::instance()->Configure(EMROSCmdType::Basecontroller::ModeOfOperation::FOUR_WD);
InstanceFactory::instance()->Configure(Robot::KindOfKinematics::FOUR_WD);
#endif
#ifdef DEF_2WD
InstanceFactory::instance()->Configure(
EMROSCmdType::Basecontroller::ModeOfOperation::TWO_WD);
InstanceFactory::instance()->Configure(Robot::KindOfKinematics::TWO_WD);
#endif
#ifdef DEF_2WD_PLUS_AUX
InstanceFactory::instance()->Configure(
EMROSCmdType::Basecontroller::ModeOfOperation::TWO_WD_PLUS_AUX);
InstanceFactory::instance()->Configure(Robot::KindOfKinematics::TWO_WD_PLUS_AUX);
#endif
SdoHandler::instance()->Init();
CANSyncHandler::instance()->Init(can_sync_interval_us);
Security security { };
Basecontroller basecontroller = Basecontroller();
Basecontroller basecontroller { };
Robotstatecontroller robotstatecontroller { };
Service service { };
CanHelper::instance()->init();
myLogger.InsertInfo("Bootup finished!");
//-----------------------------MAINLOOP------------------------------//
//-----------------------------MAINLOOP-----------------------------------//
myLogger.InsertInfo("Enable Syncs, PDOs and Timer!");
CANSyncHandler::instance()->StartCanSync();
TimerHandler::instance()->StartTimer(10*timeout_rt_task_mainloop_ms);
......@@ -122,8 +112,8 @@ void RealtimeTask(void) {
MSG_PRIORITY_HIGH); //TODO: This should be handled via TimerHandler
}
SdoHandler::instance()->CheckAndHandleNewSdo(msg);
CANSyncHandler::instance()->CheckAndHandleNewSync(msg);
SdoHandler::instance()->CheckAndHandleNewSdo(msg);
robotstatecontroller.CheckButton(msg); //TODO: This should run in a "Button-Handler"
TimerHandler::instance()->CheckAndHandleNewTimerEvent(msg);
}
......@@ -143,23 +133,23 @@ void UserTask(void) {
os_msg_t msg;
//-----------------------------GUI SETUP------------------------------//
//-----------------------------GUI SETUP----------------------------------//
EMROS_GUI::Interface *emros_gui;
#ifdef DEF_4WD
emros_gui = new EMROS_GUI::MecanumGUI();
#endif
#ifdef DEF_2WD
emros_gui = new EMROS_GUI::DifferentialGUI;
emros_gui = new EMROS_GUI::DifferentialGUI();
#endif
#ifdef DEF_2WD_PLUS_AUX
emros_gui = new EMROS_GUI::DifferentialGUI;
emros_gui = new EMROS_GUI::DifferentialGUI();
#endif
emros_gui->Init();
Graphics::SetFirstDisplayLine(0, LAYER_BOTH); //Focus on application GUI
//Graphics::SetFirstDisplayLine(272, LAYER_BOTH); //Focus on console and system informations
//-----------------------------GUI MAINLOOP------------------------------//
//-----------------------------GUI MAINLOOP-------------------------------//
while (1) {
System::WaitForMsg(timeout_user_task_mainloop_ms);
......
#pragma once
#include "../../emros_common/include/emros_common/wheel_drive_config.h"
#ifdef DEF_4WD
#include "pdo_mapping_4WD.h"
#elif defined DEF_2WD
#include "pdo_mapping_2WD.h"
#elif defined DEF_2WD_PLUS_AUX
#include "pdo_mapping_2WD_plus_auxiliary.h"
#endif
#include "../../system/inc/factories.h"
#include <vector>
#include "../../utility_ofc/include/utility_ofc/singleton.h"
#include "../../utility_ofc/include/utility_ofc/error.h"
#include <cstdint>
#include "../../../nanotec/inc/nanoj.h"
#define MASK_NOT_READY_AND_SWITCH_DISABLED 0x004F
#define MASK_GENERAL 0x006F
#define MASK_FAULT 0x004F
#define NOT_READY_TO_SWITCHED_ON 0x0000
#define SWITCH_ON_DISABLED 0x0040
#define READY_TO_SWITCH_ON 0x0021
#define SWITCHED_ON 0x0023
#define OPERATION_ENABLED 0x0027
#define PROCRESS_PROFILE 0x0010
#define POSITION_REACHED 0x1400
#define FAULT_REACTION_ACTIVE 0x000F
#define FAULT 0x0008
class CanHelper final: public Singleton<CanHelper>
{
friend class Singleton<CanHelper> ;
public:
//TODO: insert init checks into methodes
~CanHelper()
{
}
bool init()
{
if (!mInitialized)
{
mMotors = InstanceFactory::instance()->GetMotors();
if (mMotors != nullptr)
{
mMode_of_operation_display = new int8_t[mMotors->size()];
mInitialized = true;
return (true);
}
else
{
//mMode_of_operation_display = new int8_t[4];
mInitialized = false;
return (false);
}
}
else
{
return (true);
}
}
Error AppEnableCsvMode(void);
Error AppDisableCsvMode(void);
Error AppProcessCsvMode(void);
private:
Error callculateNextControlWordToSwitchUpCia402Statemachine(uint16_t statusword, uint16_t &ctrlword);
int8_t *mMode_of_operation_display;
std::vector<MotorPdoMapping*>* mMotors;
uint8_t mBootupSequence[127];
uint8_t mBootupSequenceLen;
uint8_t mBootupSequenceReadIdx;
bool mPowerEnabled;
bool mAppAutosetupModeEnabled;
bool mInitialized;
protected:
CanHelper() :
mPowerEnabled(false), mAppAutosetupModeEnabled(false), mBootupSequence(), mBootupSequenceLen(0), mBootupSequenceReadIdx(
0), mMotors(nullptr), mMode_of_operation_display(nullptr), mInitialized(false)
{
}
};
......@@ -11,8 +11,12 @@ namespace{
class Motor {
public:
Motor(MotorPdoMapping pdoHandle) :
mPdoHanlde(pdoHandle) {
typedef uint8_t node_id_t;
const node_id_t node_id;
const char * name;
Motor(node_id_t _node_id, const char * _name) :
node_id(_node_id), name(_name) {
}
MotorPdoMapping* getPdoHandle (void){
......@@ -97,11 +101,11 @@ public:
case SimplifiedMotorState::ResetingFault:
OFCLoggerRTClient::instance()->InsertInfo("Try to reset fault of ",
mPdoHanlde.name);
this->name);
mTargetPowerState = PowerState::NotReadyToSwitchOn;
faultResetCounter++;
if (IsActualPowerStateEqualTargetState()) {
OFCLoggerRTClient::instance()->InsertInfo("Fault of ", mPdoHanlde.name, " reseted successfully.");
OFCLoggerRTClient::instance()->InsertInfo("Fault of ", this->name, " reseted successfully.");
mMotorState = SimplifiedMotorState::Disabled;
break;
} else if (faultResetCounter >= maxRetriesFaultReset) {
......@@ -131,13 +135,17 @@ public:
void SetAppConfigOngoing(void) {
mApplicationConfigurationFinished = false;
}
void SetAppConfigFinished(void) {
void SetApplicationConfigFinished(void) {
mApplicationConfigurationFinished = true;
}
bool IsAppConfigFinished(void) {
return mApplicationConfigurationFinished;
}
void SetTargetModeOfOperation(modes_of_operation_specific::type operation_mode) {
mPdoHanlde.setModeOfOperation(operation_mode);
}
PowerState getActualPowerState(void) {
status_word_t readout;
mPdoHanlde.getStatusWord(readout);
......@@ -206,7 +214,7 @@ private:
void LockPowerStateMachine(void)
{
mPowerStateMachineLockedDueToFault = true;
OFCLoggerRTClient::instance()->InsertError("Motor ",mPdoHanlde.name, "(Node-ID ", mPdoHanlde.node_id,
OFCLoggerRTClient::instance()->InsertError("Motor ",this->name, "(Node-ID ", this->node_id,
") is in Failure State");
}
......@@ -435,7 +443,7 @@ private:
}
void ResetEnablePowerRequest(void) {
mPowerEnableRequested = true;
mPowerEnableRequested = false;
}
};
......
#include "motor.h"
#include <vector>
struct cmp_str {
bool operator()(char const *a, char const *b) {
......@@ -12,14 +13,39 @@ public:
virtual ~MotorManager() {
}
Motor* getMotor(const char* value) {
Motor* m { this->mMotors[value] };
Motor* GetMotor(const char* value) {
Motor* m { this->mMotorsMap[value] };
return (m);
}
std::vector<Motor*>* GetMotors()
{
return &mMotorsVector;
}
protected:
MotorManager() {
for(auto const& entry : mMotorsMap )
{
Motor* motor = entry.second;
mMotorsVector.push_back(motor);
}
}
void AddMotor(Motor* motor){
mMotorsVector.push_back(motor);
this->mMotorsMap[motor->name] = motor;
}
std::map<const char*, Motor*, cmp_str> mMotors;
void AddMotors(std::vector<Motor*>* motors){
//mMotorsVector.insert( mMotorsVector.end(), motors->begin(), motors->end());
for (auto const& motor : *motors)
{
AddMotor(motor);
}
}
std::map<const char*, Motor*, cmp_str> mMotorsMap;
std::vector<Motor*> mMotorsVector;
};
#pragma once
#include "pdo_mapping_motors.h"
class MotorRight : public MotorPdoMapping
{
#include "motor_manager.h"
namespace{
Motor::node_id_t NodeIdLeftMotor =1;
Motor::node_id_t NodeIdRightMotor =2;
}
class MotorManagerWD final: public MotorManager,
public Singleton<MotorManagerWD> {
friend class Singleton<MotorManagerWD> ;
public:
MotorRight()
{
this->mode_of_operation_display.updateMapping(0xA000,2);
this->velocity_actual_value.updateMapping(0xA1C0,3);
this->position_actual_value.updateMapping(0xA1C0,4);
this->status_word.updateMapping(0xA100,2);
this->mode_of_operation.updateMapping(0xA480,2);
this->target_velocity.updateMapping(0xA640,2);
this->control_word.updateMapping(0xA580,2);
}
~MotorManagerWD() {
}
private:
Motor mMotorRight;
Motor mMotorLeft;
protected:
MotorManagerWD() :
mMotorLeft(NodeIdLeftMotor, "MotorLeft"),
mMotorRight(NodeIdRightMotor, "MotorRight")
{
mMotorLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,1);
mMotorLeft.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0,1);
mMotorLeft.getPdoHandle()->position_actual_value.updateMapping(0xA1C0,2);
mMotorLeft.getPdoHandle()->status_word.updateMapping(0xA100,1);
mMotorLeft.getPdoHandle()->mode_of_operation.updateMapping(0xA480,1);
mMotorLeft.getPdoHandle()->target_velocity.updateMapping(0xA640,1);
mMotorLeft.getPdoHandle()->control_word.updateMapping(0xA580,1);
mMotorRight.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,2);
mMotorRight.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0,3);
mMotorRight.getPdoHandle()->position_actual_value.updateMapping(0xA1C0,4);
mMotorRight.getPdoHandle()->status_word.updateMapping(0xA100,2);
mMotorRight.getPdoHandle()->mode_of_operation.updateMapping(0xA480,2);
mMotorRight.getPdoHandle()->target_velocity.updateMapping(0xA640,2);
mMotorRight.getPdoHandle()->control_word.updateMapping(0xA580,2);
AddMotor(&mMotorLeft);
AddMotor(&mMotorRight);
}
};
class MotorLeft : public MotorPdoMapping
{
class MotorManagerAux final: public MotorManager,
public Singleton<MotorManagerAux> {
friend class Singleton<MotorManagerAux> ;
public:
MotorLeft()
{
this->mode_of_operation_display.updateMapping(0xA000,1);
this->velocity_actual_value.updateMapping(0xA1C0,1);
this->position_actual_value.updateMapping(0xA1C0,2);
this->status_word.updateMapping(0xA100,1);
this->mode_of_operation.updateMapping(0xA480,1);
this->target_velocity.updateMapping(0xA640,1);
this->control_word.updateMapping(0xA580,1);
}
~MotorManagerAux() {
}
private:
protected:
MotorManagerAux(){
//Placeholder - No auxiliary motors used
}
};
class MotorManagerWD final: public MotorManager, public Singleton<MotorManagerWD>
{
friend class Singleton<MotorManagerWD>;
class MotorManagerAllMotors final: public MotorManager,
public Singleton<MotorManagerAllMotors> {
friend class Singleton<MotorManagerAllMotors> ;
public:
~MotorManagerWD()
{
}
~MotorManagerAllMotors() {
}
private:
MotorRight mMotorRight;
MotorLeft mMotorLeft;
protected:
MotorManagerWD()
{
this->mMotors["MotorRight"] = &mMotorRight;
this->mMotors["MotorLeft"] = &mMotorLeft;
}
MotorManagerAllMotors()
{
this->AddMotors(MotorManagerWD::instance()->GetMotors());
this->AddMotors(MotorManagerAux::instance()->GetMotors());
}
};
......@@ -4,9 +4,9 @@
namespace{
MotorPdoMapping::node_id_t NodeIdLeftMotor =1;
MotorPdoMapping::node_id_t NodeIdRightMotor =2;
MotorPdoMapping::node_id_t NodeIdAuxiliaryMotor =3;
Motor::node_id_t NodeIdLeftMotor =1;
Motor::node_id_t NodeIdRightMotor =2;
Motor::node_id_t NodeIdAuxiliaryMotorOne =3;
}
class MotorManagerWD final: public MotorManager,
......@@ -17,15 +17,23 @@ public:
}
private:
Motor mMotorRight;
Motor mMotorLeft;
Motor mMotorAuxiliary;
Motor mMotorRight;
protected:
MotorManagerWD() :
mMotorRight(MotorPdoMapping(NodeIdRightMotor, "MotorRight")),
mMotorLeft(MotorPdoMapping(NodeIdLeftMotor, "MotorLeft")),
mMotorAuxiliary(MotorPdoMapping(NodeIdAuxiliaryMotor, "MotorAuxiliary")){
mMotorLeft(NodeIdLeftMotor, "MotorLeft"),
mMotorRight(NodeIdRightMotor, "MotorRight")
{
mMotorLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000, 1);
mMotorLeft.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0, 1);
mMotorLeft.getPdoHandle()->position_actual_value.updateMapping(0xA1C0, 2);
mMotorLeft.getPdoHandle()->status_word.updateMapping(0xA100, 1);
mMotorLeft.getPdoHandle()->mode_of_operation.updateMapping(0xA480, 1);
mMotorLeft.getPdoHandle()->target_velocity.updateMapping(0xA640, 1);
mMotorLeft.getPdoHandle()->control_word.updateMapping(0xA580, 1);
mMotorRight.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000, 2);
mMotorRight.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0, 3);
......@@ -35,24 +43,54 @@ protected:
mMotorRight.getPdoHandle()->target_velocity.updateMapping(0xA640, 2);
mMotorRight.getPdoHandle()->control_word.updateMapping(0xA580, 2);
mMotorLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000, 1);
mMotorLeft.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0, 1);
mMotorLeft.getPdoHandle()->position_actual_value.updateMapping(0xA1C0, 2);
mMotorLeft.getPdoHandle()->status_word.updateMapping(0xA100, 1);
mMotorLeft.getPdoHandle()->mode_of_operation.updateMapping(0xA480, 1);
mMotorLeft.getPdoHandle()->target_velocity.updateMapping(0xA640, 1);
mMotorLeft.getPdoHandle()->control_word.updateMapping(0xA580, 1);
AddMotor(&mMotorLeft);
AddMotor(&mMotorRight);
mMotorAuxiliary.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000, 3);
mMotorAuxiliary.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0, 5);
mMotorAuxiliary.getPdoHandle()->position_actual_value.updateMapping(0xA1C0, 6);
mMotorAuxiliary.getPdoHandle()->status_word.updateMapping(0xA100, 3);
mMotorAuxiliary.getPdoHandle()->mode_of_operation.updateMapping(0xA480, 3);
mMotorAuxiliary.getPdoHandle()->target_velocity.updateMapping(0xA640, 3);
mMotorAuxiliary.getPdoHandle()->control_word.updateMapping(0xA580, 3);
this->mMotors["MotorRight"] = &mMotorRight;
this->mMotors["MotorLeft"] = &mMotorLeft;
this->mMotors["MotorAuxiliary"] = &mMotorAuxiliary;
}
};
class MotorManagerAux final: public MotorManager,
public Singleton<MotorManagerAux> {
friend class Singleton<MotorManagerAux> ;
public:
~MotorManagerAux() {
}
private:
Motor mMotorAuxiliaryOne;
protected:
MotorManagerAux() :
mMotorAuxiliaryOne(NodeIdAuxiliaryMotorOne, "MotorAuxiliaryOne"){
mMotorAuxiliaryOne.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000, 3);
mMotorAuxiliaryOne.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0, 5);
mMotorAuxiliaryOne.getPdoHandle()->position_actual_value.updateMapping(0xA1C0, 6);
mMotorAuxiliaryOne.getPdoHandle()->status_word.updateMapping(0xA100, 3);
mMotorAuxiliaryOne.getPdoHandle()->mode_of_operation.updateMapping(0xA480, 3);
mMotorAuxiliaryOne.getPdoHandle()->target_velocity.updateMapping(0xA640, 3);
mMotorAuxiliaryOne.getPdoHandle()->control_word.updateMapping(0xA580, 3);
AddMotor(&mMotorAuxiliaryOne);
}
};
class MotorManagerAllMotors final: public MotorManager,
public Singleton<MotorManagerAllMotors> {
friend class Singleton<MotorManagerAllMotors> ;
public:
~MotorManagerAllMotors() {
}
private:
protected:
MotorManagerAllMotors()
{
this->AddMotors(MotorManagerWD::instance()->GetMotors());
this->AddMotors(MotorManagerAux::instance()->GetMotors());
}
};
#pragma once
#include "pdo_mapping_motors.h"
#include "motor_manager.h"
class MotorFrontLeft : public MotorPdoMapping
{
public:
MotorFrontLeft():velocity_demand(0xA0C0,1)
{
this->mode_of_operation_display.updateMapping(0xA000,1);
this->velocity_actual_value.updateMapping(0xA1C0,1);
this->position_actual_value.updateMapping(0xA1C0,2);
this->status_word.updateMapping(0xA100,1);
this->mode_of_operation.updateMapping(0xA480,1);
this->target_velocity.updateMapping(0xA640,1);
this->control_word.updateMapping(0xA580,1);
}
typedef int16_t velocity_demand_t;
CanRxNetworkVariable <velocity_demand_t> velocity_demand;
};
class MotorRearRight : public MotorPdoMapping
{
public:
MotorRearRight():velocity(0xA540,3)
{
this->mode_of_operation_display.updateMapping(0xA000,4);
this->velocity_actual_value.updateMapping(0xA1C0,7);
this->position_actual_value.updateMapping(0xA1C0,8);
this->status_word.updateMapping(0xA100,4);
this->mode_of_operation.updateMapping(0xA480,4);
this->target_velocity.updateMapping(0xA640,4);
this->control_word.updateMapping(0xA580,4);
}
typedef int16_t velocity_t;
CanTxNetworkVariable <velocity_t> velocity;
};
class MotorFrontRight : public MotorPdoMapping
{
public:
MotorFrontRight():velocity(0xA540,1)
{
this->mode_of_operation_display.updateMapping(0xA000,2);
this->velocity_actual_value.updateMapping(0xA1C0,3);
this->position_actual_value.updateMapping(0xA1C0,4);
this->status_word.updateMapping(0xA100,2);
this->mode_of_operation.updateMapping(0xA480,2);
this->target_velocity.updateMapping(0xA640,2);
this->control_word.updateMapping(0xA580,2);
}
typedef int16_t velocity_t;
CanTxNetworkVariable <velocity_t> velocity;
};
class MotorRearLeft : public MotorPdoMapping
{
public:
MotorRearLeft():velocity(0xA540,2)
{
this->mode_of_operation_display.updateMapping(0xA000,3);
this->velocity_actual_value.updateMapping(0xA1C0,5);
this->position_actual_value.updateMapping(0xA1C0,6);
this->status_word.updateMapping(0xA100,3);
this->mode_of_operation.updateMapping(0xA480,3);
this->target_velocity.updateMapping(0xA640,3);
this->control_word.updateMapping(0xA580,3);
}
typedef int16_t velocity_t;
CanTxNetworkVariable <velocity_t> velocity;
};
class MotorManagerWD : public MotorManager, public Singleton<MotorManagerWD>
{
friend class Singleton<MotorManagerWD>;
namespace{
MotorPdoMapping::node_id_t NodeIdFrontLeftMotor =1;
MotorPdoMapping::node_id_t NodeIdFrontRightMotor =2;
MotorPdoMapping::node_id_t NodeIdRearLeftMotor =3;
MotorPdoMapping::node_id_t NodeIdRearRightMotor =4;
}
class MotorManagerWD final: public MotorManager,
public Singleton<MotorManagerWD> {
friend class Singleton<MotorManagerWD> ;
public:
~MotorManagerWD()
{
}
~MotorManagerWD() {
}
private:
MotorFrontLeft mMotorFrontLeft;
MotorRearRight mMotorRearRight;
MotorFrontRight mMotorFrontRight;
MotorRearLeft mMotorRearLeft;
Motor mMotorFrontLeft;
Motor mMotorFrontRight;
Motor mMotorRearLeft;
Motor mMotorRearRight;
protected:
MotorManagerWD()
{
this->mMotors["MotorFrontLeft"] = &mMotorFrontLeft;
this->mMotors["MotorRearRight"] = &mMotorRearRight;
this->mMotors["MotorFrontRight"] = &mMotorFrontRight;
this->mMotors["MotorRearLeft"] = &mMotorRearLeft;
}
MotorManagerWD() :
mMotorFrontLeft(MotorPdoMapping(NodeIdFrontLeftMotor, "MotorFrontLeft")),
mMotorFrontRight(MotorPdoMapping(NodeIdFrontRightMotor, "MotorFrontRight")),
mMotorRearLeft(MotorPdoMapping(NodeIdRearLeftMotor, "MotorRearLeft")),
mMotorRearRight(MotorPdoMapping(NodeIdRearRightMotor, "MotorRearRight"))
{
mMotorFrontLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,1);
mMotorFrontLeft.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0,1);
mMotorFrontLeft.getPdoHandle()->position_actual_value.updateMapping(0xA1C0,2);
mMotorFrontLeft.getPdoHandle()->status_word.updateMapping(0xA100,1);
mMotorFrontLeft.getPdoHandle()->mode_of_operation.updateMapping(0xA480,1);
mMotorFrontLeft.getPdoHandle()->target_velocity.updateMapping(0xA640,1);
mMotorFrontLeft.getPdoHandle()->control_word.updateMapping(0xA580,1);
mMotorFrontRight.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,2);
mMotorFrontRight.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0,3);
mMotorFrontRight.getPdoHandle()->position_actual_value.updateMapping(0xA1C0,4);
mMotorFrontRight.getPdoHandle()->status_word.updateMapping(0xA100,2);
mMotorFrontRight.getPdoHandle()->mode_of_operation.updateMapping(0xA480,2);
mMotorFrontRight.getPdoHandle()->target_velocity.updateMapping(0xA640,2);
mMotorFrontRight.getPdoHandle()->control_word.updateMapping(0xA580,2);
mMotorRearLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,3);
mMotorRearLeft.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0,5);
mMotorRearLeft.getPdoHandle()->position_actual_value.updateMapping(0xA1C0,6);
mMotorRearLeft.getPdoHandle()->status_word.updateMapping(0xA100,3);
mMotorRearLeft.getPdoHandle()->mode_of_operation.updateMapping(0xA480,3);
mMotorRearLeft.getPdoHandle()->target_velocity.updateMapping(0xA640,3);
mMotorRearLeft.getPdoHandle()->control_word.updateMapping(0xA580,3);
mMotorRearRight.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,4);
mMotorRearRight.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0,7);
mMotorRearRight.getPdoHandle()->position_actual_value.updateMapping(0xA1C0,8);
mMotorRearRight.getPdoHandle()->status_word.updateMapping(0xA100,4);
mMotorRearRight.getPdoHandle()->mode_of_operation.updateMapping(0xA480,4);
mMotorRearRight.getPdoHandle()->target_velocity.updateMapping(0xA640,4);
mMotorRearRight.getPdoHandle()->control_word.updateMapping(0xA580,4);
this->mMotorsMap["MotorFrontLeft"] = &mMotorFrontLeft;
this->mMotorsMap["MotorFrontRight"] = &mMotorFrontRight;
this->mMotorsMap["MotorRearLeft"] = &mMotorRearLeft;
this->mMotorsMap["MotorRearRight"] = &mMotorRearRight;
}
};
......@@ -18,7 +18,7 @@ public:
typedef controlword_specific::type control_word_t;
typedef modes_of_operation_specific::type mode_of_operation_t;
typedef modes_of_operation_specific::type mode_of_operation_display_t;
typedef uint8_t node_id_t;
virtual ~MotorPdoMapping() {
}
......@@ -63,8 +63,7 @@ public:
}
MotorPdoMapping(node_id_t _node_id, const char * _name) :
node_id(_node_id), name(_name) {
MotorPdoMapping() {
}
CanRxNetworkVariable<mode_of_operation_display_t> mode_of_operation_display;
......@@ -75,8 +74,7 @@ public:
CanTxNetworkVariable<target_velocity_t> target_velocity;
CanTxNetworkVariable<target_position_t> target_position;
CanTxNetworkVariable<control_word_t> control_word;
const node_id_t node_id;
const char * name;
private:
......
#include "../inc/can_helper.h"
namespace{
const int8_t Cyclic_Synchronous_Velocity_Mode = 9; //Operation Mode "CSV" of Object of 6060h and 6061h (see CIA DS402 Part 2)
}
// TODO XXXXXXXXXXXXX BEGIN OF DEPRACTED XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
Error CanHelper::AppEnableCsvMode(void)
{
uint16_t ctrlword = 6;
for (auto motor : *mMotors)
{
motor->setControlWord(ctrlword);
}
int8_t mode = 9; // CSV
for (auto motor : *mMotors)
{
motor->setModeOfOperation(mode);
}
mPowerEnabled = true;
return (Error::NO_ERROR);
}
Error CanHelper::AppDisableCsvMode(void)
{
uint16_t ctrlword = 6;
for (auto motor : *mMotors)
{
motor->setControlWord(ctrlword);
}
int8_t mode = 9; // CSV
for (auto motor : *mMotors)
{
motor->setModeOfOperation(mode);
}
mPowerEnabled = false;
return (Error::NO_ERROR);
}
Error CanHelper::AppProcessCsvMode(void)
{
if (mPowerEnabled)
{
size_t size = mMotors->size();
for (size_t i = 0; i < size; i++)
{
(mMotors->at(i))->getModeOfOperationDisplay(mMode_of_operation_display[i]);
}
for (size_t i = 0; i < size; i++)
{
if (mMode_of_operation_display[i] == Cyclic_Synchronous_Velocity_Mode)
{
uint16_t status_word = 0;
uint16_t next_control_word = 0;
(mMotors->at(i))->getStatusWord(status_word);
callculateNextControlWordToSwitchUpCia402Statemachine(status_word, next_control_word);
(mMotors->at(i))->setControlWord(next_control_word);
}
}
}
return (Error::NO_ERROR);
}
Error CanHelper::callculateNextControlWordToSwitchUpCia402Statemachine(uint16_t statusword, uint16_t &ctrlword)
{
if (((statusword & MASK_NOT_READY_AND_SWITCH_DISABLED) == NOT_READY_TO_SWITCHED_ON)
|| ((statusword & MASK_NOT_READY_AND_SWITCH_DISABLED) == SWITCH_ON_DISABLED))
{
ctrlword = 6;
return (Error::NO_ERROR);
}
else if ((statusword & MASK_GENERAL) == READY_TO_SWITCH_ON)
{
ctrlword = 7;
return (Error::NO_ERROR);
}
else if ((statusword & MASK_GENERAL) == SWITCHED_ON)
{
ctrlword = 15;
return (Error::NO_ERROR);
}
else if ((statusword & MASK_GENERAL) == OPERATION_ENABLED)
{
ctrlword = 31;
return (Error::NO_ERROR);
}
else if (((statusword & MASK_FAULT) == FAULT_REACTION_ACTIVE) || ((statusword & MASK_FAULT) == FAULT))
{
ctrlword = 134; // Reset error
return (Error::NO_ERROR);
}
return (Error::NO_ERROR);
}
// TODO XXXXXXXXXXXXX END OF DEPRACTED XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
......@@ -8,8 +8,6 @@
#include "../../system/inc/msg.h"
#include "../../system/inc/factories.h"
#include "../../utility_ofc/include/utility_ofc/singleton.h"
#include "robotstatehelper.h"
#include "../../motors/inc/power_states.h"
//using namespace EMROSCmdType::RobotStateController;
......@@ -46,14 +44,14 @@ private:
bool mMotors_power_enabled;
bool mHearbeat;
bool mHeartbeat;
bool mCritical_fault;
RobotStates mRobotState;
PowerState mMotorsTargetPowerstate;
//---------------------------------------------
std::vector<MotorPdoMapping*>* mMotors;
std::vector<Motor*>* mMotors;
EthHandler::ObserverEthHandler mObserver_eth;
MsgBoxHandler::ObserverMsgBox mObserver_msg_box;
......
......@@ -9,11 +9,11 @@ Robotstatecontroller::Robotstatecontroller():
mDevice_id(EMROSCmdType::RobotStateController::DEVICE_ID),
mRos_link_enabled(false),
mMotors_power_enabled(false),
mHearbeat(true),
mHeartbeat(true),
mCritical_fault(false),
mRobotState(RobotStates::Start),
mMotorsTargetPowerstate(PowerState::SwitchedOnDisabled),
mMotors(mInstanceFactory.GetMotors()),
mMotors(mInstanceFactory.GetAllMotors()),
mObserver_eth(mDevice_id,&Robotstatecontroller::EthMsgCallback,this),
mObserver_msg_box(mDevice_id,&Robotstatecontroller::MsgBoxCallback,this),
mObserver_msg_box_broadcast(EMROSCmdType::BROADCAST_ID,&Robotstatecontroller::MsgBoxCallbackBroadcast,this),
......@@ -33,14 +33,18 @@ Error Robotstatecontroller::CheckButton(os_msg_t& msg)
OFCLoggerRTClient::instance()->InsertDebug("Button Enable Motors");
if(mMotors_power_enabled == true)
{
//TODO insertMessageForBasecontroller_to_enableOperation;
//TODO insertMessageForAuxiliaryDrives_to_enableOperation;
for (auto motor : *mMotors)
{
motor->DisablePower();
}
mMotors_power_enabled = !mMotors_power_enabled;
}
else
{
//TODO insertMessageForBasecontroller_to_disableOperation;
//TODO insertMessageForAuxiliaryDrives_to_disableOperation;
for (auto motor : *mMotors)
{
motor->EnablePower();
}
mMotors_power_enabled = !mMotors_power_enabled;
}
}
......@@ -49,42 +53,65 @@ Error Robotstatecontroller::CheckButton(os_msg_t& msg)
}
Error Robotstatecontroller::MsgBoxCallbackBroadcast(
const MsgBoxHandler::Dataframe& dataframe) {
switch (dataframe.getData().cmd) {
case EMROSCmdType::Service::ENABLE_SYNC:
mRos_link_enabled = true;
break;
case EMROSCmdType::Service::DISABLE_SYNC:
mRos_link_enabled = false;
break;
case EMROSCmdType::Security::HEARTBEAT:
mHeartbeat = true;
break;
Error Robotstatecontroller::MsgBoxCallback(const MsgBoxHandler::Dataframe& dataframe)
{
//TODO
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__," function not yet implemented"); //TODO
default:
break;
}
return (Error::NO_ERROR);
}
Error Robotstatecontroller::MsgBoxCallbackBroadcast(const MsgBoxHandler::Dataframe& dataframe)
{
//TODO
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__," function not yet implemented"); //TODO
return (Error::NO_ERROR);
}
Error Robotstatecontroller::EthMsgCallback(const EthHandler::Dataframe& dataframe)
Error Robotstatecontroller::MsgBoxCallback(const MsgBoxHandler::Dataframe& dataframe)
{
//TODO
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__," function not yet implemented"); //TODO
switch (dataframe.getData().cmd)
{
//TODO
default:
;
break;
}
return (Error::NO_ERROR);
}
Error Robotstatecontroller::FilterAndSendActualVelocitiesCallback()
Error Robotstatecontroller::EthMsgCallback(const EthHandler::Dataframe& dataframe)
{
//TODO
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__," function not yet implemented"); //TODO
switch (dataframe.getData().cmd)
{
//TODO
default:
;
break;
}
return (Error::NO_ERROR);
}
Error Robotstatecontroller::CyclicCANSyncCallback()
{
//TODO
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__," function not yet implemented"); //TODO
return (Error::NO_ERROR);
}
Error Robotstatecontroller::CyclicTimerCallback()
{
//TODO
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__," function not yet implemented"); //TODO
for (auto motor : *mMotors)
{
motor->ProcessMotorStateMachine();
}
return (Error::NO_ERROR);
}
[Master]
file=4WD/dcfs/em5.dcf
numberOfSlaves=4
project_name=WD
slaves=MotorFrontLeft,MotorFrontRight,MotorRearLeft,MotorRearRight
[MotorFrontLeft]
file=4WD/dcfs/1_front_left_stepper.dcf
nodeID=0x01
numTxMap=5
numRxMap=3
[MotorFrontLeftMapRx0]
index=0x6040
subindex=0x00
size=16
signed=0
name=control_word
[MotorFrontLeftMapRx1]
index=0x6060
subindex=0x00
size=8
signed=1
name=mode_of_operation
[MotorFrontLeftMapRx2]
index=0x60FF
subindex=0x00
size=32
signed=1
name=target_velocity
[MotorFrontLeftMapTx0]
index=0x6041
subindex=0x00
size=16
signed=0
name=status_word
[MotorFrontLeftMapTx1]
index=0x6061
subindex=0
size=8
signed=1
name=mode_of_operation_display
[MotorFrontLeftMapTx2]
index=0x606C
subindex=0x00
size=32
signed=1
name=velocity_actual_value
[MotorFrontLeftMapTx3]
index=0x6064
subindex=0x00
size=32
signed=1
name=position_actual_value
[MotorFrontLeftMapTx4]
index=0x6043
subindex=0x00
size=16
signed=1
name=velocity_demand
[MotorFrontRight]
file=4WD/dcfs/2_front_right_stepper.dcf
nodeID=0x02
numTxMap=4
numRxMap=4
[MotorFrontRightMapRx0]
index=0x6040
subindex=0x00
size=16
signed=0
name=control_word
[MotorFrontRightMapRx1]
index=0x6060
subindex=0x00
size=8
signed=1
name=mode_of_operation
[MotorFrontRightMapRx2]
index=0x60FF
subindex=0x00
size=32
signed=1
name=target_velocity
[MotorFrontRightMapRx3]
index=0x6042
subindex=0x00
size=16
signed=1
name=velocity
[MotorFrontRightMapTx0]
index=0x6041
subindex=0x00
size=16
signed=0
name=status_word
[MotorFrontRightMapTx1]
index=0x6061
subindex=0
size=8
signed=1
name=mode_of_operation_display
[MotorFrontRightMapTx2]
index=0x606C
subindex=0x00
size=32
signed=1
name=velocity_actual_value
[MotorFrontRightMapTx3]
index=0x6064
subindex=0x00
size=32
signed=1
name=position_actual_value
[MotorRearLeft]
file=4WD/dcfs/3_rear_left_stepper.dcf
nodeID=0x03
numTxMap=4
numRxMap=4
[MotorRearLeftMapRx0]
index=0x6040
subindex=0x00
size=16
signed=0
name=control_word
[MotorRearLeftMapRx1]
index=0x6060
subindex=0x00
size=8
signed=1
name=mode_of_operation
[MotorRearLeftMapRx2]
index=0x60FF
subindex=0x00
size=32
signed=1
name=target_velocity
[MotorRearLeftMapRx3]
index=0x6042
subindex=0x00
size=16
signed=1
name=velocity
[MotorRearLeftMapTx0]
index=0x6041
subindex=0x00
size=16
signed=0
name=status_word
[MotorRearLeftMapTx1]
index=0x6061
subindex=0
size=8
signed=1
name=mode_of_operation_display
[MotorRearLeftMapTx2]
index=0x606C
subindex=0x00
size=32
signed=1
name=velocity_actual_value
[MotorRearLeftMapTx3]
index=0x6064
subindex=0x00
size=32
signed=1
name=position_actual_value
[MotorRearRight]
file=4WD/dcfs/4_rear_right_stepper.dcf
nodeID=0x04
numTxMap=4
numRxMap=4
[MotorRearRightMapRx0]
index=0x6040
subindex=0x00
size=16
signed=0
name=control_word
[MotorRearRightMapRx1]
index=0x6060
subindex=0x00
size=8
signed=1
name=mode_of_operation
[MotorRearRightMapRx2]
index=0x60FF
subindex=0x00
size=32
signed=1
name=target_velocity
[MotorRearRightMapRx3]
index=0x6042
subindex=0x00
size=16
signed=1
name=velocity
[MotorRearRightMapTx0]
index=0x6041
subindex=0x00
size=16
signed=0
name=status_word
[MotorRearRightMapTx1]
index=0x6061
subindex=0
size=8
signed=1
name=mode_of_operation_display