|
@@ -69,6 +69,11 @@ void BoatModule::subscribe()
|
|
|
char cmd[MAX_NAME_LEN];
|
|
|
sprintf(cmd, "%s.SetBoatMotion.boatload",MODULE_NAME);
|
|
|
auto opNode=OP->subscribe(cmd);
|
|
|
+ opNode->addReset([]()->OperatorStatusEnum {
|
|
|
+ //异常之后boat由上位机手工操作进行复位,这里直接设置成IDLE保证recipe能够继续执行
|
|
|
+ boatModule->status = BoatStatusEnum::Idle;
|
|
|
+ return OperatorStatusEnum::SUCCESS;
|
|
|
+ });
|
|
|
//第一个CheckPrepareMove
|
|
|
auto subOp=opNode->addChild("CheckPrepareMove");
|
|
|
|
|
@@ -142,7 +147,18 @@ void BoatModule::subscribe()
|
|
|
subOp = opNode->addChild("SetBoatZAxisMove");
|
|
|
subOp->addExec([]()->OperatorStatusEnum {
|
|
|
OP->current->setTimes( 0, 0, 0, boatModule->boatElevator->scMotionTimeout);
|
|
|
- boatModule->setBoatZAxisMove(SERVO_MOVE_POSITION(1));//CAP2
|
|
|
+ double defaultSpeed = boatModule->boatElevator->scMoveSpeed;
|
|
|
+ double speed = 0;
|
|
|
+ if (OP->currentRoot->params.length > 1) {
|
|
|
+ //TODO:根据参数直接解析出速度
|
|
|
+ speed = atof(OP->currentRoot->params.get(1));
|
|
|
+
|
|
|
+ }
|
|
|
+ if (speed<0.001) {
|
|
|
+ speed = defaultSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ boatModule->setBoatZAxisMove(SERVO_MOVE_POSITION(1),speed);//CAP2
|
|
|
return OperatorStatusEnum::RUNNING;
|
|
|
});
|
|
|
subOp->addCheck([]()->OperatorStatusEnum {
|
|
@@ -180,6 +196,11 @@ void BoatModule::subscribe()
|
|
|
#pragma region boatunload
|
|
|
sprintf(cmd, "%s.SetBoatMotion.boatunload", MODULE_NAME);
|
|
|
opNode = OP->subscribe(cmd);
|
|
|
+ opNode->addReset([]()->OperatorStatusEnum {
|
|
|
+ //异常之后boat由上位机手工操作进行复位,这里直接设置成IDLE保证recipe能够继续执行
|
|
|
+ boatModule->status = BoatStatusEnum::Idle;
|
|
|
+ return OperatorStatusEnum::SUCCESS;
|
|
|
+ });
|
|
|
//第一个CheckPrepareMove
|
|
|
subOp = opNode->addChild("CheckPrepareMove");
|
|
|
|
|
@@ -254,7 +275,19 @@ void BoatModule::subscribe()
|
|
|
subOp = opNode->addChild("SetBoatZAxisMove");
|
|
|
subOp->addExec([]()->OperatorStatusEnum {
|
|
|
OP->current->setTimes( 0, 0, 0, boatModule->boatElevator->scMotionTimeout);
|
|
|
- boatModule->setBoatZAxisMove(SERVO_MOVE_POSITION(3));//HOME
|
|
|
+ double defaultSpeed = boatModule->boatElevator->scMoveSpeed;
|
|
|
+ double speed = 0;
|
|
|
+ if (OP->currentRoot->params.length > 1) {
|
|
|
+ //TODO:根据参数直接解析出速度
|
|
|
+ speed = atof(OP->currentRoot->params.get(1));
|
|
|
+
|
|
|
+ }
|
|
|
+ if (speed < 0.001) {
|
|
|
+ speed = defaultSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ boatModule->setBoatZAxisMove(SERVO_MOVE_POSITION(3),speed);//HOME
|
|
|
return OperatorStatusEnum::RUNNING;
|
|
|
});
|
|
|
subOp->addCheck([]()->OperatorStatusEnum {
|
|
@@ -318,6 +351,11 @@ void BoatModule::subscribe()
|
|
|
#pragma region boatrotate
|
|
|
sprintf(cmd, "%s.SetBoatMotion.boatrotate", MODULE_NAME);
|
|
|
opNode = OP->subscribe(cmd);
|
|
|
+ opNode->addReset([]()->OperatorStatusEnum {
|
|
|
+ //异常之后boat由上位机手工操作进行复位,这里直接设置成IDLE保证recipe能够继续执行
|
|
|
+ boatModule->status = BoatStatusEnum::Idle;
|
|
|
+ return OperatorStatusEnum::SUCCESS;
|
|
|
+ });
|
|
|
//SetBoatRAxisMove
|
|
|
subOp = opNode->addChild("SetBoatRAxisMove");
|
|
|
subOp->addExec([]()->OperatorStatusEnum {
|
|
@@ -357,6 +395,11 @@ void BoatModule::subscribe()
|
|
|
#pragma region boatstoprotate
|
|
|
sprintf(cmd, "%s.SetBoatMotion.boatstoprotate", MODULE_NAME);
|
|
|
opNode = OP->subscribe(cmd);
|
|
|
+ opNode->addReset([]()->OperatorStatusEnum {
|
|
|
+ //异常之后boat由上位机手工操作进行复位,这里直接设置成IDLE保证recipe能够继续执行
|
|
|
+ boatModule->status = BoatStatusEnum::Idle;
|
|
|
+ return OperatorStatusEnum::SUCCESS;
|
|
|
+ });
|
|
|
//SetBoatRAxisMove
|
|
|
subOp = opNode->addChild("SetBoatRAxisMoveStop");
|
|
|
subOp->addExec([]()->OperatorStatusEnum {
|
|
@@ -388,6 +431,11 @@ void BoatModule::subscribe()
|
|
|
#pragma region stop(include-axis)
|
|
|
sprintf(cmd, "%s.SetBoatMotion.stop(include-axis)", MODULE_NAME);
|
|
|
opNode = OP->subscribe(cmd);
|
|
|
+ opNode->addReset([]()->OperatorStatusEnum {
|
|
|
+ //异常之后boat由上位机手工操作进行复位,这里直接设置成IDLE保证recipe能够继续执行
|
|
|
+ boatModule->status = BoatStatusEnum::Idle;
|
|
|
+ return OperatorStatusEnum::SUCCESS;
|
|
|
+ });
|
|
|
//SetBoatRAxisMoveStop
|
|
|
subOp = opNode->addChild("SetBoatRAxisMoveStop");
|
|
|
subOp->addExec([]()->OperatorStatusEnum {
|
|
@@ -490,10 +538,10 @@ PMCBOOL BoatModule::autoShutterOpen(PMCBOOL shutterOpen)
|
|
|
return PMCBOOL();
|
|
|
}
|
|
|
|
|
|
-PMCBOOL BoatModule::setBoatZAxisMove(int position)
|
|
|
+PMCBOOL BoatModule::setBoatZAxisMove(int position,double speed)
|
|
|
{
|
|
|
boatModule->boatElevator->aoTargetPosition.setFloatValue(position);
|
|
|
- boatModule->boatElevator->aoTargetSpeed.setDoubleValue(boatModule->boatElevator->scMoveSpeed);
|
|
|
+ boatModule->boatElevator->aoTargetSpeed.setDoubleValue(speed);
|
|
|
boatModule->boatElevator->doHome.setBoolValue(FALSE);
|
|
|
boatModule->boatElevator->doInit.setBoolValue(FALSE);
|
|
|
boatModule->boatElevator->doMove.setBoolValue(FALSE);
|