有大神做过机械手运动控制系统设计吗，求其单片机代码，急，如图

开始控制之前机械手的运动部件处于运动范围内的随机位置上，开始控制后，运动部件向任意方向移动。当位置传感器检测到运动部件到达运动范围的极限位置时，运动部件停止 1 秒，然后反向移动，在运动范围的中点停下，该位置作为机械手的初始位置。
机械手的运动部件到达初始位置后停 2 秒，以初始位置为中心，在±5mm 的位置之间移动，移动到±15 mm时停 1 秒。
求写代码！