...
 
Commits (2)
......@@ -5,6 +5,7 @@
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include "../../system/inc/msg.h"
#include "../../system/inc/factories.h"
#include "../../motors/inc/profile_position_mode.h"
using namespace EMROSCmdType::AuxiliaryController;
......@@ -22,7 +23,12 @@ private:
Error SendActualPositionsCallback();
Error CyclicCANSyncCallback();
Error CyclicTimerCallback();
void UpdateTargetPosition();
void UpdateMotorTargetPosition();
Error CopyTargetPositionFromDataFrame(const EthHandler::Dataframe& dataframe);
void SetStartTravelCommand();
void ResetStartTravelCommand();
void SetControlwordBit(uint8_t bit_position);
void ResetControlwordBit(uint8_t bit_position);
InstanceFactory& mInstanceFactory;
......
......@@ -37,12 +37,6 @@ 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) {
......@@ -89,13 +83,9 @@ Error AuxiliaryController::EthMsgCallback(const EthHandler::Dataframe& dataframe
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);
}
CopyTargetPositionFromDataFrame(dataframe);
UpdateMotorTargetPosition();
SetStartTravelCommand();
break;
default:
break;
......@@ -120,7 +110,7 @@ Error AuxiliaryController::CyclicCANSyncCallback() {
mCritical_fault = true;
}
UpdateTargetPosition();
ResetStartTravelCommand();
if (mCritical_fault) {
OFCLoggerRTClient::instance()->InsertCriticalError(__PRETTY_FUNCTION__,
......@@ -152,3 +142,56 @@ Error AuxiliaryController::SendActualPositionsCallback() {
}
Error AuxiliaryController::CopyTargetPositionFromDataFrame(const EthHandler::Dataframe& dataframe)
{
Error status { Error::NO_ERROR };
status = mTargetPositions->set(dataframe.getData().payload.begin(),
dataframe.getData().payload_length);
if (status == Error::ERROR) {
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"wrong data size for target positions)");
}
return (status);
}
void AuxiliaryController::UpdateMotorTargetPosition() {
for (auto motor : *mMotors) {
motor->getPdoHandle()->setTargetPosition(
mTargetPositions->dataByType()[motor->ordinal_in_datagram]);
}
}
void AuxiliaryController::SetStartTravelCommand()
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::start_travel));
}
void AuxiliaryController::ResetStartTravelCommand()
{
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::start_travel));
}
void AuxiliaryController::SetControlwordBit(uint8_t bit_position)
{
using namespace profile_position_mode::controlword_specific;
using control_word_t = type;
control_word_t controlword;
for (auto motor : *mMotors) {
motor->getPdoHandle()->getControlWord(controlword);
controlword |= control_word_t(true<<bit_position);
motor->getPdoHandle()->setControlWord(controlword);
}
}
void AuxiliaryController::ResetControlwordBit(uint8_t bit_position)
{
using namespace profile_position_mode::controlword_specific;
using control_word_t = type;
control_word_t controlword;
for (auto motor : *mMotors) {
motor->getPdoHandle()->getControlWord(controlword);
controlword &= control_word_t(~(true<<bit_position));
motor->getPdoHandle()->setControlWord(controlword);
}
}
Subproject commit 4c4515461ed6da5002005df587e1d0ec6b3a3571
Subproject commit d8b325c263e750d710fb35fde959c91ce83da895
......@@ -36,18 +36,6 @@ const uint32_t timeout_rt_task_mainloop_ms = 10;
const uint32_t timeout_user_task_mainloop_ms = 50;
}
//-----------------------------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
#if !(defined(DEF_4WD) != defined(DEF_2WD) != defined(DEF_2WD_PLUS_AUX))
#error "More then one of DEF_2WD, DEF_2WD_PLUS_AUX or DEF_4WD are defined. Choose one only!!!!"
#endif
#if defined (DEF_4WD) && defined (DEF_2WD) && defined (DEF_2WD_PLUS_AUX)
#error "All of DEF_2WD, DEF_2WD_PLUS_AUX and DEF_4WD are defined. Choose one only!!!!"
#endif
///////////////////////////////////////////////////////////////////////
/// \brief Task used for all time critical duties.
/// \details The RealtimeTask gets a guaranteed time of 250 Microseconds every Millisecond.
......@@ -56,11 +44,10 @@ const uint32_t timeout_user_task_mainloop_ms = 50;
/// Hence the RealtimeTask can be considered as the heart of OFC.
///////////////////////////////////////////////////////////////////////
void RealtimeTask(void) {
//DBGBRK(true); //Software breakpoint
//-----------------------------GENERAL SETUP------------------------------//
//DBGBRK(true); //Software breakpoint
os_msg_t msg;
System::SetTimeZone(TIMEZONE_UTC_PLUS1, true);
OFCLoggerRTClient& myLogger { *OFCLoggerRTClient::instance() };
EthHandler::instance()->Init(ip_ros_pc, port_ros_pc);// Configure communication with ROS-PC
......@@ -74,17 +61,7 @@ void RealtimeTask(void) {
bringupHelper.BootMotors();
//-----------------------------EMROS SETUP--------------------------------//
#ifdef DEF_4WD
InstanceFactory::instance()->Configure(Robot::KindOfKinematics::FOUR_WD);
#endif
#ifdef DEF_2WD
InstanceFactory::instance()->Configure(Robot::KindOfKinematics::TWO_WD);
#endif
#ifdef DEF_2WD_PLUS_AUX
InstanceFactory::instance()->Configure(Robot::KindOfKinematics::TWO_WD_PLUS_AUX);
#endif
InstanceFactory::instance()->Init();
SdoHandler::instance()->Init();
CANSyncHandler::instance()->Init(can_sync_interval_us);
......
......@@ -151,26 +151,28 @@ public:
status_word_t readout;
mPdoHanlde.getStatusWord(readout);
if ((readout & statusword_specific::mask::fault)
== statusword_specific::code::fault) {
using namespace power_state::statusword_specific;
if ((readout & mask::fault)
== code::fault) {
return PowerState::Fault;
} else if ((readout & statusword_specific::mask::fault_reaction_active)
== statusword_specific::code::fault_reaction_active) {
} else if ((readout & mask::fault_reaction_active)
== code::fault_reaction_active) {
return PowerState::FaultReactionActive;
} else if ((readout & statusword_specific::mask::not_ready_to_switch_on)
== statusword_specific::code::not_ready_to_switch_on) {
} else if ((readout & mask::not_ready_to_switch_on)
== code::not_ready_to_switch_on) {
return PowerState::NotReadyToSwitchOn;
} else if ((readout & statusword_specific::mask::switch_on_disabled)
== statusword_specific::code::switch_on_disabled) {
} else if ((readout & mask::switch_on_disabled)
== code::switch_on_disabled) {
return PowerState::SwitchedOnDisabled;
} else if ((readout & statusword_specific::mask::ready_to_switch_on)
== statusword_specific::code::ready_to_switch_on) {
} else if ((readout & mask::ready_to_switch_on)
== code::ready_to_switch_on) {
return PowerState::ReadyToSwitchOn;
} else if ((readout & statusword_specific::mask::switched_on)
== statusword_specific::code::switched_on) {
} else if ((readout & mask::switched_on)
== code::switched_on) {
return PowerState::SwitchedOn;
} else if ((readout & statusword_specific::mask::operation_enabled)
== statusword_specific::code::operation_enabled) {
} else if ((readout & mask::operation_enabled)
== code::operation_enabled) {
return PowerState::OperationEnabled;
} else {
return PowerState::NotSpecified;
......@@ -180,8 +182,8 @@ public:
void ResetFault(){mPowerStateMachineLockedDueToFault = false;}
private:
typedef statusword_specific::type status_word_t;
typedef controlword_specific::type control_word_t;
typedef power_state::statusword_specific::type status_word_t;
typedef power_state::controlword_specific::type control_word_t;
MotorPdoMapping mPdoHanlde;
PowerState mTargetPowerState = PowerState::NotSpecified;
......@@ -226,38 +228,40 @@ private:
control_word_t tmp_controlword;
Error status = Error::NO_ERROR;
using namespace power_state::controlword_specific;
switch (transition) {
case (PowerStateTransition::DisableVoltage):
mask_to_apply = controlword_specific::mask::disable_voltage;
code_to_write = controlword_specific::code::disable_voltage;
mask_to_apply = mask::disable_voltage;
code_to_write = code::disable_voltage;
break;
case (PowerStateTransition::Shutdown):
mask_to_apply = controlword_specific::mask::shutdown;
code_to_write = controlword_specific::code::shutdown;
mask_to_apply = mask::shutdown;
code_to_write = code::shutdown;
break;
case (PowerStateTransition::SwitchOn):
mask_to_apply = controlword_specific::mask::switch_on;
code_to_write = controlword_specific::code::switch_on;
mask_to_apply = mask::switch_on;
code_to_write = code::switch_on;
break;
case (PowerStateTransition::EnableOperation):
mask_to_apply = controlword_specific::mask::enable_operation;
code_to_write = controlword_specific::code::enable_operation;
mask_to_apply = mask::enable_operation;
code_to_write = code::enable_operation;
break;
case (PowerStateTransition::DisableOperation):
mask_to_apply = controlword_specific::mask::disable_operation;
code_to_write = controlword_specific::code::disable_operation;
mask_to_apply = mask::disable_operation;
code_to_write = code::disable_operation;
break;
case (PowerStateTransition::QuickStop):
mask_to_apply = controlword_specific::mask::quick_stop;
code_to_write = controlword_specific::code::quick_stop;
mask_to_apply = mask::quick_stop;
code_to_write = code::quick_stop;
break;
case (PowerStateTransition::FaultReset):
mask_to_apply = controlword_specific::mask::fault_reset;
code_to_write = controlword_specific::code::fault_reset;
mask_to_apply = mask::fault_reset;
code_to_write = code::fault_reset;
break;
case (PowerStateTransition::NotDefined):
mask_to_apply = controlword_specific::mask::disable_voltage;
code_to_write = controlword_specific::code::disable_voltage;
mask_to_apply = mask::disable_voltage;
code_to_write = code::disable_voltage;
status = Error::ERROR;
break;
}
......
......@@ -14,8 +14,8 @@ public:
typedef int32_t position_actual_value_t;
typedef int32_t target_velocity_t;
typedef int32_t target_position_t;
typedef statusword_specific::type status_word_t;
typedef controlword_specific::type control_word_t;
typedef power_state::statusword_specific::type status_word_t;
typedef power_state::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;
......@@ -56,7 +56,7 @@ public:
this->target_velocity.write(value);
}
void setTargetPosition(target_position_t value) {
this->target_velocity.write(value);
this->target_position.write(value);
}
void setControlWord(control_word_t value) {
this->control_word.write(value);
......
......@@ -10,64 +10,66 @@ enum class PowerStateTransition{
Shutdown, SwitchOn, DisableVoltage, QuickStop, DisableOperation, EnableOperation, FaultReset, NotDefined
};
namespace statusword_specific{
typedef uint16_t type; //Type of object 6041h (Statusword)
namespace mask{
const type not_ready_to_switch_on = 0x004F;
const type switch_on_disabled = 0x004F;
const type ready_to_switch_on = 0x006F;
const type switched_on = 0x006F;
const type operation_enabled = 0x006F;
const type quick_stop_active = 0x006F;
const type fault_reaction_active = 0x004F;
const type fault = 0x004F;
}
namespace power_state{
namespace statusword_specific{
typedef uint16_t type; //Type of object 6041h (Statusword)
namespace mask{
const type not_ready_to_switch_on = 0x004F;
const type switch_on_disabled = 0x004F;
const type ready_to_switch_on = 0x006F;
const type switched_on = 0x006F;
const type operation_enabled = 0x006F;
const type quick_stop_active = 0x006F;
const type fault_reaction_active = 0x004F;
const type fault = 0x004F;
}
namespace code{
const type not_ready_to_switch_on = 0x0000;
const type switch_on_disabled = 0x0040;
const type ready_to_switch_on = 0x0021;
const type switched_on = 0x0023;
const type operation_enabled = 0x0027;
const type quick_stop_active = 0x0007;
const type fault_reaction_active = 0x000F;
const type fault = 0x0008;
namespace code{
const type not_ready_to_switch_on = 0x0000;
const type switch_on_disabled = 0x0040;
const type ready_to_switch_on = 0x0021;
const type switched_on = 0x0023;
const type operation_enabled = 0x0027;
const type quick_stop_active = 0x0007;
const type fault_reaction_active = 0x000F;
const type fault = 0x0008;
}
}
}
namespace controlword_specific{
typedef uint16_t type; //Type of object 6040h (Controlword)
namespace bit{
const uint8_t switch_on = 0;
const uint8_t enable_voltage = 1;
const uint8_t quick_stop = 2;
const uint8_t enable_operation = 3;
const uint8_t operation_mode_specific_0 = 4;
const uint8_t operation_mode_specific_1 = 5;
const uint8_t operation_mode_specific_2 = 6;
const uint8_t fault_reset = 7;
const uint8_t halt = 8;
const uint8_t operation_mode_specific_3 = 9;
}
namespace mask{
const type shutdown = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::fault_reset);
const type switch_on = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::enable_operation)|(true<<bit::fault_reset);
const type disable_voltage = (true<<bit::enable_voltage)|(true<<bit::fault_reset);
const type quick_stop = (true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::fault_reset);
const type disable_operation = switch_on;
const type enable_operation = switch_on;
const type fault_reset = (true<<bit::fault_reset);
const type operation_mode_specific = (true<<bit::operation_mode_specific_0)|(true<<bit::operation_mode_specific_1)|(true<<bit::operation_mode_specific_2)|(true<<bit::operation_mode_specific_3);
}
namespace controlword_specific{
typedef uint16_t type; //Type of object 6040h (Controlword)
namespace bit{
const uint8_t switch_on = 0;
const uint8_t enable_voltage = 1;
const uint8_t quick_stop = 2;
const uint8_t enable_operation = 3;
const uint8_t operation_mode_specific_0 = 4;
const uint8_t operation_mode_specific_1 = 5;
const uint8_t operation_mode_specific_2 = 6;
const uint8_t fault_reset = 7;
const uint8_t halt = 8;
const uint8_t operation_mode_specific_3 = 9;
}
namespace mask{
const type shutdown = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::fault_reset);
const type switch_on = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::enable_operation)|(true<<bit::fault_reset);
const type disable_voltage = (true<<bit::enable_voltage)|(true<<bit::fault_reset);
const type quick_stop = (true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::fault_reset);
const type disable_operation = switch_on;
const type enable_operation = switch_on;
const type fault_reset = (true<<bit::fault_reset);
const type operation_mode_specific = (true<<bit::operation_mode_specific_0)|(true<<bit::operation_mode_specific_1)|(true<<bit::operation_mode_specific_2)|(true<<bit::operation_mode_specific_3);
}
namespace code{
const type shutdown = (false<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(false<<bit::fault_reset);
const type switch_on = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(false<<bit::enable_operation)|(false<<bit::fault_reset);
const type disable_voltage = (false<<bit::enable_voltage)|(false<<bit::fault_reset);
const type quick_stop = (true<<bit::enable_voltage)|(false<<bit::quick_stop)|(false<<bit::fault_reset);
const type disable_operation = switch_on;
const type enable_operation = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::enable_operation)|(false<<bit::fault_reset);
const type fault_reset = (true<<bit::fault_reset);
namespace code{
const type shutdown = (false<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(false<<bit::fault_reset);
const type switch_on = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(false<<bit::enable_operation)|(false<<bit::fault_reset);
const type disable_voltage = (false<<bit::enable_voltage)|(false<<bit::fault_reset);
const type quick_stop = (true<<bit::enable_voltage)|(false<<bit::quick_stop)|(false<<bit::fault_reset);
const type disable_operation = switch_on;
const type enable_operation = (true<<bit::switch_on)|(true<<bit::enable_voltage)|(true<<bit::quick_stop)|(true<<bit::enable_operation)|(false<<bit::fault_reset);
const type fault_reset = (true<<bit::fault_reset);
}
}
}
......
#pragma once
#include "power_states.h"
namespace profile_position_mode
{
namespace controlword_specific{
typedef power_state::controlword_specific::type type; //Type of object 6040h (Controlword)
namespace bit{
const uint8_t start_travel = power_state::controlword_specific::bit::operation_mode_specific_0;
const uint8_t execute_immediately = power_state::controlword_specific::bit::operation_mode_specific_1;
const uint8_t positin_mode = power_state::controlword_specific::bit::operation_mode_specific_2;
const uint8_t change_on_setpoint = power_state::controlword_specific::bit::operation_mode_specific_3;
const uint8_t halt = power_state::controlword_specific::bit::halt;
}
namespace mask{
const type start_travel = (true<<bit::start_travel);
const type execute_immediately = (true<<bit::execute_immediately);
const type position_mode = (true<<bit::positin_mode);
const type change_on_setpoint = (true<<bit::change_on_setpoint);
const type halt = (true<<bit::halt);
}
namespace code{
const type start_travel = (true<<bit::start_travel);
const type execute_immediately = (true<<bit::execute_immediately);
const type position_mode = (true<<bit::positin_mode);
const type halt = (true<<bit::halt);
}
}
}
......@@ -29,7 +29,7 @@ class InstanceFactory final : public Singleton<InstanceFactory> {
public:
~InstanceFactory() {
}
void Configure(Robot::KindOfKinematics config);
void Init();
EMROSCmdType::Basecontroller::TargetVelocitiesInterface* GetTargetVelocitiesInstance();
EMROSCmdType::Basecontroller::ActualVelocitiesInterface* GetActualVelocitiesInstance();
EMROSCmdType::AuxiliaryController::TargetPositionsInterface* GetTargetPositionsInstance();
......
......@@ -50,9 +50,9 @@ MotorStatus* InstanceFactory::GetMotorStatusInstance()
}
void InstanceFactory::Configure(Robot::KindOfKinematics config)
void InstanceFactory::Init()
{
mDriveConfiguration = config;
mDriveConfiguration = Robot::GetKindOfKinematics();
}
Basecontroller::TargetVelocitiesInterface* InstanceFactory::GetTargetVelocitiesInstance()
......