Actually I found the problem it was the wrong scale. I had my code measured for meters when I should have used centimeters.