// 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()); } }