def drive_condition(
lmotor,
rmotor,
testFunction,
state=True
): # Drives while "testFunction" returns "state" | an example would be: x.drive_condition(50, 50, x.getWait)
print "driving under condition"
_clear_ticks()
if lmotor == 0 or rmotor == 0:
print "please use pivot instead!"
elif abs(rmotor) <= abs(lmotor):
mod = rmotor / (lmotor * 1.0)
newLeftSpeed = lmotor
newRightSpeed = int(mod * lmotor)
elif abs(lmotor) < abs(rmotor):
mod = (lmotor * 1.0) / rmotor
newLeftSpeed = int(mod * rmotor)
newRightSpeed = rmotor
while testFunction() is state:
if int(_right_ticks() / mod) == int(_left_ticks() / mod):
_drive(newLeftSpeed, newRightSpeed)
if int(_right_ticks() / mod) > int(_left_ticks() / mod):
_drive(newLeftSpeed, int(newRightSpeed / 1.3))
if int(_left_ticks() / mod) > int(_right_ticks() / mod):
_drive(int(newLeftSpeed / 1.3), newRightSpeed)
freeze_motors()
print get_motor_position_counter(RMOTOR)
def arc_radius(angle, turnRadius, speed): # Turns the robot "angle" degrees by arcing about "turnRadius".
smallCircRadius = turnRadius - (WHEEL_DISTANCE / 2)
largeCircRadius = turnRadius + (WHEEL_DISTANCE / 2)
smallCircum = pi * 2 * smallCircRadius
largeCircum = pi * 2 * largeCircRadius
smallCircSeg = (angle / 360.0) * smallCircum
largeCircSeg = (angle / 360.0) * largeCircum
if turnRadius < 0: msleep(300)
pivot_right(3, -5)
speed = -speed
_clear_ticks()
smallTicks = abs(INCHES_TO_TICKS * smallCircSeg)
largeTicks = abs(INCHES_TO_TICKS * largeCircSeg)
if angle > 0:
smallSpeed = int(speed * (smallTicks / largeTicks))
largeSpeed = int(speed)
print smallTicks
print largeTicks
print smallTicks / largeTicks
print smallSpeed
print largeSpeed
while _right_ticks() <= largeTicks:
if (_right_ticks() / largeTicks) == (_left_ticks() / smallTicks):
_drive(smallSpeed, largeSpeed)
if (_right_ticks() / largeTicks) > (_left_ticks() / smallTicks):
_drive(smallSpeed, int(largeSpeed / 1.3))
if (_left_ticks() / smallTicks) > (_right_ticks() / largeTicks):
_drive(int(smallSpeed / 1.3), largeSpeed)
freeze_motors()
print smallTicks
print largeTicks
print get_motor_position_counter(RMOTOR)
def rotate_spinner(rotations, speed):
full_rotation = 1400.0
start = get_motor_position_counter(SPINNER)
motor_power(SPINNER, speed)
tries_remaining = 3
previous = 0
counter = 0
while abs(get_motor_position_counter(SPINNER) - start) < abs(
full_rotation * rotations) and tries_remaining > 0:
if counter >= 10:
counter = 0
if tries_remaining > 0:
motor_power(SPINNER, int(-speed))
msleep(300)
motor_power(SPINNER, speed)
tries_remaining -= 1
elif abs(get_motor_position_counter(SPINNER)) == previous:
counter += 1
else:
counter = 0
previous = abs(get_motor_position_counter(SPINNER))
msleep(10)
print "rotated {} out of {}".format(
get_motor_position_counter(SPINNER) - start,
abs(full_rotation * rotations))
freeze(SPINNER)
def rotate_until_stalled(speed):
counter = 0
motor_power(SPINNER, speed)
previous = abs(get_motor_position_counter(SPINNER))
while counter < 10:
if abs(get_motor_position_counter(SPINNER)) == previous:
counter += 1
else:
counter = 0
previous = abs(get_motor_position_counter(SPINNER))
msleep(10)
freeze(SPINNER)
clear_motor_position_counter(SPINNER)
def rotate_compass(deg, speed): # Rotates by using both wheels equally.
if deg < 0:
speed = -speed
deg = -deg
angle = deg / 360.0
circ = pi * WHEEL_DISTANCE
inches = angle * circ
print circ
print inches
ticks = int(INCHES_TO_TICKS * inches)
_clear_ticks()
_drive(-speed, speed)
for _ in range(0, 10):
magneto_x()
magneto_y()
msleep(10)
mx = magneto_x()
my = magneto_y()
while abs(mx) > 100 or abs(my) > 100:
mx = magneto_x()
my = magneto_y()
data = {"min_x": mx, "min_y": my, "max_x": mx, "max_y": my}
while _right_ticks() <= ticks:
mag_x = magneto_x()
mag_y = magneto_y()
print(str(mag_x) + "\t" + str(mag_y))
if mag_x > data["max_x"] and abs(
mag_x -
data["max_x"]) < 5: # and abs(mag_x - data["max_x"])< 10
data["max_x"] = mag_x
# print("new max")
if mag_y > data["max_y"] and abs(mag_y - data["max_y"]) < 5:
data["max_y"] = mag_y
print("new max y " + str(mag_y))
if mag_x < data["min_x"] and abs(mag_x - data["min_x"]) < 5:
data["min_x"] = mag_x
if mag_y < data["min_y"] and abs(mag_y - data["min_y"]) < 5:
data["min_y"] = mag_y
freeze_motors()
print get_motor_position_counter(RMOTOR)
return data
def drive_timed(lmotor, rmotor, time):
print("driving timed")
_clear_ticks()
end = seconds() + time
if lmotor == 0 or rmotor == 0:
print("please use pivot instead!")
elif abs(rmotor) <= abs(lmotor):
mod = rmotor / (lmotor * 1.0)
newLeftSpeed = lmotor
newRightSpeed = int(mod * lmotor)
elif abs(lmotor) < abs(rmotor):
mod = (lmotor * 1.0) / rmotor
newLeftSpeed = int(mod * rmotor)
newRightSpeed = rmotor
while seconds() <= end:
if int(_right_ticks() / mod) == int(_left_ticks() / mod):
_drive(newLeftSpeed, newRightSpeed)
if int(_right_ticks() / mod) > int(_left_ticks() / mod):
_drive(newLeftSpeed, int(newRightSpeed / 1.3))
if int(_left_ticks() / mod) > int(_right_ticks() / mod):
_drive(int(newLeftSpeed / 1.3), newRightSpeed)
freeze_motors()
print(get_motor_position_counter(RMOTOR))
ao()
def rotate(deg, speed): # Rotates by using both wheels equally.
if deg < 0:
speed = -speed
deg = -deg
angle = deg / 360.0
circ = pi * WHEEL_DISTANCE
inches = angle * circ
print circ
print inches
ticks = int(INCHES_TO_TICKS * inches)
_clear_ticks()
_drive(-speed, speed)
while _right_ticks() <= ticks:
pass
freeze_motors()
print get_motor_position_counter(RMOTOR)
def drive_speed(inches, speed): # Drives an exact distance in inches.
print "driving exact distance"
if inches < 0:
speed = -speed
_clear_ticks()
ticks = abs(INCHES_TO_TICKS * inches)
while _right_ticks() <= ticks:
if _right_ticks() == _left_ticks():
_drive(speed, speed)
if _right_ticks() > _left_ticks():
_drive(speed, int(speed / 1.3))
if _left_ticks() > _right_ticks():
_drive(int(speed / 1.3), speed)
freeze_motors()
print ticks
print get_motor_position_counter(RMOTOR)
def drive_condition(lmotor, rmotor, testFunction, state=True):
print "driving under condition"
_clear_ticks()
if lmotor == 0 or rmotor == 0:
print "this won't work! please use pivot_right_condition or pivot_left_condition instead!"
exit(0)
elif abs(rmotor) <= abs(lmotor):
mod = rmotor / (lmotor * 1.0)
newLeftSpeed = lmotor
newRightSpeed = int(mod * lmotor)
elif abs(lmotor) < abs(rmotor):
mod = (lmotor * 1.0) / rmotor
newLeftSpeed = int(mod * rmotor)
newRightSpeed = rmotor
while testFunction() is state:
if int(_right_ticks() / mod) == int(_left_ticks() / mod):
_drive(newLeftSpeed, newRightSpeed)
if int(_right_ticks() / mod) > int(_left_ticks() / mod):
_drive(newLeftSpeed, int(newRightSpeed / 1.3))
if int(_left_ticks() / mod) > int(_right_ticks() / mod):
_drive(int(newLeftSpeed / 1.3), newRightSpeed)
freeze_motors()
print get_motor_position_counter(RMOTOR)
def drive_speed(inches, speed): # Drives an exact distance in inches.
print("driving exact distance")
global lAdjust
if inches < 0:
speed = -speed
if speed < 0:
lAdjust = lAdjustBack
else:
lAdjust = lAdjustForward
_clear_ticks()
ticks = abs(INCHES_TO_TICKS * inches)
while _right_ticks() <= ticks:
if _right_ticks() == _left_ticks():
_drive(speed, speed)
if _right_ticks() > _left_ticks():
_drive(speed, int(speed / 1.3))
if _left_ticks() > _right_ticks():
_drive(int(speed / 1.3), speed)
freeze_motors()
print(ticks)
print(get_motor_position_counter(RMOTOR))
def _left_ticks(): # Returns the left motor's tick count.
return abs(get_motor_position_counter(LMOTOR) * lAdjust)
def _right_ticks(): # Returns the right motor's tick count.
return abs(get_motor_position_counter(RMOTOR))
def rotate_to_safe(power):
full_rotation = 1400.0
motor_power(SPINNER, power)
while abs(get_motor_position_counter(SPINNER)) % full_rotation > 50:
pass
freeze(SPINNER)
def wait_for_someone_to_rotate():
display("please spin me back")
clear_motor_position_counter(SPINNER)
while abs(get_motor_position_counter(SPINNER)) < 350:
pass
display("good job")
newRightSpeed = int(mod * lmotor)
elif abs(lmotor) < abs(rmotor):
mod = (lmotor * 1.0) / rmotor
newLeftSpeed = int(mod * rmotor)
newRightSpeed = rmotor
while seconds() <= end:
if int(_right_ticks() / mod) == int(_left_ticks() / mod):
_drive(newLeftSpeed, newRightSpeed)
if int(_right_ticks() / mod) > int(_left_ticks() / mod):
_drive(newLeftSpeed, int(newRightSpeed / 1.3))
if int(_left_ticks() / mod) > int(_right_ticks() / mod):
_drive(int(newLeftSpeed / 1.3), newRightSpeed)
freeze_motors()
print get_motor_position_counter(RMOTOR)
def drive_condition(lmotor, rmotor, testFunction, state=True):
# Drives while "testFunction" returns "state" | an example would be: x.drive_condition(50, 50, x.getWait)
print "driving under condition"
_clear_ticks()
if lmotor == 0 or rmotor == 0:
print "this won't work! please use pivot_right_condition or pivot_left_condition instead!"
exit(0)
elif abs(rmotor) <= abs(lmotor):
mod = rmotor / (lmotor * 1.0)
newLeftSpeed = lmotor
newRightSpeed = int(mod * lmotor)
elif abs(lmotor) < abs(rmotor):
