Here is the code:
def pStraight(DISTANCE, HEADING, VELOCITY, KP):
left_drive.set_position(0, DEGREES)
right_drive.set_position(0, DEGREES)
if VELOCITY > 0:
#If driving forward
while left_drive.position(DEGREES) < DISTANCE and right_drive.position(DEGREES) < DISTANCE:
#Calculates amount to adjust
error = HEADING - brain_inertial.rotation(DEGREES)
output = error * KP
#Sets velocity of motors
left_drive.set_velocity(VELOCITY - output, PERCENT)
right_drive.set_velocity(VELOCITY + output, PERCENT)
#Drive
left_drive.spin(FORWARD)
right_drive.spin(FORWARD)
else:
#If driving reverse
while left_drive.position(DEGREES) > DISTANCE and right_drive.position(DEGREES) > DISTANCE:
#Same as before
error = HEADING - brain_inertial.rotation(DEGREES)
output = error * KP
left_drive.set_velocity(VELOCITY - output, PERCENT)
right_drive.set_velocity(VELOCITY + output, PERCENT)
left_drive.spin(FORWARD)
right_drive.spin(FORWARD)
Also, I did use the Caution Tape code (modified a little bit)