Pick and place using vision for part locate and force feedback for stacking

I recently worked on a demo for a client who would use the Robotiq Wrist Camera to locate the part for a pick and place application (See the video below). We wanted the robot to automatically find the height of the pile where the object was to be stacked. We did this using the FT300 sensor. So I first made the attached stackingWithForce.urp program which contains only the vision locate part of the program. I used a variable called obj_found which is initially set to zero and set to 1 when the camera locates an object. I can then do some logic such that the robot just keeps on taking snapshots from the snapshot position until it finds a part or the operator puts parts back in the field of view of the camera. The rest is just simple move commands and gripper commands.

Once I was done with this program, I had to program the stacking using forces. Usually to make life easier I start from the force template demo that are included in the Robotiq software package for FT300 that can be downloaded on support.robotiq.com. But now I was stuck because I did not want to erase the program that I just made. So I actually saved my actual program and opened the demo_force_push_release.urp template and started from there. This demo moves the robot in Z direction until it senses a certain amount of force set by the variable Fmax. To be able to do what I wanted to do, I needed to do some modifications to this demo. First, Fmax is initially set to 30N, which was too high for what I needed so I changed that to 5N. Then as the first line under Robot Program I called a subprogram that would be the stackingWithForce program that I made earlier. Before doing so I went back to this program and made sure that the last point I wanted to go to in the program (Waypoint_4) was the point above the pile that I wanted to stack, but this only if I found an object (obj_found=1). So after that subprogram call, I know that if I was able to locate a puck I would be standing with the puck above the pile that I want to stack on. I then zeroed the sensor because I am now holding the puck which can have some weight difference between each of them. Then I added an If statement to perform the force push part of the program only if a puck was found and picked. Once this push process is done I simply open the gripper, move back above the stack and start over.

Note that I also made some changes to make the stack detection faster in the push_z_axis thread. I increased the position increment to .5 and increased the speed of the movel command to 0.05. Since the loop is continuously checking the force condition this works perfect. I first tried to keep small increments and increased the speed but this led to the robot vibrating a lot between increments!!!