Commit ad29ff2a authored by Christian Sponfeldner's avatar Christian Sponfeldner

WIP: Implement ProfilePosition mode for auxiliary motor (Part 3)

parent 1d71579a
......@@ -25,17 +25,22 @@ private:
Error CyclicTimerCallback();
void UpdateMotorTargetPosition();
Error CopyTargetPositionFromDataFrame(const EthHandler::Dataframe& dataframe);
void SetStartTravelCommand();
void ResetStartTravelCommand();
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();
InstanceFactory& mInstanceFactory;
EthHandler::DeviceId mDevice_id;
bool mRos_link_enabled;
bool mCSV_mode_enabled;
bool mHeartbeat;
bool mCritical_fault;
......
......@@ -4,7 +4,7 @@
namespace {
const int every_cansync_event = 1;
const TimerHandler::timer_event_t every_timer_event = 1;
const uint32_t filter_period_in_cansyncs = 8;
const uint32_t period_for_reporting_actual_values_in_cansyncs = 8;
}
using namespace EMROSCmdType::AuxiliaryController;
......@@ -13,7 +13,6 @@ 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()),
......@@ -23,11 +22,14 @@ AuxiliaryController::AuxiliaryController() :
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) {
mObserver_delayed_can_sync(period_for_reporting_actual_values_in_cansyncs,&AuxiliaryController::SendActualPositionsCallback,this){
for (auto motor : *mMotors) {
motor->SetTargetModeOfOperation(
modes_of_operation_specific::code::ProfilePosition);
}
PreConfigurePositionMode();
for (auto motor : *mMotors) {
motor->SetApplicationConfigFinished();
}
......@@ -35,6 +37,8 @@ AuxiliaryController::AuxiliaryController() :
AuxiliaryController::~AuxiliaryController() {
delete &mMotors;
delete &mTargetPositions;
delete &mActualPositions;
}
......@@ -82,9 +86,22 @@ Error AuxiliaryController::EthMsgCallback(const EthHandler::Dataframe& dataframe
EthHandler::Dataframe tx_eth_msg { };
switch (dataframe.getData().cmd) {
case SET_ABSOLUT_POSITION_MODE:
SetAbsolutePositionMode();
break;
case SET_RELATIVE_POSITION_MODE:
SetRelativePositionMode();
break;
case SET_IMMIDIATE_ADAPTION_OF_NEW_MOVE_ORDERS:
SetImmidiateAdaptionOfNewMoveOrders();
break;
case SET_FINISH_MOVE_BEFORE_NEW_MOVE_ORDERS:
SetFinishMoveOrdersBeforeNewMoveOrders();
break;
case TARGET_POSITION:
CopyTargetPositionFromDataFrame(dataframe);
UpdateMotorTargetPosition();
ConfigurePositionMethod();
SetStartTravelCommand();
break;
default:
......@@ -122,6 +139,7 @@ Error AuxiliaryController::CyclicCANSyncCallback() {
}
Error AuxiliaryController::SendActualPositionsCallback() {
Error status { Error::NO_ERROR };
if (mRos_link_enabled == true) {
EthHandler::Dataframe tx_eth_msg;
tx_eth_msg.setData().id = mDevice_id;
......@@ -129,8 +147,17 @@ Error AuxiliaryController::SendActualPositionsCallback() {
tx_eth_msg.setData().time =
CANSyncHandler::instance()->GetTimeOfLastSyncInUs();
for (auto motor : *mMotors) {
int32_t temp;
// motor->getPdoHandle()->getPositionActualValue(
// mActualPositions->dataByType()[motor->ordinal_in_datagram]);
motor->getPdoHandle()->getPositionActualValue(
mActualPositions->dataByType()[motor->ordinal_in_datagram]);
temp);
mActualPositions->dataByType()[motor->ordinal_in_datagram] = temp;
status = mActualPositions->get(tx_eth_msg.setData().payload.begin(),
Ethprotocol::MAX_PAYLOAD_LENGTH);
if (status != Error::NO_ERROR) {
return (status);
}
}
tx_eth_msg.setData().payload_length = Ethprotocol::PayLen(
......@@ -138,7 +165,7 @@ Error AuxiliaryController::SendActualPositionsCallback() {
EthHandler::instance()->SendEthMsg(tx_eth_msg);
}
return (Error::NO_ERROR);
return (status);
}
......@@ -154,7 +181,6 @@ Error AuxiliaryController::CopyTargetPositionFromDataFrame(const EthHandler::Dat
return (status);
}
void AuxiliaryController::UpdateMotorTargetPosition() {
for (auto motor : *mMotors) {
motor->getPdoHandle()->setTargetPosition(
......@@ -162,10 +188,16 @@ void AuxiliaryController::UpdateMotorTargetPosition() {
}
}
void AuxiliaryController::ConfigurePositionMethod(void)
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::execute_immediately));
}
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));
......@@ -195,3 +227,28 @@ void AuxiliaryController::ResetControlwordBit(uint8_t bit_position)
}
}
void AuxiliaryController::PreConfigurePositionMode()
{
SetAbsolutePositionMode();
SetImmidiateAdaptionOfNewMoveOrders();
}
void AuxiliaryController::SetAbsolutePositionMode()
{
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::position_mode));
}
void AuxiliaryController::SetRelativePositionMode()
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::position_mode));
}
void AuxiliaryController::SetImmidiateAdaptionOfNewMoveOrders()
{
SetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::execute_immediately));
}
void AuxiliaryController::SetFinishMoveOrdersBeforeNewMoveOrders()
{
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::execute_immediately));
ResetControlwordBit(uint8_t(profile_position_mode::controlword_specific::bit::change_on_setpoint));
}
Subproject commit d8b325c263e750d710fb35fde959c91ce83da895
Subproject commit c733e414188b1dc5f5b0e94d3daa97aef51f03ff
#pragma once
#include "../../NanoOFC/core/inc/nano_ofc.h"
#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;
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 position_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 mask {
const type start_travel = (true << bit::start_travel);
const type execute_immediately = (true << bit::execute_immediately);
const type position_mode = (true << bit::position_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);
namespace code {
const type start_travel = (true << bit::start_travel);
const type execute_immediately = (true << bit::execute_immediately);
const type position_mode = (true << bit::position_mode);
const type halt = (true << bit::halt);
}
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment