Commit 1239160c authored by Christian Sponfeldner's avatar Christian Sponfeldner

WIP: Implement homing on index and limit switches

parent 0e6e4780
......@@ -8,7 +8,7 @@
#include "../../motors/inc/profile_position_mode.h"
#include "../../motors/inc/homing_mode.h"
using namespace EMROSCmdType::AuxiliaryController;
class AuxiliaryController
{
......@@ -30,9 +30,11 @@ private:
void ReactToCriticalError();
void EnterPositioningState();
void PerformPositioningState();
void EnterHomingState();
Error EnterHomingState(homing_mode::method::type homing_method);
void PerformHomingState();
void sendCommandToRos(Ethprotocol::Cmd command);
InstanceFactory& mInstanceFactory;
EthHandler::DeviceId mDevice_id;
......@@ -42,8 +44,8 @@ private:
bool mCritical_fault;
Motor* mMotor;
TargetPositionsInterface* mTargetPositions;
ActualPositionsInterface* mActualPositions;
EMROSCmdType::AuxiliaryController::TargetPositionsInterface* mTargetPositions;
EMROSCmdType::AuxiliaryController::ActualPositionsInterface* mActualPositions;
EthHandler::ObserverEthHandler mObserver_eth;
MsgBoxHandler::ObserverMsgBox mObserver_msg_box;
......
......@@ -5,7 +5,7 @@ namespace {
const int every_cansync_event = 1;
const TimerHandler::timer_event_t every_timer_event = 1;
const uint32_t period_for_reporting_actual_values_in_cansyncs = 8;
const uint8_t time_to_set_start_travel_order_in_syncs = 2;
const uint8_t time_to_set_start_travel_order_in_syncs = 3;
}
using namespace EMROSCmdType::AuxiliaryController;
......@@ -29,8 +29,8 @@ AuxiliaryController::AuxiliaryController() :
POSITIONING_STATE), mCntSyncLeftTillResetStartTravelBit(0) {
EnterPositioningState();
PreConfigurePositionMode();
mMotor->SetApplicationConfigFinished();
//PreConfigurePositionMode();
//mMotor->SetApplicationConfigFinished();
}
AuxiliaryController::~AuxiliaryController() {
......@@ -102,8 +102,26 @@ Error AuxiliaryController::EthMsgCallback(
mProfilePositionHandle.SetStartTravelCommand();
mCntSyncLeftTillResetStartTravelBit = time_to_set_start_travel_order_in_syncs;
break;
case HOME_TO_NEGATIVE_LIMIT_SWITCH_AND_INDEX_METHOD_1:
EnterHomingState(homing_mode::method::code::method_1_home_to_negative_limit_switch_and_index);
break;
case HOME_TO_POSITIVE_LIMIT_SWITCH_AND_INDEX_METHOD_2:
EnterHomingState(homing_mode::method::code::method_2_home_to_positive_limit_switch_and_index);
break;
case HOME_TO_NEGATIVE_LIMIT_SWITCH_METHOD_17:
EnterHomingState(homing_mode::method::code::method_17_home_to_negative_limit_switch);
break;
case HOME_TO_POSITIVE_LIMIT_SWITCH_METHOD_18:
EnterHomingState(homing_mode::method::code::method_18_home_to_positive_limit_switch);
break;
case HOME_TO_INDEX_IN_NEGATIVE_DIRECTION_METHOD_33:
EnterHomingState(homing_mode::method::code::method_33_home_to_negative_index);
break;
case HOME_TO_INDEX_IN_POSITIVE_DIRECTION_METHOD_34:
EnterHomingState(homing_mode::method::code::method_34_home_to_positive_index);
break;
case HOME_TO_CURRENT_POSITION_METHOD_35:
EnterHomingState();
EnterHomingState(homing_mode::method::code::method_35_home_to_current_position);
break;
default:
break;
......@@ -173,11 +191,11 @@ Error AuxiliaryController::CopyTargetPositionFromDataFrame(
return (status);
}
void AuxiliaryController::PreConfigurePositionMode() {
mProfilePositionHandle.ResetStartTravelCommand();
mProfilePositionHandle.ConfigureAbsolutePositionMode();
mProfilePositionHandle.ConfigureImmidiateAdaptionOfNewMoveOrders();
}
//void AuxiliaryController::PreConfigurePositionMode() {
// mProfilePositionHandle.ResetStartTravelCommand();
// mProfilePositionHandle.ConfigureAbsolutePositionMode();
// mProfilePositionHandle.ConfigureImmidiateAdaptionOfNewMoveOrders();
//}
Error AuxiliaryController::CheckAndResetHeartbeat() {
if (mHeartbeat == true) {
......@@ -197,35 +215,55 @@ void AuxiliaryController::ReactToCriticalError() {
}
void AuxiliaryController::EnterPositioningState(){
mProfilePositionHandle.ResetStartTravelCommand();
mOperationalState = POSITIONING_STATE;
mMotor->SetTargetModeOfOperation(
modes_of_operation_specific::code::ProfilePosition);
mProfilePositionHandle.ConfigureAbsolutePositionMode();
mProfilePositionHandle.ConfigureImmidiateAdaptionOfNewMoveOrders();
mMotor->SetApplicationConfigFinished();
}
void AuxiliaryController::PerformPositioningState() {
static uint8_t syn_cnt = 0;
if (syn_cnt != 0){
--syn_cnt;
}else{
if (mProfilePositionHandle.IsSetPointAcknoledged()){
mProfilePositionHandle.ResetStartTravelCommand();
}
}
void AuxiliaryController::EnterHomingState()
Error AuxiliaryController::EnterHomingState(homing_mode::method::type homing_method)
{
mOperationalState =HOMING_STATE;
OFCLoggerRTClient::instance()->InsertInfo("Start Homing of ",mMotor->name);
mHomingModeHandle.StartHomingProcedure();
if(mHomingModeHandle.StartHomingProcedure(homing_method)==Error::ERROR){
return Error::ERROR;
}else{
return Error::NO_ERROR;
}
}
void AuxiliaryController::PerformHomingState()
{
if(mHomingModeHandle.PerformHomingProcedure() == Error::ERROR){
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
" homing failed");
sendCommandToRos(HOMING_ERROR);
EnterPositioningState();
}else if (mHomingModeHandle.IsHomingProcedureFinished()) {
OFCLoggerRTClient::instance()->InsertInfo("Homing of ",mMotor->name," complete");
sendCommandToRos(HOMING_COMPLETED_SUCCESSFULLY);
EnterPositioningState();
}
}
void AuxiliaryController::sendCommandToRos(Ethprotocol::Cmd command){
if (mRos_link_enabled == true) {
EthHandler::Dataframe tx_eth_msg;
tx_eth_msg.clearData();
tx_eth_msg.setData().id = mDevice_id;
tx_eth_msg.setData().cmd = command;
tx_eth_msg.setData().time =
CANSyncHandler::instance()->GetTimeOfLastSyncInUs();
EthHandler::instance()->SendEthMsg(tx_eth_msg);
}
}
Subproject commit b98a163aa1fcff8627af46b4c083a4f6ffacf21b
Subproject commit 6a312e36234cd99cd697ba6505c131b3e094e7ba
......@@ -55,7 +55,7 @@ public:
HomingMode(Motor* motor);
~HomingMode();
Error StartHomingProcedure();
Error StartHomingProcedure(homing_mode::method::type homing_mehod);
Error PerformHomingProcedure();
bool IsHomingProcedureFinished();
......@@ -68,17 +68,17 @@ private:
enum class HomingState {
ConfigurationState, WaitBeforeHomingState, HomingRunState, WaitAfterHomingState, IdleState
ConfigurationState, WaitBeforeHomingState, HomingRunState, CheckStatuswordAfterHomingState, IdleState
} mHomingState;
void EnterConfigurationState();
Error EnterConfigurationState(homing_mode::method::type homing_mehod);
Error PerformConfigurationState();
void EnterWaitBeforeHomingState();
Error PerformWaitBeforeHomingState();
void EnterHomingRunState();
Error PerformHomingRunState();
void EnterWaitAfterHomingState();
Error PerformWaitAfterHomingState();
void EnterCheckStatuswordAfterHomingState();
Error PerformCheckStatuswordAfterHomingState();
void EnterIdleStateState();
Error PerformIdleStateState();
......
......@@ -9,7 +9,13 @@ namespace homing_mode {
namespace method {
typedef int8_t type;
namespace code {
const type method_35_to_current_position = 35;
const type method_1_home_to_negative_limit_switch_and_index = 1;
const type method_2_home_to_positive_limit_switch_and_index = 2;
const type method_17_home_to_negative_limit_switch = 17;
const type method_18_home_to_positive_limit_switch = 18;
const type method_33_home_to_negative_index = 33;
const type method_34_home_to_positive_index = 34;
const type method_35_home_to_current_position = 35;
}
}
}
......@@ -21,13 +27,14 @@ public:
HomingModeConfiguration(Motor* motor);
~HomingModeConfiguration();
void StartConfigurationProcedure();
Error StartConfigurationProcedure(homing_mode::method::type homing_method);
Error PerformConfigurationProcedure();
bool isConfigurationProcedureFinished();
private:
Motor* mMotor;
bool mConfigurationProcedureRunning;
homing_mode::method::type mHomingMethod;
enum class ConfigurationState
{
......
......@@ -87,7 +87,7 @@ public:
} else if (!IsMotorConfigReady()
|| !IsEnablePowerRequested()
|| !IsActualPowerStateEqualTargetState()) {
DisablePower();
//DisablePower();
mMotorState = SimplifiedMotorState::Disabled;
break;
} else {
......@@ -159,7 +159,7 @@ public:
}
PowerState GetActualPowerState(void) {
status_word_t readout;
status_word_t readout = 0;
mPdoHanlde.getStatusWord(readout);
using namespace power_state::statusword_specific;
......
......@@ -35,6 +35,22 @@ namespace profile_position_mode {
const type halt = (true << bit::halt);
}
}
namespace statusword_specific {
namespace mask {
const statusword::type setpoint_acknoledge = (true
<< statusword::bit::operation_mode_specific_1);
const statusword::type following_error = (true
<< statusword::bit::operation_mode_specific_2);
}
namespace code {
const statusword::type setpoint_is_acknoledged = (true
<< statusword::bit::operation_mode_specific_1);
const statusword::type following_error_is_occured = (true
<< statusword::bit::operation_mode_specific_2);
}
}
}
class ProfilePositionMode {
......@@ -53,6 +69,7 @@ public:
EMROSCmdType::AuxiliaryController::ActualPositionsInterface* actualPositions);
void SetHalt();
void ResetHalt();
bool IsSetPointAcknoledged();
private:
void SetControlwordBit(uint8_t bit_position);
......
......@@ -3,13 +3,13 @@
namespace {
const int tolerance_time_for_homing_start_in_cycles = 5;
const int additional_waiting_time_in_cycles = 10;
const int before_homing_waiting_time_in_cycles = 10;
const int after_homing_waiting_time_in_cycles = 10;
}
HomingMode::HomingMode(Motor* motor) :
mMotor(motor), mHomingState(HomingState::IdleState), mHomingProcedureRunning(
false), mConfigurationHandle(motor), mCycleCounter(
0) {
false), mConfigurationHandle(motor), mCycleCounter(0) {
}
......@@ -17,11 +17,16 @@ HomingMode::~HomingMode() {
delete &mMotor;
}
Error HomingMode::StartHomingProcedure() {
Error HomingMode::StartHomingProcedure(homing_mode::method::type homing_mehod) {
if (mHomingState == HomingState::IdleState) {
mHomingProcedureRunning = true;
EnterConfigurationState();
return Error::NO_ERROR;
OFCLoggerRTClient::instance()->InsertInfo("Start Homing of ",mMotor->name," (homing_method ", int(homing_mehod));
if(EnterConfigurationState(homing_mehod)==Error::ERROR){
EnterIdleStateState();
return Error::ERROR;
}else{
return Error::NO_ERROR;
}
} else {
OFCLoggerRTClient::instance()->InsertWarning(__PRETTY_FUNCTION__,
"Attempted to start homing procedure, but it runs already in state code: ",
......@@ -42,8 +47,8 @@ Error HomingMode::PerformHomingProcedure() {
case (HomingState::HomingRunState):
status = PerformHomingRunState();
break;
case (HomingState::WaitAfterHomingState):
status = PerformWaitAfterHomingState();
case (HomingState::CheckStatuswordAfterHomingState):
status = PerformCheckStatuswordAfterHomingState();
break;
case (HomingState::IdleState):
status = PerformIdleStateState();
......@@ -66,10 +71,17 @@ bool HomingMode::IsHomingProcedureFinished() {
return !mHomingProcedureRunning;
}
void HomingMode::EnterConfigurationState() {
Error HomingMode::EnterConfigurationState(
homing_mode::method::type homing_mehod) {
mHomingState = HomingState::ConfigurationState;
ResetStartTravelCommand();
mConfigurationHandle.StartConfigurationProcedure();
if (mConfigurationHandle.StartConfigurationProcedure(homing_mehod)
== Error::ERROR) {
EnterIdleStateState();
return Error::ERROR;
} else {
return Error::NO_ERROR;
}
}
Error HomingMode::PerformConfigurationState() {
......@@ -83,14 +95,14 @@ Error HomingMode::PerformConfigurationState() {
return Error::NO_ERROR;
}
void HomingMode::EnterWaitBeforeHomingState(){
void HomingMode::EnterWaitBeforeHomingState() {
mCycleCounter = 0;
mHomingState = HomingState::WaitBeforeHomingState;
}
Error HomingMode::PerformWaitBeforeHomingState(){
Error HomingMode::PerformWaitBeforeHomingState() {
++mCycleCounter;
if(mCycleCounter > additional_waiting_time_in_cycles){
if (mCycleCounter > before_homing_waiting_time_in_cycles) {
EnterHomingRunState();
}
return Error::NO_ERROR;
......@@ -110,11 +122,10 @@ Error HomingMode::PerformHomingRunState() {
|| (homing_status == alias::confirmed_but_not_yet_reached)) {
return Error::NO_ERROR;
} else if (homing_status == alias::complete) {
EnterWaitAfterHomingState();
EnterCheckStatuswordAfterHomingState();
return Error::NO_ERROR;
} else if (homing_status == alias::not_performing) {
if (mCycleCounter
<= tolerance_time_for_homing_start_in_cycles) {
if (mCycleCounter <= tolerance_time_for_homing_start_in_cycles) {
return Error::NO_ERROR;
} else {
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
......@@ -135,14 +146,25 @@ Error HomingMode::PerformHomingRunState() {
}
void HomingMode::EnterWaitAfterHomingState(){
void HomingMode::EnterCheckStatuswordAfterHomingState() {
mCycleCounter = 0;
mHomingState = HomingState::WaitAfterHomingState;
mHomingState = HomingState::CheckStatuswordAfterHomingState;
}
Error HomingMode::PerformWaitAfterHomingState(){
Error HomingMode::PerformCheckStatuswordAfterHomingState() {
++mCycleCounter;
if(mCycleCounter > additional_waiting_time_in_cycles){
EnterIdleStateState();
if (mCycleCounter > after_homing_waiting_time_in_cycles) {
namespace alias = homing_mode::statusword_specific::code;
statusword::type homing_status = GetMaskedStatusword();
if (homing_status == alias::complete) {
EnterIdleStateState();
return Error::NO_ERROR;
} else {
OFCLoggerRTClient::instance()->InsertDebug(__PRETTY_FUNCTION__,
"Status word (masked) changed from 'complete' to ",
int(homing_status), " after homing");
EnterHomingRunState();
return Error::NO_ERROR;
}
}
return Error::NO_ERROR;
}
......@@ -157,7 +179,7 @@ Error HomingMode::PerformIdleStateState() {
mHomingProcedureRunning = false;
OFCLoggerRTClient::instance()->InsertWarning(__PRETTY_FUNCTION__,
"Homing procedure hangs in Idle State");
return Error::NO_ERROR;
return Error::ERROR;
}
void HomingMode::SetControlwordBit(uint8_t bit_position) {
......@@ -185,3 +207,4 @@ statusword::type HomingMode::GetMaskedStatusword() {
}
......@@ -3,18 +3,32 @@
namespace {
const SdoHandler::Index index_homing_method = 0x6098;
const SdoBridgeIntern::Subindex subidx_homing_method = 0;
const homing_mode::method::type lowest_allowed_homing_method_code =
homing_mode::method::code::method_1_home_to_negative_limit_switch_and_index;
const homing_mode::method::type highest_allowed_homing_method_code =
homing_mode::method::code::method_35_home_to_current_position;
const homing_mode::method::type default_homing_method_code =
homing_mode::method::code::method_35_home_to_current_position;
}
HomingModeConfiguration::HomingModeConfiguration(Motor* motor) :
mMotor(motor), mConfigurationState(ConfigurationState::IdleState), mConfigurationProcedureRunning(
false) {
false), mHomingMethod(default_homing_method_code) {
}
void HomingModeConfiguration::StartConfigurationProcedure() {
// if (mConfigurationState == ConfigurationState::IdleState) {
// EnterConfigureModeOfOperationState();
// }
EnterConfigureModeOfOperationState();
Error HomingModeConfiguration::StartConfigurationProcedure(
homing_mode::method::type homing_method) {
if (homing_method >= lowest_allowed_homing_method_code
&& homing_method <= highest_allowed_homing_method_code) {
mHomingMethod = homing_method;
EnterConfigureModeOfOperationState();
return Error::NO_ERROR;
} else {
OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__,
"Homing method code ", int(homing_method), " not valid");
return Error::ERROR;
}
}
bool HomingModeConfiguration::isConfigurationProcedureFinished() {
......@@ -44,6 +58,7 @@ Error HomingModeConfiguration::PerformConfigurationProcedure() {
}
void HomingModeConfiguration::EnterConfigureModeOfOperationState() {
mConfigurationProcedureRunning = true;
mConfigurationState = ConfigurationState::ConfigureModeOfOperationState;
}
......@@ -61,11 +76,9 @@ void HomingModeConfiguration::EnterConfigureHomingMethodState() {
}
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);
subidx_homing_method, sdo_finished, mHomingMethod);
if (status == Error::ERROR) {
EnterIdleState();
return Error::ERROR;
......@@ -82,6 +95,6 @@ void HomingModeConfiguration::EnterIdleState() {
Error HomingModeConfiguration::PerformIdleState() {
mConfigurationProcedureRunning = false;
OFCLoggerRTClient::instance()->InsertWarning(__PRETTY_FUNCTION__,
"Homing procedure hangs in Idle State");
return Error::NO_ERROR;
"Homing configuration procedure hangs in Idle State");
return Error::ERROR;
}
......@@ -73,3 +73,11 @@ void ProfilePositionMode::ResetControlwordBit(uint8_t bit_position)
{
ControlWorldHelper::ResetBit(bit_position,mMotor->getPdoHandle());
}
bool ProfilePositionMode::IsSetPointAcknoledged(){
statusword::type value = 0;
mMotor->getPdoHandle()->getStatusWord(value);
namespace ns_alias = profile_position_mode::statusword_specific;
value &= ns_alias::mask::setpoint_acknoledge;
return (value == ns_alias::code::setpoint_is_acknoledged);
}
......@@ -1297,6 +1297,176 @@ AccessType=rw
DefaultValue=0
PDOMapping=0
[3240]
ParameterName=Digital Inputs Control
ObjectType=0x8
SubNumber=0x08
[3240sub0]
ParameterName=Highest sub-index supported
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=8
PDOMapping=0
[3240sub1]
ParameterName=Special Function Enable
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=0
PDOMapping=0
ParameterValue=3
[3240sub2]
ParameterName=Function Inverted
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=0
PDOMapping=0
[3701]
ParameterName=Limit Switch Error Option Code
ObjectType=0x7
DataType=0x0006
AccessType=rww
DefaultValue=0xFFFF
PDOMapping=1
[607A]
ParameterName=Target Position
ObjectType=0x7
DataType=0x0004
AccessType=rww
DefaultValue=4000
PDOMapping=1
ParameterValue=0
[607D]
ParameterName=Software Position Limit
ObjectType=0x8
SubNumber=0x08
[607Dsub0]
ParameterName=Highest sub-index supported
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=2
PDOMapping=0
[607Dsub1]
ParameterName=Min Position Limit
ObjectType=0x7
DataType=0x0004
AccessType=rw
DefaultValue=0
PDOMapping=0
[607Dsub2]
ParameterName=Max Position Limit
ObjectType=0x7
DataType=0x0004
AccessType=rw
DefaultValue=0
PDOMapping=0
[6080]
ParameterName=Max motor speed
ObjectType=0x7
DataType=0x0007
AccessType=rww
DefaultValue=6000
PDOMapping=1
ParameterValue=62819
[6081]
ParameterName=Profile Velocity
ObjectType=0x7
DataType=0x0007
AccessType=rww
DefaultValue=500
PDOMapping=1
ParameterValue=52360
[6083]
ParameterName=Profile Acceleration
ObjectType=0x7
DataType=0x0007
AccessType=rww
DefaultValue=500
PDOMapping=1
ParameterValue=52360
[6084]
ParameterName=Profile Deceleration
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=500
PDOMapping=1
ParameterValue=52360
[6099]
ParameterName=Homing Speed
ObjectType=0x8
SubNumber=0x02
[6099sub0]
ParameterName=Highest sub-index supported
ObjectType=0x7
DataType=0x0005
AccessType=r
DefaultValue=2
PDOMapping=0
[6099sub1]
ParameterName=Speed During Search For Switch
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=50
PDOMapping=0
ParameterValue=5236
[6099sub2]
ParameterName=Speed During Search For Zero
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=10
PDOMapping=0
ParameterValue=1047
[609A]
ParameterName=Homing Acceleration
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=500
PDOMapping=1
ParameterValue=52360
[60A9]
ParameterName=SI unit velocity
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=0x00B44700
PDOMapping=0
ParameterValue=0xFD100300
[60A8]
ParameterName=SI unit position
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=0xFF410000
PDOMapping=0
ParameterValue=0xFD100000
[60C2]
ParameterName=Interpolation time period
ObjectType=0x9
......@@ -1391,40 +1561,24 @@ ParameterValue=1
[60A9]
ParameterName=SI unit velocity
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=0x00B44700
PDOMapping=0
ParameterValue=0xfd100300
[6080]
ParameterName=Max motor speed
ObjectType=0x7
DataType=0x0007
AccessType=rww
DefaultValue=6000
PDOMapping=1
ParameterValue=200000
[2061]
ParameterName= Velocity Numerator
ObjectType=0x7
DataType=0x0007
AccessType=rww
DefaultValue=0x01
PDOMapping=1
ParameterValue=1000000
[2062]
ParameterName= Velocity Denominator
ObjectType=0x7
DataType=0x0007
AccessType=rww
DefaultValue=0x01
PDOMapping=1
ParameterValue=3141592654
;
;
;[2061]
;ParameterName= Velocity Numerator
;ObjectType=0x7
;DataType=0x0007
;AccessType=rww
;DefaultValue=0x01
;PDOMapping=1
;ParameterValue=1000000
;
;[2062]
;ParameterName= Velocity Denominator
;ObjectType=0x7
;DataType=