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()