Python:
def drivepid(direction, speed, distance):
# convert distance to rotations
truedistance = distance // 5.5
# check if speed is greater than 95
if speed >= 96:
# change speed to allow corrections
truespeed = speed - 5
else:
# set truespeed variable to speed input if it is not greater than 95
truespeed = speed
# get the starting point of the inertial sensor
startrotation = inertial20.orientation(OrientationType.ROLL, DEGREES)
# define positive error to activate correction
errorpositive = startrotation + 5
# define negative error to activate correction
errornegative = startrotation - 5
# set the velocity of the left side
leftmotors.set_velocity(truespeed, PERCENT)
# set the velocity of the right side
rightmotors.set_velocity(truespeed, PERCENT)
# spin the left side
leftmotors.spin_for(FORWARD, truedistance, TURNS, wait = False)
# spin the right side
rightmotors.spin_for(FORWARD, truedistance, TURNS, wait = False)
# loop while left side is spinning
while leftmotors.is_spinning():
# compare inertial sensor position to allowable error
if inertial20.orientation(OrientationType.ROLL, DEGREES) >= errorpositive:
# loop while inertial sensor position is not within allowable error
while inertial20.orientation(OrientationType.ROLL, DEGREES) >= errorpositive:
# add 0.25% to the velocity of the right side every loop
rightmotors.set_velocity((rightmotors.velocity(PERCENT) + 0.25), PERCENT)
# remove 0.25% from the velocity of the left side to prevent constant speeding up
leftmotors.set_velocity((leftmotors.velocity(PERCENT) - 0.25), PERCENT)
# wait to allow time for correction
wait(0.25, SECONDS)
# compare inertial sensor position to allowable error
elif inertial20.orientation(OrientationType.ROLL, DEGREES) >= errornegative:
# loop while inertial sensor position is not within allowable error
while inertial20.orientation(OrientationType.ROLL, DEGREES) >= errornegative:
# add 0.25% to the velocity of the left side every loop
leftmotors.set_velocity((leftmotors.velocity(PERCENT) + 0.25), PERCENT)
# remove 0.25% from the velocity of the right side to prevent constant speeding up
rightmotors.set_velocity((rightmotors.velocity(PERCENT) - 0.25), PERCENT)
# wait to allow time for correction
wait(0.25, SECONDS)