1) Are your motors wired correctly? If you uncomment the code at lines 401-2 does it behave as you would expect? If so, there's a software problem that needs looking into...If not, and it turns in circles, tip it up on end and note which motor(s) are driving the wrong direction. Reverse the motor wires at the M+ M- terminals of the speed controller.
2) One problem that I see is that what happens if p1_y == 254 (i.e. full forward)? Following your code, dist would equal 1, making dist = 254-127...which is 127. You then use dist in your lookup table, however, you don't have an entry for 127...you when you go full reverse (p1_y == 0).
3) Not sure about this one...It may be an issue with using pwm13-16...Is there a reason that you are using pwm13-16 other than the fact that you're electrically connected there? If not, I would suggest that you relocate that functionality to any unused pwm1-12 that you have. The reasoning is described in
this thread. I can't guarantee that that's your problem there, but it may help.
<EDIT> Looks like Alan responded while I was typing...</EDIT>