It is glad to meet you who take up the robot at here. I had read your article and vi ,i think your problem is at the algorithm of your robot arm. the problem you had encountered is redountant control of robot and there are many method to solve it .for example optimise theory . u should look up some book about robotic control. good luck.
