def takeNscans(n, t):
if (n <= 0 or t <= 0):
print(
"Invalid scan number or scan duration provided. See takeNscans function."
)
sys.exit()
# Create list for distances to be measured
distances = []
# do a scan of n measurements towards left
for x in range(n):
ct.init()
ct.pivotLeft(t)
time.sleep(0.5)
dist = myround(int(distance() + 0.5))
distances.append(dist)
return distances
def scan(turnDur, turns):
distances = np.zeros([turns, turnDur])
for y in range(0, turns, 1):
distanceZ = []
for z in range(turnDur):
ct.init()
ct.pivotLeft(0.075)
time.sleep(0.5)
dist = round(distance(), 2)
distanceZ.append(dist)
distances[y] = distanceZ
print("Round " + str(y + 1) + " of " + str(turns) + " done.")
# mean over all turns
distances = np.mean(distances, axis=0)
return distances
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, 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()
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