def straight_backwards(cm):
global right_start
# Function for going forwards one time
rot = (cm / CM_PER_ROT) * -1
dist = rot * 360
offset = m1.degrees
accelerate(False, right_start)
while m1.degrees < (offset - dist - 180):
if command_handler(bob.commands):
reset_motors()
return
time.sleep(0.01)
# decelerate according to a fixed 1/2 rotation displacement as the end is approached
while m1.degrees < (offset - dist):
if command_handler(bob.commands):
reset_motors()
return
if right_start:
m1.on(25 - ((1 - abs(m1.degrees - offset + dist) / 180) * 20))
m2.on(25 - ((1 - abs(m1.degrees - offset + dist) / 180) * 20))
else:
m2.on(25 - ((1 - abs(m1.degrees - offset + dist) / 180) * 20))
m1.on(25 - ((1 - abs(m1.degrees - offset + dist) / 180) * 20))
time.sleep(0.01)
#print("m1:{} m2:{}".format(m1.degrees, m2.degrees))
if right_start:
m1.off()
m2.off()
else:
m2.off()
m1.off()
if right_start:
right_start = False
else:
right_start = True
def turn(degrees):
global g_off
if g_off == 0:
g_off = g.angle
else:
g_off += degrees
time.sleep(0.01)
if degrees > 0:
m1.on(-10)
m2.on(10)
while g.angle - g_off < degrees - 30:
if command_handler(bob.commands):
reset_motors()
return
f.write("offset: {} angle: {} delta: {}\n".format(
g_off, g.angle, g.angle - g_off))
m1.on(-10)
m2.on(10)
while g.angle - g_off < degrees + 8:
if command_handler(bob.commands):
reset_motors()
return
f.write("offset: {} angle: {} delta: {}\n".format(
g_off, g.angle, g.angle - g_off))
m1.on(-10 + (5 * (g.angle - g_off) / degrees))
m2.on(10 - (5 * (g.angle - g_off) / degrees))
else:
m1.on(10)
m2.on(-10)
while g.angle - g_off > degrees + 30:
if command_handler(bob.commands):
reset_motors()
return
f.write("offset: {} angle: {} delta: {}\n".format(
g_off, g.angle, g.angle - g_off))
m1.on(15)
m2.on(-15)
while g.angle - g_off > degrees - 8:
if command_handler(bob.commands):
reset_motors()
return
f.write("offset: {} angle: {} delta: {}\n".format(
g_off, g.angle, g.angle - g_off))
m1.on((10 - (5 * (g.angle - g_off) / degrees)))
m2.on((-10 + (5 * (g.angle - g_off) / degrees)))
m1.off()
m2.off()
g_off = 0
time.sleep(1)
def open_claw():
m4.on(-40)
while m4.degrees > -1000:
if command_handler(bob.commands):
reset_motors()
return
time.sleep(0.005)
m4.off()
def close_claw():
m4.on(40)
while m4.degrees < 0:
if command_handler(bob.commands):
reset_motors()
return
time.sleep(0.005)
m4.off()
def accelerate(forwards, r):
if forwards:
for i in range(0, 25):
if command_handler(bob.commands):
return
if r:
m1.on(-25 * i / 25)
m2.on(-25 * i / 25)
else:
m2.on(-25 * i / 25)
m1.on(-25 * i / 25)
time.sleep(0.005)
else:
for i in range(0, 25):
if command_handler(bob.commands):
reset_motors()
return
m1.on(25 * i / 25)
m2.on(25 * i / 25)
time.sleep(0.005)
def dance():
global bob
while True:
m1.on(100)
m2.on(-100)
if command_handler(bob.commands):
reset_motors()
return
time.sleep(0.5)
m1.on(-100)
m2.on(100)
time.sleep(0.5)