// Moves the device to the specified position and return
// the actual position of the device.
// The movement is relative to current position
double MM3000::MoveInDeviceUnit(double position)
{
if(isSafeMove(ConvertToUserUnit(position, "ps")))
{
validatePosition = 0;
position = getActualPosition() + position;
return GoToInDeviceUnit(position);
validatePosition = 1;
}
}
// Moves the device to the specified position and return
// the position of the device in device units.
// The movement is absolute
double MM3000::GoToInDeviceUnit(double position)
{
if(isSafeMove(ConvertToUserUnit(position, "ps")))
{
validatePosition = 0;
int curr_axis = getAxis(); //Used to select axis is being used (currently 2)
int p =(int)position;
char command[30]; //Place holders for the commands to be sent
char subcommand[30];
//int wait = 1;
sprintf(command,"%d%s%ld",curr_axis,"pa", p); //Writes the command "pa" to direct position absolute
//sprintf(subcommand,"%d%s%ld",curr_axis,"ws", wait); /*Writes the command "ws" to tell the motor to wait for
// motion to complete for 500ms to execute next command */
WriteData(command); //Sends move command
//WriteData(subcommand); //Sends wait command
while(IsMoving()) //Sleep(1000); //Check if device is still moving every 1000ms
Sleep(20);
return getActualPosition(); //Return the current position when device stops
validatePosition = 1;
}
}
void AMCSubNode::update() {
double current, velocity, position;
processStatus();
if (getState() != AMCSubNode::OperationEnabled) {
setPosition(tpdo_ptr_->actual_position);
}
if (getState() == AMCSubNode::Fault) {
// go to ReadyToSwitchOn state
RTT::log(RTT::Warning) << "Fault" << RTT::endlog();
target_state_ = ResetFault;
} else if (getState() == AMCSubNode::QuickStopActive) {
// go to ReadyToSwitchOn state
RTT::log(RTT::Warning) << "QuickStop" << RTT::endlog();
target_state_ = DisableVoltage;
} else if (getState() == AMCSubNode::NotReadyToSwitchOn) {
// go to ReadyToSwitchOn state
//RTT::log(RTT::Warning) << "NotReadyToSwitchOn" << RTT::endlog();
target_state_ = ShutDown;
} else if (getState() == AMCSubNode::SwitchOnDisabled) {
// go to ReadyToSwitchOn state
//RTT::log(RTT::Warning) << "ReadyToSwitchOn" << RTT::endlog();
target_state_ = ShutDown;
} else if (getState() == AMCSubNode::ReadyToSwitchOn) {
// go to OperationDisabled state
//RTT::log(RTT::Warning) << "OperationDisabled" << RTT::endlog();
setPosition(tpdo_ptr_->actual_position);
target_state_ = SwitchOn;
} else if (getState() == AMCSubNode::SwitchedOn) {
//RTT::log(RTT::Warning) << "SwitchedOn" << RTT::endlog();
setPosition(tpdo_ptr_->actual_position);
if (position_valid_) {
RTT::log(RTT::Warning) << "enable" << RTT::endlog();
setControlMode(mode_);
target_state_ = EnableOperation;
} else {
RTT::log(RTT::Warning) << "homing" << RTT::endlog();
setControlMode(Homing);
target_state_ = BeginHoming;
homing_ = true;
}
} else if (getState() == AMCSubNode::OperationEnabled) {
//RTT::log(RTT::Warning) << "OperationEnabled" << RTT::endlog();
if (position_valid_) {
if (port_motor_position_command_.read(position) == RTT::NewData) {
setPosition(position);
}
if (port_motor_velocity_command_.read(velocity) == RTT::NewData) {
setVelocity(velocity);
}
if (port_motor_current_command_.read(current) == RTT::NewData) {
setCurrent(current);
}
// dio
bool dio;
if (port_do1_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(0);
} else {
resetDO(0);
}
}
if (port_do2_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(1);
} else {
resetDO(1);
}
}
if (port_do3_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(2);
} else {
resetDO(2);
}
}
if (port_do4_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(3);
} else {
resetDO(3);
}
}
} else {
if ((actualMode_ == Homing)) {
if (isHomingCompleted()) {
position_valid_ = true;
setPosition(tpdo_ptr_->actual_position);
setControlMode(mode_);
target_state_ = EnableOperation;
} else {
target_state_ = BeginHoming;
}
} else {
setControlMode(Homing);
target_state_ = BeginHoming;
}
}
}
enterStateNoCheck(target_state_);
if (position_valid_) {
port_motor_position_.write(getActualPosition());
port_motor_velocity_.write(getActualVelocity());
port_motor_current_.write(getActualCurrent());
}
}
