Right after these lines:
Code:
drive_R = (((2000 + p1_y + p1_x - 127) Min 2000 Max 2254) - 2000)
drive_L = (((2000 + p1_y - p1_x + 127) Min 2000 Max 2254) - 2000)
Insert:
Code:
if p4_sw_aux1 = 1 then
drive_R = 254 - drive_R
drive_L = 254 - drive_L
endif