...
 
Commits (3)
......@@ -15,13 +15,6 @@
#include "../../system/inc/msg.h"
#include "ramp_generator.h"
#include "basecontroller_types.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;
......@@ -43,7 +36,7 @@ private:
Error FilterAndSendActualVelocitiesCallback();
Error CyclicCANSyncCallback();
Error CyclicTimerCallback();
void UpdateVelocities();
void UpdateMotorTargetVelocities();
......
......@@ -8,7 +8,7 @@
#ifndef EMROS_EMROS_INC_BASECONTROLLER_TYPES_H_
#define EMROS_EMROS_INC_BASECONTROLLER_TYPES_H_
#include "../../emros_common/include/emros_common/velocities.h"
#include "../../emros_common/include/emros_common/attributes.h"
#include "../../utility_ofc/include/utility_ofc/error.h"
#define MASK_NOT_READY_AND_SWITCH_DISABLED 0x004F
......@@ -44,7 +44,7 @@ protected:
};
template<typename VEL_T>
class ExtendedVelocities : virtual public Velocities<VEL_T>, public ExtendedFunctions
class ExtendedVelocities : virtual public MotorAttributeInterface<VEL_T>, public ExtendedFunctions
{
public:
virtual ~ExtendedVelocities()
......@@ -58,7 +58,7 @@ template<typename VEL_T>
};
template<typename VEL_T>
class WheelVelocities_2WD_ext : virtual public WheelVelocities<VEL_T, struct_2WD<VEL_T> >, public ExtendedVelocities<
class WheelVelocities_2WD_ext : virtual public CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >, public ExtendedVelocities<
VEL_T>
{
public:
......@@ -71,10 +71,10 @@ template<typename VEL_T>
Error addVelocities(int32_t* real_velocities, size_t size) override
{
if (size == WheelVelocities<VEL_T, struct_2WD<VEL_T> >::NUMBER_OF_MOTORS)
if (size == CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::NUMBER_OF_MOTORS)
{
WheelVelocities<VEL_T, struct_2WD<VEL_T> >::data().left += real_velocities[0];
WheelVelocities<VEL_T, struct_2WD<VEL_T> >::data().right += real_velocities[1];
CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::data().left += real_velocities[0];
CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::data().right += real_velocities[1];
}
else
......@@ -88,16 +88,16 @@ template<typename VEL_T>
}
Error calcAverage(uint8_t number_of_interations) override
{
WheelVelocities<VEL_T, struct_2WD<VEL_T> >::data().right /= number_of_interations;
WheelVelocities<VEL_T, struct_2WD<VEL_T> >::data().left /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::data().right /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::data().left /= number_of_interations;
return Error::NO_ERROR;
}
Error setVelocities(int32_t* velocities, size_t size) override
{
if (size == WheelVelocities<VEL_T, struct_2WD<VEL_T> >::NUMBER_OF_MOTORS)
if (size == CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::NUMBER_OF_MOTORS)
{
velocities[0] = WheelVelocities<VEL_T, struct_2WD<VEL_T> >::data().left;
velocities[1] = WheelVelocities<VEL_T, struct_2WD<VEL_T> >::data().right;
velocities[0] = CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::data().left;
velocities[1] = CummulatedAttributOfMotors<VEL_T, struct_2WD<VEL_T> >::data().right;
}
else
......@@ -113,7 +113,7 @@ template<typename VEL_T>
template<typename VEL_T>
class WheelVelocities_2WD_Auxiliary_ext : virtual public WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >, public ExtendedVelocities<
class WheelVelocities_2WD_Auxiliary_ext : virtual public CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >, public ExtendedVelocities<
VEL_T> //TODO: Refactor code style (whole file)
{
public:
......@@ -126,11 +126,11 @@ template<typename VEL_T>
Error addVelocities(int32_t* real_velocities, size_t size) override
{
if (size == WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::NUMBER_OF_MOTORS)
if (size == CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::NUMBER_OF_MOTORS)
{
WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().left += real_velocities[0];
WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().right += real_velocities[1];
WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().auxiliary += real_velocities[2];
CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().left += real_velocities[0];
CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().right += real_velocities[1];
CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().auxiliary += real_velocities[2];
}
else
......@@ -144,18 +144,18 @@ template<typename VEL_T>
}
Error calcAverage(uint8_t number_of_interations) override
{
WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().right /= number_of_interations;
WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().left /= number_of_interations;
WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().auxiliary /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().right /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().left /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().auxiliary /= number_of_interations;
return Error::NO_ERROR;
}
Error setVelocities(int32_t* velocities, size_t size) override
{
if (size == WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::NUMBER_OF_MOTORS)
if (size == CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::NUMBER_OF_MOTORS)
{
velocities[0] = WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().left;
velocities[1] = WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().right;
velocities[2] = WheelVelocities<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().auxiliary;
velocities[0] = CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().left;
velocities[1] = CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().right;
velocities[2] = CummulatedAttributOfMotors<VEL_T, struct_2WD_plus_auxiliary<VEL_T> >::data().auxiliary;
}
else
......@@ -171,7 +171,7 @@ template<typename VEL_T>
template<typename VEL_T>
class WheelVelocities_4WD_ext : virtual public WheelVelocities<VEL_T, struct_4WD<VEL_T> >, public ExtendedVelocities<
class WheelVelocities_4WD_ext : virtual public CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >, public ExtendedVelocities<
VEL_T>
{
public:
......@@ -184,12 +184,12 @@ template<typename VEL_T>
Error addVelocities(int32_t* real_velocities, size_t size) override
{
if (size == WheelVelocities<VEL_T, struct_4WD<VEL_T> >::NUMBER_OF_MOTORS)
if (size == CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::NUMBER_OF_MOTORS)
{
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().front_left_wheel += real_velocities[0];
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().front_right_wheel += real_velocities[1];
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().rear_left_wheel += real_velocities[2];
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().rear_right_wheel += real_velocities[3];
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().front_left_wheel += real_velocities[0];
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().front_right_wheel += real_velocities[1];
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().rear_left_wheel += real_velocities[2];
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().rear_right_wheel += real_velocities[3];
}
else
......@@ -203,20 +203,20 @@ template<typename VEL_T>
}
Error calcAverage(uint8_t number_of_interations) override
{
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().front_left_wheel /= number_of_interations;
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().front_right_wheel /= number_of_interations;
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().rear_left_wheel /= number_of_interations;
WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().rear_right_wheel /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().front_left_wheel /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().front_right_wheel /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().rear_left_wheel /= number_of_interations;
CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().rear_right_wheel /= number_of_interations;
return Error::NO_ERROR;
}
Error setVelocities(int32_t* velocities, size_t size) override
{
if (size == WheelVelocities<VEL_T, struct_4WD<VEL_T> >::NUMBER_OF_MOTORS)
if (size == CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::NUMBER_OF_MOTORS)
{
velocities[0] = WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().front_left_wheel;
velocities[1] = WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().front_right_wheel;
velocities[2] = WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().rear_left_wheel;
velocities[3] = WheelVelocities<VEL_T, struct_4WD<VEL_T> >::data().rear_right_wheel;
velocities[0] = CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().front_left_wheel;
velocities[1] = CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().front_right_wheel;
velocities[2] = CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().rear_left_wheel;
velocities[3] = CummulatedAttributOfMotors<VEL_T, struct_4WD<VEL_T> >::data().rear_right_wheel;
}
else
......
......@@ -9,7 +9,7 @@
#define EMROS_EMROS_INC_RAMP_GENERATOR_H_
#include "../../utility_ofc/include/utility_ofc/error.h"
#include "../../emros_common/include/emros_common/velocities.h"
#include "../../emros_common/include/emros_common/attributes.h"
#include <cstring>
#include <cmath>
#include "../../system/inc/factories.h"
......@@ -39,11 +39,11 @@ template<typename VEL_T>
Error Init(RampConfig);
Error GenerateRamp(Velocities<VEL_T>& new_velocities, Velocities<VEL_T>& target_velocities);
Error GenerateRamp(MotorAttributeInterface<VEL_T>& new_velocities, MotorAttributeInterface<VEL_T>& target_velocities);
private:
Error CalcRamp(Velocities<VEL_T>& new_velocities, Velocities<VEL_T>& target_velocities, uint8_t index);
Error CalcRamp(MotorAttributeInterface<VEL_T>& new_velocities, MotorAttributeInterface<VEL_T>& target_velocities, uint8_t index);
RampConfig mConfig;
EMROSCmdType::Basecontroller::TargetVelocitiesInterface* mLast_velocities;
......@@ -74,8 +74,8 @@ template<typename VEL_T>
}
template<typename VEL_T>
Error RampGenerator<VEL_T>::GenerateRamp(Velocities<VEL_T>& new_velocities,
Velocities<VEL_T>& target_velocities)
Error RampGenerator<VEL_T>::GenerateRamp(MotorAttributeInterface<VEL_T>& new_velocities,
MotorAttributeInterface<VEL_T>& target_velocities)
{
if (mInitialized)
{
......@@ -93,7 +93,7 @@ template<typename VEL_T>
}
template<typename VEL_T>
Error RampGenerator<VEL_T>::CalcRamp(Velocities<VEL_T>& new_velocities, Velocities<VEL_T>& target_velocities,
Error RampGenerator<VEL_T>::CalcRamp(MotorAttributeInterface<VEL_T>& new_velocities, MotorAttributeInterface<VEL_T>& target_velocities,
uint8_t index)
{
......
......@@ -63,15 +63,13 @@ Basecontroller::~Basecontroller() {
}
void Basecontroller::UpdateVelocities() {
void Basecontroller::UpdateMotorTargetVelocities() {
mRampedVelocities->clearData();
mRamp.GenerateRamp(*mRampedVelocities, *mTargetVelocities);
uint8_t i { 0 };
for (auto motor : *mMotors) {
motor->getPdoHandle()->setTargetVelocity(
mRampedVelocities->dataByType()[i]);
i++;
mRampedVelocities->dataByType()[motor->ordinal_in_datagram]);
}
}
......@@ -209,7 +207,7 @@ Error Basecontroller::EthMsgCallback(const EthHandler::Dataframe& dataframe) {
//set target velocities to zero
mTargetVelocities->clearData();
OFCLoggerRTClient::instance()->InsertCriticalError(__PRETTY_FUNCTION__,
" security hearbeat failed");
" security heartbeat failed");
return (Error::ERROR);
}
return (status);
......@@ -225,19 +223,17 @@ Error Basecontroller::CyclicCANSyncCallback() {
mCritical_fault = true;
}
UpdateVelocities();
UpdateMotorTargetVelocities();
uint8_t i { 0 };
for (auto motor : *mMotors) {
motor->getPdoHandle()->getStatusWord((*mStatus_word)[i]);
motor->getPdoHandle()->getStatusWord((*mStatus_word)[motor->ordinal_in_datagram]);
motor->getPdoHandle()->getVelocityActualValue(
mActualVelocities->dataByType()[i]);
i++;
mActualVelocities->dataByType()[motor->ordinal_in_datagram]);
}
if (mCritical_fault) {
OFCLoggerRTClient::instance()->InsertCriticalError(__PRETTY_FUNCTION__,
"security hearbeat failed");
"security heartbeat failed");
return (Error::ERROR);
}
......
......@@ -5,65 +5,37 @@
#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;
using namespace EMROSCmdType::AuxiliaryController;
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 SendActualPositionsCallback();
Error CyclicCANSyncCallback();
Error CyclicTimerCallback();
void UpdateVelocities();
void UpdateTargetPosition();
InstanceFactory& mInstanceFactory;
EthHandler::DeviceId mDevice_id;
bool mRos_link_enabled;
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;
TargetPositionsInterface* mTargetPositions;
ActualPositionsInterface* mActualPositions;
EthHandler::ObserverEthHandler mObserver_eth;
MsgBoxHandler::ObserverMsgBox mObserver_msg_box;
......@@ -71,5 +43,7 @@ private:
CANSyncHandler::ObserverSyncHandler mObserver_delayed_can_sync;
CANSyncHandler::ObserverSyncHandler mObserver_cyclic_can_sync;
};
#include "../inc/auxiliary_controller.h"
namespace {
const int every_cansync_event = 1;
const TimerHandler::timer_event_t every_timer_event = 1;
const uint32_t filter_period_in_cansyncs = 8;
}
using namespace EMROSCmdType::AuxiliaryController;
AuxiliaryController::AuxiliaryController() :
mInstanceFactory(*InstanceFactory::instance()),
mDevice_id(DEVICE_ID),
mRos_link_enabled(false),
mCSV_mode_enabled(false),
mHeartbeat(true),
mCritical_fault(false),
mMotors(mInstanceFactory.GetAuxiliaryMotors()),
mTargetPositions(mInstanceFactory.GetTargetPositionsInstance()),
mActualPositions(mInstanceFactory.GetActualPositionsInstance()),
mObserver_eth(mDevice_id,&AuxiliaryController::EthMsgCallback,this),
mObserver_msg_box(mDevice_id,&AuxiliaryController::MsgBoxCallback,this),
mObserver_msg_box_broadcast(EMROSCmdType::BROADCAST_ID,&AuxiliaryController::MsgBoxCallbackBroadcast,this),
mObserver_cyclic_can_sync(every_cansync_event,&AuxiliaryController::CyclicCANSyncCallback,this),
mObserver_delayed_can_sync(filter_period_in_cansyncs,&AuxiliaryController::SendActualPositionsCallback,this) {
for (auto motor : *mMotors) {
motor->SetTargetModeOfOperation(
modes_of_operation_specific::code::ProfilePosition);
motor->SetApplicationConfigFinished();
}
}
AuxiliaryController::~AuxiliaryController() {
delete &mMotors;
}
void AuxiliaryController::UpdateTargetPosition() {
for (auto motor : *mMotors) {
motor->getPdoHandle()->setTargetPosition(
mTargetPositions->dataByType()[motor->ordinal_in_datagram]);
}
}
Error AuxiliaryController::MsgBoxCallback(
const MsgBoxHandler::Dataframe& dataframe) {
Error status { Error::NO_ERROR };
MsgBoxHandler::Dataframe tx_msg_box;
switch (dataframe.getData().cmd) {
case EMROSCmdType::Security::CMD_VEL_DOWNLINK_TIMEOUT:
// TODO Error handling -> stop motors
break;
default:
break;
}
return (status);
}
Error AuxiliaryController::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;
// TODO stop motors
break;
case EMROSCmdType::Security::HEARTBEAT:
mHeartbeat = true;
break;
default:
break;
}
return (Error::NO_ERROR);
}
Error AuxiliaryController::EthMsgCallback(const EthHandler::Dataframe& dataframe) {
Error status { Error::NO_ERROR };
if ((mCritical_fault == false) && (mRos_link_enabled == true)) {
EthHandler::Dataframe tx_eth_msg { };
switch (dataframe.getData().cmd) {
case TARGET_POSITION:
status = mTargetPositions->set(dataframe.getData().payload.begin(),
dataframe.getData().payload_length);
if (status != Error::NO_ERROR) {
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"wrong data size for target positions)");
return (status);
}
break;
default:
break;
}
} else if (mCritical_fault) {
// this should never happen !
// TODO Error handling -> stop motors
OFCLoggerRTClient::instance()->InsertCriticalError(__PRETTY_FUNCTION__,
" security heartbeat failed");
return (Error::ERROR);
}
return (status);
}
Error AuxiliaryController::CyclicCANSyncCallback() {
if (mHeartbeat == true) {
mHeartbeat = false;
} else {
// TODO Error handling -> stop motors
mCritical_fault = true;
}
UpdateTargetPosition();
if (mCritical_fault) {
OFCLoggerRTClient::instance()->InsertCriticalError(__PRETTY_FUNCTION__,
"security heartbeat failed");
return (Error::ERROR);
}else{
return (Error::NO_ERROR);
}
}
Error AuxiliaryController::SendActualPositionsCallback() {
if (mRos_link_enabled == true) {
EthHandler::Dataframe tx_eth_msg;
tx_eth_msg.setData().id = mDevice_id;
tx_eth_msg.setData().cmd = ACTUAL_POSITION;
tx_eth_msg.setData().time =
CANSyncHandler::instance()->GetTimeOfLastSyncInUs();
for (auto motor : *mMotors) {
motor->getPdoHandle()->getPositionActualValue(
mActualPositions->dataByType()[motor->ordinal_in_datagram]);
}
tx_eth_msg.setData().payload_length = Ethprotocol::PayLen(
mActualPositions->getRawSize());
EthHandler::instance()->SendEthMsg(tx_eth_msg);
}
return (Error::NO_ERROR);
}
Subproject commit fa6fd21ca38fa63730aeb2010c4386015f5a827b
Subproject commit 4c4515461ed6da5002005df587e1d0ec6b3a3571
......@@ -16,6 +16,7 @@
#include "NanoOFC/logger/inc/ofc_client_RT.h"
#include "system/inc/msg.h"
#include "basecontroller/inc/basecontroller.h"
#include "devices/inc/auxiliary_controller.h"
#include "robotstatecontroller/inc/robotstatecontroller.h"
#include "devices/inc/service.h"
#include "devices/inc/security.h"
......@@ -89,6 +90,7 @@ void RealtimeTask(void) {
Security security { };
Basecontroller basecontroller { };
AuxiliaryController auxiliarycontroller { };
Robotstatecontroller robotstatecontroller { };
Service service { };
myLogger.InsertInfo("Bootup finished!");
......
......@@ -14,9 +14,10 @@ public:
typedef uint8_t node_id_t;
const node_id_t node_id;
const char * name;
const uint8_t ordinal_in_datagram;
Motor(node_id_t _node_id, const char * _name) :
node_id(_node_id), name(_name) {
Motor(node_id_t _node_id, const char * _name, uint8_t _position_in_datagram) :
node_id(_node_id), name(_name), ordinal_in_datagram(_position_in_datagram){
}
MotorPdoMapping* getPdoHandle (void){
......
#pragma once
#ifdef DEF_2WD
#if !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_LEFT)
#elif !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_RIGHT)
#error "ORDINAL_IN_DATAGRAM of motors need to be defined"
#endif
#include "pdo_mapping_motors.h"
#include "motor_manager.h"
......@@ -21,8 +29,8 @@ private:
protected:
MotorManagerWD() :
mMotorLeft(NodeIdLeftMotor, "MotorLeft"),
mMotorRight(NodeIdRightMotor, "MotorRight")
mMotorLeft(NodeIdLeftMotor, "MotorLeft", ORDINAL_IN_DATAGRAM_WD_MOTOR_LEFT),
mMotorRight(NodeIdRightMotor, "MotorRight", ORDINAL_IN_DATAGRAM_WD_MOTOR_RIGHT)
{
mMotorLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,1);
......@@ -61,19 +69,4 @@ protected:
}
};
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());
}
};
#endif /* DEF_2WD */
#pragma once
#ifdef DEF_2WD_PLUS_AUX
#if !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_LEFT)
#elif !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_RIGHT)
#elif !defined (ORDINAL_IN_DATAGRAM_AUX_MOTOR)
#error "ORDINAL_IN_DATAGRAM of motors need to be defined"
#endif
#include "pdo_mapping_motors.h"
#include "motor_manager.h"
......@@ -23,8 +32,8 @@ private:
protected:
MotorManagerWD() :
mMotorLeft(NodeIdLeftMotor, "MotorLeft"),
mMotorRight(NodeIdRightMotor, "MotorRight")
mMotorLeft(NodeIdLeftMotor, "MotorLeft", ORDINAL_IN_DATAGRAM_WD_MOTOR_LEFT),
mMotorRight(NodeIdRightMotor, "MotorRight", ORDINAL_IN_DATAGRAM_WD_MOTOR_RIGHT)
{
mMotorLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000, 1);
......@@ -61,7 +70,7 @@ private:
protected:
MotorManagerAux() :
mMotorAuxiliaryOne(NodeIdAuxiliaryMotorOne, "MotorAuxiliaryOne"){
mMotorAuxiliaryOne(NodeIdAuxiliaryMotorOne, "MotorAuxiliaryOne", ORDINAL_IN_DATAGRAM_AUX_MOTOR){
mMotorAuxiliaryOne.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000, 3);
mMotorAuxiliaryOne.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0, 5);
......@@ -75,22 +84,4 @@ protected:
}
};
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());
}
};
#endif /* DEF_2WD_PLUS_AUX */
#pragma once
#ifdef DEF_4WD
#if !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_FRONT_LEFT)
#elif !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_FRONT_RIGHT)
#elif !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_REAR_LEFT)
#elif !defined (ORDINAL_IN_DATAGRAM_WD_MOTOR_REAR_RIGHT)
#error "ORDINAL_IN_DATAGRAM of motors need to be defined"
#endif
#include "pdo_mapping_motors.h"
#include "motor_manager.h"
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;
Motor::node_id_t NodeIdFrontLeftMotor =1;
Motor::node_id_t NodeIdFrontRightMotor =2;
Motor::node_id_t NodeIdRearLeftMotor =3;
Motor::node_id_t NodeIdRearRightMotor =4;
}
class MotorManagerWD final: public MotorManager,
......@@ -25,10 +35,10 @@ private:
protected:
MotorManagerWD() :
mMotorFrontLeft(MotorPdoMapping(NodeIdFrontLeftMotor, "MotorFrontLeft")),
mMotorFrontRight(MotorPdoMapping(NodeIdFrontRightMotor, "MotorFrontRight")),
mMotorRearLeft(MotorPdoMapping(NodeIdRearLeftMotor, "MotorRearLeft")),
mMotorRearRight(MotorPdoMapping(NodeIdRearRightMotor, "MotorRearRight"))
mMotorFrontLeft(NodeIdFrontLeftMotor, "MotorFrontLeft", ORDINAL_IN_DATAGRAM_WD_MOTOR_FRONT_LEFT),
mMotorFrontRight(NodeIdFrontRightMotor, "MotorFrontRight", ORDINAL_IN_DATAGRAM_WD_MOTOR_FRONT_RIGHT),
mMotorRearLeft(NodeIdRearLeftMotor, "MotorRearLeft", ORDINAL_IN_DATAGRAM_WD_MOTOR_REAR_LEFT),
mMotorRearRight(NodeIdRearRightMotor, "MotorRearRight", ORDINAL_IN_DATAGRAM_WD_MOTOR_REAR_RIGHT)
{
mMotorFrontLeft.getPdoHandle()->mode_of_operation_display.updateMapping(0xA000,1);
mMotorFrontLeft.getPdoHandle()->velocity_actual_value.updateMapping(0xA1C0,1);
......@@ -62,9 +72,27 @@ protected:
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;
AddMotor(&mMotorFrontLeft);
AddMotor(&mMotorFrontRight);
AddMotor(&mMotorRearLeft);
AddMotor(&mMotorRearRight);
}
};
class MotorManagerAux final: public MotorManager,
public Singleton<MotorManagerAux> {
friend class Singleton<MotorManagerAux> ;
public:
~MotorManagerAux() {
}
private:
protected:
MotorManagerAux(){
//Placeholder - No auxiliary motors used
}
};
#endif /* DEF_4WD */
......@@ -32,6 +32,8 @@ public:
void Configure(Robot::KindOfKinematics config);
EMROSCmdType::Basecontroller::TargetVelocitiesInterface* GetTargetVelocitiesInstance();
EMROSCmdType::Basecontroller::ActualVelocitiesInterface* GetActualVelocitiesInstance();
EMROSCmdType::AuxiliaryController::TargetPositionsInterface* GetTargetPositionsInstance();
EMROSCmdType::AuxiliaryController::ActualPositionsInterface* GetActualPositionsInstance();
MotorStatus* GetMotorStatusInstance();
std::vector<Motor*>* GetAllMotors();
std::vector<Motor*>* GetWheelDriveMotors();
......
......@@ -7,7 +7,25 @@
#include "../inc/factories.h"
using namespace EMROSCmdType::Basecontroller;
using namespace EMROSCmdType;
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());
}
};
MotorStatus* InstanceFactory::GetMotorStatusInstance()
{
......@@ -37,18 +55,63 @@ void InstanceFactory::Configure(Robot::KindOfKinematics config)
mDriveConfiguration = config;
}
TargetVelocitiesInterface* InstanceFactory::GetTargetVelocitiesInstance()
Basecontroller::TargetVelocitiesInterface* InstanceFactory::GetTargetVelocitiesInstance()
{
switch (mDriveConfiguration)
{
case Robot::KindOfKinematics::TWO_WD:
return (new Basecontroller::TargetWheelVelocities2WD);
break;
case Robot::KindOfKinematics::TWO_WD_PLUS_AUX:
return (new Basecontroller::TargetWheelVelocities2WDplusAuxiliary);
break;
case Robot::KindOfKinematics::FOUR_WD:
return (new Basecontroller::TargetWheelVelocities4WD);
break;
case Robot::KindOfKinematics::UNDEFINED:
return (nullptr);
break;
default:
return (nullptr);
break;
}
}
Basecontroller::ActualVelocitiesInterface* InstanceFactory::GetActualVelocitiesInstance()
{
switch (mDriveConfiguration)
{
case Robot::KindOfKinematics::TWO_WD:
return (new Basecontroller::ActualWheelVelocities2WD);
break;
case Robot::KindOfKinematics::TWO_WD_PLUS_AUX:
return (new Basecontroller::ActualWheelVelocities2WDplusAuxiliary);
break;
case Robot::KindOfKinematics::FOUR_WD:
return (new Basecontroller::ActualWheelVelocities4WD);
break;
case Robot::KindOfKinematics::UNDEFINED:
return (nullptr);
break;
default:
return (nullptr);
break;
}
}
AuxiliaryController::TargetPositionsInterface* InstanceFactory::GetTargetPositionsInstance()
{
switch (mDriveConfiguration)
{
case Robot::KindOfKinematics::TWO_WD:
return (new TargetWheelVelocities2WD);
return (new AuxiliaryController::TargetPositions2WD);
break;
case Robot::KindOfKinematics::TWO_WD_PLUS_AUX:
return (new TargetWheelVelocities2WDplusAuxiliary);
return (new AuxiliaryController::TargetPositions2WDplusAuxiliary);
break;
case Robot::KindOfKinematics::FOUR_WD:
return (new TargetWheelVelocities4WD);
return (new AuxiliaryController::TargetPositions4WD);
break;
case Robot::KindOfKinematics::UNDEFINED:
return (nullptr);
......@@ -60,18 +123,18 @@ TargetVelocitiesInterface* InstanceFactory::GetTargetVelocitiesInstance()
}
}
ActualVelocitiesInterface* InstanceFactory::GetActualVelocitiesInstance()
AuxiliaryController::ActualPositionsInterface* InstanceFactory::GetActualPositionsInstance()
{
switch (mDriveConfiguration)
{
case Robot::KindOfKinematics::TWO_WD:
return (new ActualWheelVelocities2WD);
return (new AuxiliaryController::ActualPositions2WD);
break;
case Robot::KindOfKinematics::TWO_WD_PLUS_AUX:
return (new ActualWheelVelocities2WDplusAuxiliary);
return (new AuxiliaryController::ActualPositions2WDplusAuxiliary);
break;
case Robot::KindOfKinematics::FOUR_WD:
return (new ActualWheelVelocities4WD);
return (new AuxiliaryController::ActualPositions4WD);
break;
case Robot::KindOfKinematics::UNDEFINED:
return (nullptr);
......