...
 
Commits (2)
......@@ -6,6 +6,7 @@
#include "../../system/inc/msg.h"
#include "../../system/inc/factories.h"
#include "../../motors/inc/profile_position_mode.h"
#include "../../motors/inc/homing_mode.h"
using namespace EMROSCmdType::AuxiliaryController;
......@@ -23,18 +24,14 @@ private:
Error SendActualPositionsCallback();
Error CyclicCANSyncCallback();
Error CyclicTimerCallback();
void UpdateMotorTargetPosition();
Error CopyTargetPositionFromDataFrame(const EthHandler::Dataframe& dataframe);
void SetControlwordBit(uint8_t bit_position);
void ResetControlwordBit(uint8_t bit_position);
void ConfigurePositionMethod();
void SetStartTravelCommand();
void ResetStartTravelCommand();
void PreConfigurePositionMode();
void SetAbsolutePositionMode();
void SetRelativePositionMode();
void SetImmidiateAdaptionOfNewMoveOrders();
void SetFinishMoveOrdersBeforeNewMoveOrders();
Error CheckAndResetHeartbeat();
void ReactToCriticalError();
void EnterPositioningState();
void PerformPositioningState();
void EnterHomingState();
void PerformHomingState();
InstanceFactory& mInstanceFactory;
......@@ -44,7 +41,7 @@ private:
bool mHeartbeat;
bool mCritical_fault;
std::vector<Motor*>* mMotors;
Motor* mMotor;
TargetPositionsInterface* mTargetPositions;
ActualPositionsInterface* mActualPositions;
......@@ -54,7 +51,14 @@ private:
CANSyncHandler::ObserverSyncHandler mObserver_delayed_can_sync;
CANSyncHandler::ObserverSyncHandler mObserver_cyclic_can_sync;
ProfilePositionMode mProfilePositionHandle;
HomingMode mHomingModeHandle;
uint8_t mCntSyncLeftTillResetStartTravelBit;
enum OperationalState {
POSITIONING_STATE, HOMING_STATE
}mOperationalState;
};
......@@ -31,8 +31,10 @@ public:
Error sdoRead(const Index index, const Subindex subindex, bool &finished, uint16_t &readout);
Error sdoRead(const Index index, const Subindex subindex, bool &finished, uint32_t &readout);
Error sdoRead(const Index index, const Subindex subindex, bool &finished, float &readout);
Error sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint16_t write_value);
Error sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint8_t write_value);
Error sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint16_t write_value);
Error sdoWrite(const Index index, const Subindex subindex, bool &finished, const int8_t write_value);
enum SdoCommand {
READ, WRITE
......
This diff is collapsed.
......@@ -79,6 +79,20 @@ Error SdoBridgeIntern::sdoRead(const Index index, const Subindex subindex, bool
return retStatus;
}
//This method needs to be called multiple times until the finished-flag is true
Error SdoBridgeIntern::sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint8_t write_value) {
mSdo_transmission.number_bytes = sizeof(uint8_t);
union_t<uint8_t> temp { uint8_t(write_value) };
std::copy_n(temp.raw.begin(), sizeof(uint8_t), mSdo_transmission.data.begin());
Error retStatus = sdoRequestHandler(SdoBridgeIntern::SdoRequest(index, subindex, sizeof(uint8_t), SdoBridgeIntern::SdoCommand::WRITE),
finished);
return retStatus;
}
//This method needs to be called multiple times until the finished-flag is true
Error SdoBridgeIntern::sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint16_t write_value) {
......@@ -92,12 +106,13 @@ Error SdoBridgeIntern::sdoWrite(const Index index, const Subindex subindex, bool
return retStatus;
}
//This method needs to be called multiple times until the finished-flag is true
Error SdoBridgeIntern::sdoWrite(const Index index, const Subindex subindex, bool &finished, const uint8_t write_value) {
Error SdoBridgeIntern::sdoWrite(const Index index, const Subindex subindex, bool &finished, const int8_t write_value) {
mSdo_transmission.number_bytes = sizeof(uint8_t);
union_t<uint8_t> temp { uint8_t(write_value) };
std::copy_n(temp.raw.begin(), sizeof(uint8_t), mSdo_transmission.data.begin());
mSdo_transmission.number_bytes = sizeof(int8_t);
union_t<int8_t> temp { int8_t(write_value) };
std::copy_n(temp.raw.begin(), sizeof(int8_t), mSdo_transmission.data.begin());
Error retStatus = sdoRequestHandler(SdoBridgeIntern::SdoRequest(index, subindex, sizeof(uint8_t), SdoBridgeIntern::SdoCommand::WRITE),
finished);
......@@ -105,6 +120,7 @@ Error SdoBridgeIntern::sdoWrite(const Index index, const Subindex subindex, bool
return retStatus;
}
Error SdoBridgeIntern::sdoRequestHandler(SdoRequest sdo_request, bool &sdo_finished) {
Error return_value = Error::ERROR;
......
Subproject commit c733e414188b1dc5f5b0e94d3daa97aef51f03ff
Subproject commit b98a163aa1fcff8627af46b4c083a4f6ffacf21b
#pragma once
namespace controlword {
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;
}
}
#pragma once
#include "power_state.h"
#include "pdo_mapping_motors.h"
class PowerStateHelper {
public:
PowerStateHelper(MotorPdoMapping* pdoHandle):
mPdoHanlde(pdoHandle) {
}
~PowerStateHelper() {
}
private:
MotorPdoMapping* mPdoHanlde;
};
#pragma once
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include "../../emros_common/include/emros_common/emros_commands.h"
#include "motor.h"
#include "homing_mode_configuration.h"
#include "controlword.h"
#include "statusword.h"
namespace homing_mode {
namespace controlword_specific {
namespace bit {
const uint8_t start_travel =
controlword::bit::operation_mode_specific_0;
}
namespace mask {
const controlword::type start_travel = (true << bit::start_travel);
}
namespace code {
const controlword::type start_travel = (true << bit::start_travel);
}
}
namespace statusword_specific {
namespace mask {
const statusword::type homing_general = (true
<< statusword::bit::target_reached)
| (true << statusword::bit::operation_mode_specific_1)
| (true << statusword::bit::operation_mode_specific_2);
}
namespace code {
const statusword::type performing = 0;
const statusword::type not_performing = (true
<< statusword::bit::target_reached);
const statusword::type confirmed_but_not_yet_reached = (true
<< statusword::bit::operation_mode_specific_1);
const statusword::type complete = (true
<< statusword::bit::target_reached)
| (true << statusword::bit::operation_mode_specific_1);
const statusword::type error_and_turning = (true
<< statusword::bit::operation_mode_specific_2);
const statusword::type error_and_standstill = (true
<< statusword::bit::target_reached)
| (true << statusword::bit::operation_mode_specific_2);
}
}
}
class HomingMode {
public:
HomingMode(Motor* motor);
~HomingMode();
Error StartHomingProcedure();
Error PerformHomingProcedure();
bool IsHomingProcedureFinished();
private:
Motor* mMotor;
bool mHomingProcedureRunning;
HomingModeConfiguration mConfigurationHandle;
//int mCyclesSinceStartOfHoming;
int mCycleCounter;
enum class HomingState {
ConfigurationState, WaitBeforeHomingState, HomingRunState, WaitAfterHomingState, IdleState
} mHomingState;
void EnterConfigurationState();
Error PerformConfigurationState();
void EnterWaitBeforeHomingState();
Error PerformWaitBeforeHomingState();
void EnterHomingRunState();
Error PerformHomingRunState();
void EnterWaitAfterHomingState();
Error PerformWaitAfterHomingState();
void EnterIdleStateState();
Error PerformIdleStateState();
void SetControlwordBit(uint8_t bit_position);
void ResetControlwordBit(uint8_t bit_position);
void SetStartTravelCommand();
void ResetStartTravelCommand();
statusword::type GetMaskedStatusword();
};
#pragma once
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include "../../emros_common/include/emros_common/emros_commands.h"
#include "motor.h"
namespace homing_mode {
namespace method {
typedef int8_t type;
namespace code {
const type method_35_to_current_position = 35;
}
}
}
class HomingModeConfiguration
{
public:
HomingModeConfiguration(Motor* motor);
~HomingModeConfiguration();
void StartConfigurationProcedure();
Error PerformConfigurationProcedure();
bool isConfigurationProcedureFinished();
private:
Motor* mMotor;
bool mConfigurationProcedureRunning;
enum class ConfigurationState
{
ConfigureModeOfOperationState, ConfigureHomingMethodState, IdleState
}mConfigurationState;
void EnterConfigureModeOfOperationState();
Error PerformConfigureModeOfOperationState();
void EnterConfigureHomingMethodState();
Error PerformConfigureHomingMethodState();
void EnterIdleState();
Error PerformIdleState();
};
#pragma once
#include "power_states.h"
#include "../../NanoOFC/logger/inc/ofc_client_RT.h"
#include "pdo_mapping_motors.h"
#include "operation_modes.h"
#include "../../devices/inc/sdo_bridge_intern.h"
#include "power_state.h"
#include "power_state_helper.h"
namespace{
uint8_t maxRetriesFaultReset = 5;
......@@ -15,9 +18,11 @@ public:
const node_id_t node_id;
const char * name;
const uint8_t ordinal_in_datagram;
SdoBridgeIntern sdoBridgeIntern;
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){
node_id(_node_id), name(_name), ordinal_in_datagram(_position_in_datagram),sdoBridgeIntern(SdoBridgeIntern(node_id)){
}
MotorPdoMapping* getPdoHandle (void){
......@@ -50,6 +55,7 @@ public:
}
case SimplifiedMotorState::EnablingPower:
ResetControlWordBit(controlword::bit::halt);
mTargetPowerState = PowerState::OperationEnabled;
if (IsMotorInFaultState()) {
ResetEnablePowerRequest();
......@@ -130,24 +136,29 @@ public:
void DisablePower(void) {
mPowerEnableRequested = false;
}
void ResetFault(){mPowerStateMachineLockedDueToFault = false;}
bool IsEnablePowerRequested(void) {
return mPowerEnableRequested;
}
bool IsAppConfigFinished(void) {
return mApplicationConfigurationFinished;
}
void SetAppConfigOngoing(void) {
mApplicationConfigurationFinished = false;
}
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) {
PowerState GetActualPowerState(void) {
status_word_t readout;
mPdoHanlde.getStatusWord(readout);
......@@ -179,7 +190,13 @@ public:
}
}
void ResetFault(){mPowerStateMachineLockedDueToFault = false;}
modes_of_operation_specific::type GetActualModeOfOperation() {
modes_of_operation_specific::type operation_mode;
mPdoHanlde.getModeOfOperation(operation_mode);
return operation_mode;
}
private:
typedef power_state::statusword_specific::type status_word_t;
......@@ -281,7 +298,7 @@ private:
return Error::ERROR;
}
PowerState currentPowerstate = getActualPowerState();
PowerState currentPowerstate = GetActualPowerState();
if (mTargetPowerState == PowerState::QuickStopActive
&& currentPowerstate != PowerState::OperationEnabled) {
......@@ -425,7 +442,7 @@ private:
}
bool IsActualPowerStateEqualTargetState(void) {
PowerState actualPowerstate = getActualPowerState();
PowerState actualPowerstate = GetActualPowerState();
return actualPowerstate == mTargetPowerState;
}
......@@ -443,13 +460,27 @@ private:
}
bool IsMotorInFaultState(void) {
return (getActualPowerState() == PowerState::Fault
|| getActualPowerState() == PowerState::FaultReactionActive);
return (GetActualPowerState() == PowerState::Fault
|| GetActualPowerState() == PowerState::FaultReactionActive);
}
void ResetEnablePowerRequest(void) {
mPowerEnableRequested = false;
}
void SetControlWordBit(uint8_t bit_position) {
controlword::type value;
mPdoHanlde.getControlWord(value);
value |= controlword::type(true << bit_position);
mPdoHanlde.setControlWord(value);
}
void ResetControlWordBit(uint8_t bit_position) {
controlword::type value;
mPdoHanlde.getControlWord(value);
value &= controlword::type(~(true << bit_position));
mPdoHanlde.setControlWord(value);
}
};
......@@ -5,8 +5,9 @@
#include "../../utility_ofc/include/utility_ofc/singleton.h"
#include "can_network_variables.h"
#include "../../utility_ofc/include/utility_ofc/error.h"
#include "power_states.h"
#include "operation_modes.h"
#include "controlword.h"
#include "statusword.h"
class MotorPdoMapping {
public:
......@@ -14,8 +15,8 @@ public:
typedef int32_t position_actual_value_t;
typedef int32_t target_velocity_t;
typedef int32_t target_position_t;
typedef power_state::statusword_specific::type status_word_t;
typedef power_state::controlword_specific::type control_word_t;
typedef statusword::type status_word_t;
typedef controlword::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;
......
#pragma once
#include "controlword.h"
#include "statusword.h"
enum class PowerState {
NotReadyToSwitchOn,
SwitchedOnDisabled,
ReadyToSwitchOn,
SwitchedOn,
OperationEnabled,
QuickStopActive,
FaultReactionActive,
Fault,
NotSpecified
};
enum class PowerStateTransition {
Shutdown,
SwitchOn,
DisableVoltage,
QuickStop,
DisableOperation,
EnableOperation,
FaultReset,
NotDefined
};
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 controlword_specific {
typedef uint16_t type; //Type of object 6040h (Controlword)
namespace mask {
const type shutdown = (true << controlword::bit::switch_on)
| (true << controlword::bit::enable_voltage)
| (true << controlword::bit::quick_stop)
| (true << controlword::bit::fault_reset);
const type switch_on = (true << controlword::bit::switch_on)
| (true << controlword::bit::enable_voltage)
| (true << controlword::bit::quick_stop)
| (true << controlword::bit::enable_operation)
| (true << controlword::bit::fault_reset);
const type disable_voltage = (true
<< controlword::bit::enable_voltage)
| (true << controlword::bit::fault_reset);
const type quick_stop = (true << controlword::bit::enable_voltage)
| (true << controlword::bit::quick_stop)
| (true << controlword::bit::fault_reset);
const type disable_operation = switch_on;
const type enable_operation = switch_on;
const type fault_reset = (true << controlword::bit::fault_reset);
const type operation_mode_specific = (true
<< controlword::bit::operation_mode_specific_0)
| (true << controlword::bit::operation_mode_specific_1)
| (true << controlword::bit::operation_mode_specific_2)
| (true << controlword::bit::operation_mode_specific_3);
}
namespace code {
const type shutdown = (false << controlword::bit::switch_on)
| (true << controlword::bit::enable_voltage)
| (true << controlword::bit::quick_stop)
| (false << controlword::bit::fault_reset);
const type switch_on = (true << controlword::bit::switch_on)
| (true << controlword::bit::enable_voltage)
| (true << controlword::bit::quick_stop)
| (false << controlword::bit::enable_operation)
| (false << controlword::bit::fault_reset);
const type disable_voltage = (false
<< controlword::bit::enable_voltage)
| (false << controlword::bit::fault_reset);
const type quick_stop = (true << controlword::bit::enable_voltage)
| (false << controlword::bit::quick_stop)
| (false << controlword::bit::fault_reset);
const type disable_operation = switch_on;
const type enable_operation = (true << controlword::bit::switch_on)
| (true << controlword::bit::enable_voltage)
| (true << controlword::bit::quick_stop)
| (true << controlword::bit::enable_operation)
| (false << controlword::bit::fault_reset);
const type fault_reset = (true << controlword::bit::fault_reset);
}
}
}
#pragma once
#include "controlword.h"
#include "pdo_mapping_motors.h"
class ControlWorldHelper {
public:
static void SetBit(uint8_t bit_position, MotorPdoMapping* pdoHandle) {
controlword::type value;
pdoHandle->getControlWord(value);
value |= controlword::type(true << bit_position);
pdoHandle->setControlWord(value);
}
static void ResetBit(uint8_t bit_position, MotorPdoMapping* pdoHandle) {
controlword::type value;
pdoHandle->getControlWord(value);
value &= controlword::type(~(true << bit_position));
pdoHandle->setControlWord(value);
}
};
#pragma once
enum class PowerState
{
NotReadyToSwitchOn, SwitchedOnDisabled, ReadyToSwitchOn, SwitchedOn,
OperationEnabled, QuickStopActive, FaultReactionActive, Fault, NotSpecified
};
enum class PowerStateTransition{
Shutdown, SwitchOn, DisableVoltage, QuickStop, DisableOperation, EnableOperation, FaultReset, NotDefined
};
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 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);
}
}
}
#pragma once
#include "../../NanoOFC/core/inc/nano_ofc.h"
#include "power_states.h"
#include "../../emros_common/include/emros_common/emros_commands.h"
#include "motor.h"
#include "controlword.h"
namespace profile_position_mode {
namespace controlword_specific {
typedef power_state::controlword_specific::type type;//Type of object 6040h (Controlword)
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;
controlword::bit::operation_mode_specific_0;
const uint8_t execute_immediately =
power_state::controlword_specific::bit::operation_mode_specific_1;
controlword::bit::operation_mode_specific_1;
const uint8_t position_mode =
power_state::controlword_specific::bit::operation_mode_specific_2;
controlword::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;
controlword::bit::operation_mode_specific_3;
const uint8_t halt = controlword::bit::halt;
}
namespace mask {
......@@ -35,4 +37,26 @@ namespace profile_position_mode {
}
}
class ProfilePositionMode {
public:
ProfilePositionMode(Motor *motor);
~ProfilePositionMode();
void SetStartTravelCommand();
void ResetStartTravelCommand();
void ConfigureAbsolutePositionMode();
void ConfigureRelativePositionMode();
void ConfigureImmidiateAdaptionOfNewMoveOrders();
void ConfigureFinishMoveOrdersBeforeNewMoveOrders();
void SetTargetPosition(
EMROSCmdType::AuxiliaryController::TargetPositionsInterface* targetPositions);
void GetActualPosition(
EMROSCmdType::AuxiliaryController::ActualPositionsInterface* actualPositions);
void SetHalt();
void ResetHalt();
private:
void SetControlwordBit(uint8_t bit_position);
void ResetControlwordBit(uint8_t bit_position);
Motor* mMotor;
};
#pragma once
namespace statusword {
typedef uint16_t type; //Type of object 6040h (Controlword)
namespace bit {
const uint8_t ready_to_switch_on = 0;
const uint8_t switched_on = 1;
const uint8_t operation_enabled = 2;
const uint8_t fault = 3;
const uint8_t voltage_enabled = 4;
const uint8_t quick_stop = 5;
const uint8_t switched_on_disabled = 6;
const uint8_t warning = 7;
const uint8_t synchronization = 8;
const uint8_t remote = 9;
const uint8_t target_reached = 10;
const uint8_t internal_limit_active = 11;
const uint8_t operation_mode_specific_1 = 12;
const uint8_t operation_mode_specific_2 = 13;
const uint8_t dont_care = 10;
const uint8_t closed_loop_active = 14;
}
}
#include "../inc/homing_mode.h"
#include "../inc/controlword_helper.h"
namespace {
const int tolerance_time_for_homing_start_in_cycles = 5;
const int additional_waiting_time_in_cycles = 10;
}
HomingMode::HomingMode(Motor* motor) :
mMotor(motor), mHomingState(HomingState::IdleState), mHomingProcedureRunning(
false), mConfigurationHandle(motor), mCycleCounter(
0) {
}
HomingMode::~HomingMode() {
delete &mMotor;
}
Error HomingMode::StartHomingProcedure() {
if (mHomingState == HomingState::IdleState) {
mHomingProcedureRunning = true;
EnterConfigurationState();
return Error::NO_ERROR;
} else {
OFCLoggerRTClient::instance()->InsertWarning(__PRETTY_FUNCTION__,
"Attempted to start homing procedure, but it runs already in state code: ",
int(mHomingState));
return Error::ERROR;
}
}
Error HomingMode::PerformHomingProcedure() {
Error status = Error::NO_ERROR;
switch (mHomingState) {
case (HomingState::ConfigurationState):
status = PerformConfigurationState();
break;
case (HomingState::WaitBeforeHomingState):
status = PerformWaitBeforeHomingState();
break;
case (HomingState::HomingRunState):
status = PerformHomingRunState();
break;
case (HomingState::WaitAfterHomingState):
status = PerformWaitAfterHomingState();
break;
case (HomingState::IdleState):
status = PerformIdleStateState();
break;
default:
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"homing mode state ", int(mHomingState), " not implemented");
break;
}
if (status == Error::ERROR) {
EnterIdleStateState();
OFCLoggerRTClient::instance()->InsertError("Homing failed");
}
return status;
}
bool HomingMode::IsHomingProcedureFinished() {
return !mHomingProcedureRunning;
}
void HomingMode::EnterConfigurationState() {
mHomingState = HomingState::ConfigurationState;
ResetStartTravelCommand();
mConfigurationHandle.StartConfigurationProcedure();
}
Error HomingMode::PerformConfigurationState() {
Error status = Error::NO_ERROR;
status = mConfigurationHandle.PerformConfigurationProcedure();
if (status == Error::ERROR) {
return Error::ERROR;
} else if (mConfigurationHandle.isConfigurationProcedureFinished()) {
EnterWaitBeforeHomingState();
}
return Error::NO_ERROR;
}
void HomingMode::EnterWaitBeforeHomingState(){
mCycleCounter = 0;
mHomingState = HomingState::WaitBeforeHomingState;
}
Error HomingMode::PerformWaitBeforeHomingState(){
++mCycleCounter;
if(mCycleCounter > additional_waiting_time_in_cycles){
EnterHomingRunState();
}
return Error::NO_ERROR;
}
void HomingMode::EnterHomingRunState() {
mHomingState = HomingState::HomingRunState;
mCycleCounter = 0;
SetStartTravelCommand();
}
Error HomingMode::PerformHomingRunState() {
namespace alias = homing_mode::statusword_specific::code;
++mCycleCounter;
statusword::type homing_status = GetMaskedStatusword();
if ((homing_status == alias::performing)
|| (homing_status == alias::confirmed_but_not_yet_reached)) {
return Error::NO_ERROR;
} else if (homing_status == alias::complete) {
EnterWaitAfterHomingState();
return Error::NO_ERROR;
} else if (homing_status == alias::not_performing) {
if (mCycleCounter
<= tolerance_time_for_homing_start_in_cycles) {
return Error::NO_ERROR;
} else {
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"Start of homing timed out, homing failed, status code: ",
homing_status);
return Error::ERROR;
}
} else if ((homing_status == alias::error_and_standstill)
|| (homing_status == alias::error_and_turning)) {
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"homing failed, status code: ", homing_status);
return Error::ERROR;
} else {
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"unknown homing status code: ", homing_status);
return Error::ERROR;
}
}
void HomingMode::EnterWaitAfterHomingState(){
mCycleCounter = 0;
mHomingState = HomingState::WaitAfterHomingState;
}
Error HomingMode::PerformWaitAfterHomingState(){
++mCycleCounter;
if(mCycleCounter > additional_waiting_time_in_cycles){
EnterIdleStateState();
}
return Error::NO_ERROR;
}
void HomingMode::EnterIdleStateState() {
mHomingState = HomingState::IdleState;
mHomingProcedureRunning = false;
ResetStartTravelCommand();
}
Error HomingMode::PerformIdleStateState() {
mHomingProcedureRunning = false;
OFCLoggerRTClient::instance()->InsertWarning(__PRETTY_FUNCTION__,
"Homing procedure hangs in Idle State");
return Error::NO_ERROR;
}
void HomingMode::SetControlwordBit(uint8_t bit_position) {
ControlWorldHelper::SetBit(bit_position, mMotor->getPdoHandle());
}
void HomingMode::ResetControlwordBit(uint8_t bit_position) {
ControlWorldHelper::ResetBit(bit_position, mMotor->getPdoHandle());
}
void HomingMode::SetStartTravelCommand() {
SetControlwordBit(
uint8_t(homing_mode::controlword_specific::bit::start_travel));
}
void HomingMode::ResetStartTravelCommand() {
ResetControlwordBit(
uint8_t(homing_mode::controlword_specific::bit::start_travel));
}
statusword::type HomingMode::GetMaskedStatusword() {
statusword::type value = 0;
mMotor->getPdoHandle()->getStatusWord(value);
return value & homing_mode::statusword_specific::mask::homing_general;
}
#include "../inc/homing_mode_configuration.h"
namespace {
const SdoHandler::Index index_homing_method = 0x6098;
const SdoBridgeIntern::Subindex subidx_homing_method = 0;
}
HomingModeConfiguration::HomingModeConfiguration(Motor* motor) :
mMotor(motor), mConfigurationState(ConfigurationState::IdleState), mConfigurationProcedureRunning(
false) {
}
void HomingModeConfiguration::StartConfigurationProcedure() {
// if (mConfigurationState == ConfigurationState::IdleState) {
// EnterConfigureModeOfOperationState();
// }
EnterConfigureModeOfOperationState();
}
bool HomingModeConfiguration::isConfigurationProcedureFinished() {
return (!mConfigurationProcedureRunning);
}
Error HomingModeConfiguration::PerformConfigurationProcedure() {
Error status = Error::NO_ERROR;
switch (mConfigurationState) {
case (ConfigurationState::ConfigureModeOfOperationState):
status = PerformConfigureModeOfOperationState();
break;
case (ConfigurationState::ConfigureHomingMethodState):
PerformConfigureHomingMethodState();
break;
case (ConfigurationState::IdleState):
PerformIdleState();
break;
default:
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"Homing configuration state ", int(mConfigurationState),
" not implemented");
break;
}
return status;
}
void HomingModeConfiguration::EnterConfigureModeOfOperationState() {
mConfigurationState = ConfigurationState::ConfigureModeOfOperationState;
}
Error HomingModeConfiguration::PerformConfigureModeOfOperationState() {
namespace ns_op = modes_of_operation_specific::code;
mMotor->SetTargetModeOfOperation(ns_op::HomingMode);
if (mMotor->GetActualModeOfOperation() == ns_op::HomingMode) {
EnterConfigureHomingMethodState();
}
//TODO implement mechanism to avoid blocking
return Error::NO_ERROR;
}
void HomingModeConfiguration::EnterConfigureHomingMethodState() {
mConfigurationState = ConfigurationState::ConfigureHomingMethodState;
}
Error HomingModeConfiguration::PerformConfigureHomingMethodState() {
namespace ns_hm = homing_mode::method::code;
bool sdo_finished = false;
Error status = mMotor->sdoBridgeIntern.sdoWrite(index_homing_method,
subidx_homing_method, sdo_finished,
ns_hm::method_35_to_current_position);
if (status == Error::ERROR) {
EnterIdleState();
return Error::ERROR;
} else if (sdo_finished) {
EnterIdleState();
}
return Error::NO_ERROR;
}
void HomingModeConfiguration::EnterIdleState() {
mConfigurationProcedureRunning = false;
mConfigurationState = ConfigurationState::IdleState;
}
Error HomingModeConfiguration::PerformIdleState() {
mConfigurationProcedureRunning = false;
OFCLoggerRTClient::instance()->InsertWarning(__PRETTY_FUNCTION__,
"Homing procedure hangs in Idle State");
return Error::NO_ERROR;
}
#include "../inc/profile_position_mode.h"
#include "../inc/controlword_helper.h"
ProfilePositionMode::ProfilePositionMode(Motor *motor):
mMotor(motor)
{
}
void ProfilePositionMode::SetStartTravelCommand()
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::start_travel));
}
void ProfilePositionMode::ResetStartTravelCommand()
{
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::start_travel));
}
void ProfilePositionMode::ConfigureAbsolutePositionMode()
{
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::position_mode));
}
void ProfilePositionMode::ConfigureRelativePositionMode()
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::position_mode));
}
void ProfilePositionMode::ConfigureImmidiateAdaptionOfNewMoveOrders()
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::execute_immediately));
}
void ProfilePositionMode::ConfigureFinishMoveOrdersBeforeNewMoveOrders()
{
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::execute_immediately));
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::change_on_setpoint));
}
void ProfilePositionMode::SetTargetPosition(EMROSCmdType::AuxiliaryController::TargetPositionsInterface* targetPositions)
{
mMotor->getPdoHandle()->setTargetPosition(
targetPositions->dataByType()[mMotor->ordinal_in_datagram]);
}
void ProfilePositionMode::GetActualPosition(EMROSCmdType::AuxiliaryController::ActualPositionsInterface* actualPositions)
{
mMotor->getPdoHandle()->getPositionActualValue(
actualPositions->dataByType()[mMotor->ordinal_in_datagram]);
}
void ProfilePositionMode::SetHalt()
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::halt));
}
void ProfilePositionMode::ResetHalt()
{
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::halt));
}
void ProfilePositionMode::SetControlwordBit(uint8_t bit_position)
{
ControlWorldHelper::SetBit(bit_position,mMotor->getPdoHandle());
}
void ProfilePositionMode::ResetControlwordBit(uint8_t bit_position)
{
ControlWorldHelper::ResetBit(bit_position,mMotor->getPdoHandle());
}
......@@ -8,9 +8,7 @@
#include "../../system/inc/msg.h"
#include "../../system/inc/factories.h"
#include "../../utility_ofc/include/utility_ofc/singleton.h"
#include "../../motors/inc/power_states.h"
//using namespace EMROSCmdType::RobotStateController;
#include "../../motors/inc/power_state.h"
class Robotstatecontroller
{
......