AGV调试安全防护——超叉保护

AGV调试安全防护

一、超叉保护

1.1背景

增量式电机，断电后位置不保存，所以此处增加SICK拉线编码器作为实时高度获取，为防止线断或被拉扯，在用电机获取的位置值作为校验。

1.2构想思路

1）上电时记录拉线编码器数值H_Last;电机高度（转换过）h_Last;
2)举升到最高处时，得出此时H_Difference=H_Current-H_Last;
h_Difference=h_Current-h_Last;
3)比较拉线差值与电机高度差值，fabs(H_Difference-h_Difference)>10mm;就报超叉报警，否则正常

1.3具体代码实现

1.4加入该判断的位置

举升任务->自动->轴准备动作（上线后均在）->step=4站台前一点取放货物（放货举升最高处时进行判断）

1.5具体代码实现

if(!height_record)
{
    height_motor_last = lift_motor_curdis;
	height_encoder_last = lift_CurrentDistance;
	height_record = 1;
}
if(TrapezoidalVelocity_Control(WholeLiftMotor,robot_param.lift_motor.max_distance,&lift_speed,0.01f)){
					float add_motor = fabs(lift_motor_curdis - height_motor_last);
					float add_encoder = fabs(lift_CurrentDistance -     height_encoder_last);
					if(fabs(add_motor - add_encoder) < 10.0f){
						height_record = 0;
						step = 0;
						return 1;
					}else{
						if(Sys_AxisErrorGet(EA_MotorEncoderUnequal) == false){ //举升电机与编码器距离不匹配
							Sys_AxisErrorSet(EA_MotorEncoderUnequal,true);
							Log_Error("axis,height unequal,cmh,%.4f,cml,%.4f,ceh,%.4f,cel,%.4f",lift_motor_curdis,height_motor_last,lift_CurrentDistance,height_encoder_last);
						}
						*status = Axis_Unnatural;
						Axis_Unnatural_flag=3;
					}
				}