用户自定义电机初始化，一般就是举升和旋转电机初始化

int AGVBasicDriverControl::controlUserMotorInit()
{int result=0;KEYSTATE keyStatus;KEYSTATE keyStatusUp, keyStatusDown;float angleDiff, heightDiff;bool initActionFinshFlag=false;if(mLiftManualPlatformControl.size()==0//小车举升平台控制输出&&mRotateManualPlatformControl.size()==0)//小车旋转平台控制输出{result=1;qDebug()<<"没有举升旋转马达，直接退出初始化";return result;}if(mUserMotorInitStep==0) //初始化第0步，将各个控制马达初始化标志置复位{for(int i=0;i<mLiftManualPlatformControl.size();i++){mLiftManualPlatformControl[i].nLiftPlatformInitStep=0;if(mCartLiftMotorParameterList[i].downLimitIoAddr!=0&&mCartLiftMotorParameterList[i].upLimitIoAddr==0)//有下限位,没有上限位{getInputIOStatus(mCartLiftMotorParameterList[i].downLimitIoAddr,keyStatusUp);if(keyStatusUp==KEY_OFF)mLiftManualPlatformControl[i].nLiftPlatformInitStep=1;//跳过顶盘上升5mm阶段,进入顶盘下降寻找原点}mLiftManualPlatformControl[i].mLiftMotorInitFlag=false;mLiftManualPlatformControl[i].mLiftPlatformHeightRef=0;pComManualLiftMotorCurrentHeight[i]=-1;//用于显示举升马达当前高度mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.clear();//平台升降位置记忆序列}for(int i=0;i<mRotateManualPlatformControl.size();i++){mRotateManualPlatformControl[i].nRotationPlatformInitStep=0;mRotateManualPlatformControl[i].mRotationMotorInitFlag=false;mRotateManualPlatformControl[i].mRotationPlatformAngleRef=0;pComManualRotateMotorCurrentAngle[i]=-1;mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.clear();}mUserMotorInitStep=1;qDebug()<<"用户马达初始化跳转到第 1 步";}else if(mUserMotorInitStep==1)//初始化第1步，升降电机归位零点{if(mLiftManualPlatformControl.size()==0){mUserMotorInitStep=2; //跳转到旋转盘升降初始化}initActionFinshFlag=true;for(int i=0;i<mLiftManualPlatformControl.size();i++){if(mCartLiftMotorParameterList[i].downLimitIoAddr==0)//没有指定升降马达IO，不需要初始化{mLiftManualPlatformControl[i].mLiftMotorInitFlag=true;}if(mLiftManualPlatformControl[i].mLiftMotorInitFlag==false){initActionFinshFlag=false;if(mCartLiftMotorParameterList[i].downLimitIoAddr!=0&&mCartLiftMotorParameterList[i].upLimitIoAddr!=0)//有上下限位{getInputIOStatus(mCartLiftMotorParameterList[i].downLimitIoAddr,keyStatusUp);getInputIOStatus(mCartLiftMotorParameterList[i].upLimitIoAddr,keyStatusDown);if(keyStatusUp==KEY_ON&&keyStatusDown==KEY_ON){xAGVAlarmErrorManager::addErrorCode(ERROR_AGV_LIFT_MOTOR_INIT_FAULT,0);qDebug()<<"升降电机上下限位同时感应到信号，请检查，确定限位的常开常闭属性";}}if(mLiftManualPlatformControl[i].nLiftPlatformInitStep==0)//顶盘上升5mm{if(getInputIOStatus(mCartLiftMotorParameterList[i].upLimitIoAddr,keyStatus)==true){
//                        qDebug()<<"mLiftManualPlatformControl[i].mLiftPlatformHeightRef"<<mLiftManualPlatformControl[i].mLiftPlatformHeightRef<<"mCartLiftMotorParif(mLiftManualPlatformControl[i].mLiftPlatformHeightRef>5.0f||keyStatus==KEY_ON){mLiftManualPlatformControl[i].nLiftPlatformInitStep=1;mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.clear();setLiftPlatformMotorSpeed(i,0);qDebug()<<"升降电机初始化第 0步完成，进入第1步";}else{setLiftPlatformMotorSpeed(i,PLATFORM_LIFT_INIT_SPEED_FAST);//上升控制mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.append(mLiftManualPlatformControl[i].mLiftPlatformHeightRef);if(mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.size()>MAX_MOTOR_POS_REM){mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.removeAt(0);if(fabs(mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem[MAX_MOTOR_POS_REM-1]-mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem[0])<PLATFORM_LIFT_INIT_SPEED_FAST/10.0f){xAGVAlarmErrorManager::addErrorCode(ERROR_AGV_LIFT_MOTOR_INIT_FAULT,0);}}}}else{setLiftPlatformMotorSpeed(i,0);qDebug()<<"IO通信主板超时等待中";}}else if(mLiftManualPlatformControl[i].nLiftPlatformInitStep==1)//顶盘下降寻找原点{if(getInputIOStatus(mCartLiftMotorParameterList[i].downLimitIoAddr,keyStatus)==true){if(keyStatus==KEY_ON){mLiftManualPlatformControl[i].nLiftPlatformInitStep=2;mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.clear();setFollowPlatformLiftMotorSpeed(0);qDebug()<<"顶升马达初始化第 1步完成，进入第2步";}else{
//                            qDebug()<<"顶升马达速度输出"<<PLATFORM_LIFT_INIT_SPEED_FAST;setLiftPlatformMotorSpeed(i,PLATFORM_LIFT_INIT_SPEED_FAST*-1);//下降控制mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.append(mLiftManualPlatformControl[i].mLiftPlatformHeightRef);if(mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.size()>MAX_MOTOR_POS_REM){mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.removeAt(0);if(fabs(mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem[MAX_MOTOR_POS_REM-1]-mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem[0])<PLATFORM_LIFT_INIT_SPEED_FAST/10.0f){xAGVAlarmErrorManager::addErrorCode(ERROR_AGV_LIFT_MOTOR_INIT_FAULT,0);}}}}else{setLiftPlatformMotorSpeed(i,0);qDebug()<<"IO通信主板超时等待中";}}else if(mLiftManualPlatformControl[i].nLiftPlatformInitStep==2)//顶盘举升确定顶升原点{if(getInputIOStatus(mCartLiftMotorParameterList[i].downLimitIoAddr,keyStatus)==true){if(keyStatus==KEY_OFF){mLiftManualPlatformControl[i].mFollowPlatformLiftBeginHeightRem=mLiftManualPlatformControl[i].mLiftPlatformHeightRef*-1;if(mLiftManualPlatformControl[i].mFollowPlatformLiftBeginHeightRem>mCartLiftMotorParameterList[i].liftHeight){mLiftManualPlatformControl[i].mFollowPlatformLiftBeginHeightRem=mCartLiftMotorParameterList[i].liftHeight;}if(mLiftManualPlatformControl[i].mFollowPlatformLiftBeginHeightRem<0){mLiftManualPlatformControl[i].mFollowPlatformLiftBeginHeightRem=0;}mLiftManualPlatformControl[i].nLiftMotorPlusNum=0;mLiftManualPlatformControl[i].nLiftPlatformInitStep=3;mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.clear();setLiftPlatformMotorSpeed(i,0);qDebug()<<"举升马达初始化第 2步完成，进入第3步,回到原来高度 "<<mLiftManualPlatformControl[i].mFollowPlatformLiftBeginHeightRem;}else{setLiftPlatformMotorSpeed(i,PLATFORM_LIFT_INIT_SPEED_SLOW);//上升控制mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.append(mLiftManualPlatformControl[i].mLiftPlatformHeightRef);if(mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.size()>MAX_MOTOR_POS_REM){mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem.removeAt(0);if(fabs(mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem[MAX_MOTOR_POS_REM-1]-mLiftManualPlatformControl[i].lstPlatformLiftHeightRefRem[0])<PLATFORM_LIFT_INIT_SPEED_FAST/10.0f){xAGVAlarmErrorManager::addErrorCode(ERROR_AGV_LIFT_MOTOR_INIT_FAULT,0);}}}}else{setLiftPlatformMotorSpeed(i,0);qDebug()<<"IO通信主板超时等待中";}}else if(mLiftManualPlatformControl[i].nLiftPlatformInitStep==3)//顶盘举升到原来起始位置{heightDiff=mLiftManualPlatformControl[i].mFollowPlatformLiftBeginHeightRem-mLiftManualPlatformControl[i].mLiftPlatformHeightRef;if(fabs(heightDiff)<0.5){mLiftManualPlatformControl[i].nLiftPlatformInitStep=4;//跳到转盘初始化步骤setLiftPlatformMotorSpeed(i,0);//上升控制qDebug()<<"举升马达初始化第 3步完成，进入第4步";}else{setLiftPlatformMotorSpeed(i,PLATFORM_LIFT_INIT_SPEED_FAST*sign(heightDiff));//上升控制}}else{mLiftManualPlatformControl[i].mLiftMotorInitFlag=true;qDebug()<<"完成举升马达"<<i<<"初始化所有动作";}}else{setLiftPlatformMotorSpeed(i,0);}}if(initActionFinshFlag==true){mUserMotorInitStep=2;//跳转到旋转马达初始化qDebug()<<"完成举伸马达初始化所有动作，进入旋转马达初始化";}}else if(mUserMotorInitStep==2)//初始化第2步，旋转电机归位零点{if(mRotateManualPlatformControl.size()==0){mUserMotorInitStep=3; //跳转到下一步}initActionFinshFlag=true;for(int i=0;i<mRotateManualPlatformControl.size();i++){if(mCartRotateMotorParameterList[i].rotateRightZeroIoAddr==0)//没有指定旋转马达IO，不需要初始化{mRotateManualPlatformControl[i].mRotationMotorInitFlag=true;}if(mRotateManualPlatformControl[i].mRotationMotorInitFlag==false){initActionFinshFlag=false;if(mRotateManualPlatformControl[i].nRotationPlatformInitStep==0)//顶盘逆时针旋转10度{if(getInputIOStatus(mCartRotateMotorParameterList[i].rotateLeftLimitIoAddr,keyStatus)==true){setRotatePlatformMotorSpeed(i,PLATFORM_ROTATE_INIT_SPEED_FAST);mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.append(mRotateManualPlatformControl[i].mRotationPlatformAngleRef);if(mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.size()>MAX_MOTOR_POS_REM){mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.removeAt(0);if(fabs(mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem[MAX_MOTOR_POS_REM-1]-mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem[0])<PLATFORM_ROTATE_INIT_SPEED_FAST/10.0f){xAGVAlarmErrorManager::addErrorCode(ERROR_AGV_ROTATE_MOTOR_INIT_FAULT,0);}}if(mRotateManualPlatformControl[i].mRotationPlatformAngleRef>10.0f||keyStatus==KEY_ON){mRotateManualPlatformControl[i].nRotationPlatformInitStep=1;mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.clear();setRotatePlatformMotorSpeed(i,0);}}else{qDebug()<<"IO通信主板超时等待中";}}else if(mRotateManualPlatformControl[i].nRotationPlatformInitStep==1)//顶盘顺时针旋转查找原点{if(getInputIOStatus(mCartRotateMotorParameterList[i].rotateRightZeroIoAddr,keyStatus)==true){setRotatePlatformMotorSpeed(i,PLATFORM_ROTATE_INIT_SPEED_FAST*-1);mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.append(mRotateManualPlatformControl[i].mRotationPlatformAngleRef);if(mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.size()>MAX_MOTOR_POS_REM){mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.removeAt(0);if(fabs(mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem[MAX_MOTOR_POS_REM-1]-mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem[0])<PLATFORM_ROTATE_INIT_SPEED_FAST/10.0f){xAGVAlarmErrorManager::addErrorCode(ERROR_AGV_ROTATE_MOTOR_INIT_FAULT,0);}}if(keyStatus==KEY_ON){mRotateManualPlatformControl[i].nRotationPlatformInitStep=2;mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.clear();setRotatePlatformMotorSpeed(i,0);qDebug()<<"旋转马达初始化第 1步完成，进入第2步";}}else{setRotatePlatformMotorSpeed(i,0);qDebug()<<"IO通信主板超时等待中";}}else if(mRotateManualPlatformControl[i].nRotationPlatformInitStep==2)//顶盘逆时针旋转确定原点{if(getInputIOStatus(mCartRotateMotorParameterList[i].rotateRightZeroIoAddr,keyStatus)==true){setRotatePlatformMotorSpeed(i,PLATFORM_ROTATE_INIT_SPEED_SLOW);mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.append(mRotateManualPlatformControl[i].mRotationPlatformAngleRef);if(mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.size()>MAX_MOTOR_POS_REM){mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.removeAt(0);if(fabs(mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem[MAX_MOTOR_POS_REM-1]-mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem[0])<PLATFORM_ROTATE_INIT_SPEED_SLOW/10.0f){xAGVAlarmErrorManager::addErrorCode(ERROR_AGV_ROTATE_MOTOR_INIT_FAULT,0);}}if(keyStatus==KEY_OFF){mRotateManualPlatformControl[i].nRotationPlatformInitStep=3;mRotateManualPlatformControl[i].mFollowPlatformRotationBeginAngleRem=mRotateManualPlatformControl[i].mRotationPlatformAngleRef*-1;mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.clear();mRotateManualPlatformControl[i].nRotationMotorPlusNum=0;setRotatePlatformMotorSpeed(i,0);qDebug()<<"旋转马达初始化第 2步完成，进入第3步";}}else{setRotatePlatformMotorSpeed(i,0);qDebug()<<"IO通信主板超时等待中";}}else if(mRotateManualPlatformControl[i].nRotationPlatformInitStep==3)//顶盘旋转到原来起始位置{angleDiff=fmodToNP180Degree(mRotateManualPlatformControl[i].mFollowPlatformRotationBeginAngleRem-mRotateManualPlatformControl[i].mRotationPlif(fabs(angleDiff)<0.1){setRotatePlatformMotorSpeed(i,0);mRotateManualPlatformControl[i].nRotationPlatformInitStep=6;//跳到顶盘旋转到原来起始位置步骤mRotateManualPlatformControl[i].lstPlatformRotateAngleRefRem.clear();qDebug()<<"顶盘初始化第 5步完成，进入第6步";}else{if(fabs(angleDiff)>5)setRotatePlatformMotorSpeed(i,PLATFORM_ROTATE_INIT_SPEED_FAST*sign(angleDiff));elsesetRotatePlatformMotorSpeed(i,PLATFORM_ROTATE_INIT_SPEED_SLOW*sign(angleDiff));}}else{mRotateManualPlatformControl[i].nRotationPlatformInitStep=4;mRotateManualPlatformControl[i].mRotationMotorInitFlag=true;qDebug()<<"完成旋转马达"<<i<<"初始化所有动作";}}else{setRotatePlatformMotorSpeed(i,0);mRotateManualPlatformControl[i].mRotationMotorInitFlag=true;}}if(initActionFinshFlag==true){mUserMotorInitStep=3;//跳转到旋转马达初始化qDebug()<<"完成旋转马达初始化所有动作，结束用户马达初始化动作";}}else if(mUserMotorInitStep==3)//初始化第3步，升降电机回复原来高度{result=1;}return result;
}