working fastrak-twoArm.cpp