def check_front():
# initialise gpio pins and check first distance
ctr.init()
dist = se.minDistance()
printDist(dist)
# check if distance is below 30cm, if so change direction
if (dist < 30):
#print("Too close, distance: ", dist)
ctr.init()
ctr.backward(1)
ctr.init()
ctr.pivotRight(0.65)
dist = se.minDistance()
printDist(dist)
# check new distance and again, if below 30cm change direction again
if (dist < 30):
#print("Too close, distance: ", dist)
ctr.init()
ctr.backward(1.5)
ctr.init()
ctr.pivotLeft(0.65)
dist = se.minDistance()
printDist(dist)
# check new distance and again, if below 30cm change direction again
if (dist < 30):
#print("Too close, distance: ", dist)
ctr.init()
ctr.backward(2)
ctr.init()
ctr.pivotLeft(2)
dist = se.minDistance()
printDist(dist)
# check new distance and again, if below 30cm give up
if (dist < 30):
print("Too close, giving up with dist: ", dist)
sys.exit()
def key_input(event):
ct.init()
#print 'Key:', event.char
key_press = event.char
sleep_time = 0.040
# Define keys
if key_press.lower() == 'w':
ct.forward(sleep_time)
elif key_press.lower() == 's':
ct.backward(sleep_time)
elif key_press.lower() == 'a':
ct.turnLeft(sleep_time)
elif key_press.lower() == 'd':
ct.turnRight(sleep_time)
elif key_press.lower() == 'q':
ct.pivotLeft(sleep_time)
elif key_press.lower() == 'e':
ct.pivotRight(sleep_time)
else:
time.sleep(sleep_time)
def rotate(angle):
if angle == 0:
return
# Degree conversion
if angle > 360:
angle = angle % 360
if angle < -360:
angle = angle % -360
if angle > 180 and angle < 360:
angle = -(360 - angle)
if angle < -180 and angle < -360:
angle = 360 - angle
# Conversion to unit angles (90°=45u)
uAngle = angle / 2
# Start C script to get MPU6050 running and wait for initialisation
process = subprocess.Popen('../C_GyroReader/mstest')
time.sleep(4)
print(
"------------------------------------------------------------------------"
)
# Path of buffer file
buffDir = "../C_GyroReader/rotationBuff"
# desired rotation (90deg are 45 units)
endRot = uAngle
right = True
shift = -999
if endRot < 0:
endRot = -(endRot)
right = False
# get start rotation value
try:
f = open(buffDir, "rb")
byte = f.read(4)
startRot = struct.unpack('f', byte)
startRot = startRot[0]
f.seek(0, 0)
f.close()
finally:
f.close()
try:
while int(shift) < int(endRot):
# move to the right (increase the rotation)
ctr.init()
if right:
ctr.pivotRight(0.030)
else:
ctr.pivotLeft(0.030)
# read new rotation
f = open(buffDir, "rb")
byte = f.read(4)
newRot = struct.unpack('f', byte)
newRot = newRot[0]
shift = abs(startRot - newRot)
sys.stdout.write("\rTurning = %.2f" % shift)
sys.stdout.flush()
f.seek(0, 0)
f.close()
finally:
f.close()
process.kill()
process.kill()
print
def rotate(angle, process):
# Need to fix 90 degrees
if angle == 180:
angle = 179
# counter drift due to inertia
driftBias = 20
# set sleep_time
sleep_time = 0.03
if angle == 0:
return
# Degree conversion
if angle > 360:
angle = angle % 360
if angle < -360:
angle = angle % -360
if angle > 180 and angle < 360:
angle = -(360 - angle)
if angle < -180 and angle < -360:
angle = 360 - angle
# Conversion to unit angles (90°=45u)
uAngle = angle / 2
# Path of buffer file
buffDir = "../C_GyroReader/rotationBuff"
# desired rotation (90deg are 45 units)
endRot = uAngle
right = True
shift = -999
if endRot < 0:
endRot = -(endRot)
right = False
# get start rotation value
try:
f = open(buffDir, "rb")
byte = f.read(4)
startRot = struct.unpack('f', byte)
startRot = startRot[0]
f.seek(0, 0)
f.close()
finally:
f.close()
# endRot is in [1 to 90] interval
# for the bias: [1 90-bias] interval
endRot = endRot - driftBias
if endRot <= 0:
endRot = 1
try:
while int(shift) < int(endRot):
# move to the right (increase the rotation)
ctr.init()
if right:
ctr.pivotRight(sleep_time)
else:
ctr.pivotLeft(sleep_time)
# read new rotation
f = open(buffDir, "rb")
byte = f.read(4)
newRot = struct.unpack('f', byte)
newRot = newRot[0]
shift = abs(startRot - newRot)
inDeg = shift * 2 + driftBias * 2
sys.stdout.write("\rTurning = %.2f" % inDeg)
sys.stdout.flush()
f.seek(0, 0)
f.close()
finally:
f.close()
try:
f = open(buffDir, "rb")
byte = f.read(4)
startRot = struct.unpack('f',byte)
startRot = startRot[0]
f.seek(0,0)
f.close()
finally:
f.close()
try:
while int(shift) < int(endRot):
# move to the right (increase the rotation)
ctr.init()
ctr.pivotRight(0.030)
# read new rotation
f = open(buffDir, "rb")
byte = f.read(4)
newRot = struct.unpack('f',byte)
newRot = newRot[0]
shift = abs(startRot-newRot)
sys.stdout.write("\rTurning = %.2f" % shift)
sys.stdout.flush()
f.seek(0,0)
f.close()
finally:
f.close()