class Sonar:
def __init__(self, echo_pin, trigger_pin):
self.sensor = DistanceSensor(echo=echo_pin, trigger=trigger_pin)
def get_distance(self):
return self.sensor.distance * 100
def stop(self):
self.sensor.close()
def get_values(self, count, time_span=0.001):
GPIO.setmode(GPIO.BCM)
sensor = DistanceSensor(echo=RPiGPIOPin(self.echo_port),
trigger=RPiGPIOPin(self.trigger_port),
threshold_distance=self.threshold,
max_distance=self.max_distance)
values = np.array(
[self.get_value(sensor, time_span) for _ in range(count)])
sensor.close()
return values
def run(self):
'''
Loop through each document in the lot's collection and
track the changes within the spaces.
'''
availableSpots = 0
for sensor in self.collection.find_one()['sensors']:
sensorClass = DistanceSensor(echo=sensor['echo'],
trigger=sensor['trigger'],
max_distance=0.06,
threshold_distance=0.005)
sensor[
"isVacant"] = False if sensorClass.distance < 0.0254 else True
if sensor["isVacant"] == True:
availableSpots += 1
self.collection.update_one(
{
'lotName': self.lotName,
'sensors._id': sensor['_id']
}, {'$set': {
'sensors.$.isVacant': sensor["isVacant"]
}})
sensorClass.close()
self.collection.update_one(
{'lotName': self.lotName},
{'$set': {
'availableSpots': availableSpots
}})
print('Total available spaces in lot is: {}'.format(availableSpots))
print('{}: {}'.format(
self.collection.find_one()["sensors"][0]["_id"],
self.collection.find_one()["sensors"][0]["isVacant"]))
print('{}: {}'.format(
self.collection.find_one()["sensors"][1]["_id"],
self.collection.find_one()["sensors"][1]["isVacant"]))
print('{}: {}'.format(
self.collection.find_one()["sensors"][2]["_id"],
self.collection.find_one()["sensors"][2]["isVacant"]))
print('{}: {}'.format(
self.collection.find_one()["sensors"][3]["_id"],
self.collection.find_one()["sensors"][3]["isVacant"]))
print('{}: {}'.format(
self.collection.find_one()["sensors"][4]["_id"],
self.collection.find_one()["sensors"][4]["isVacant"]))
print('{}: {}'.format(
self.collection.find_one()["sensors"][5]["_id"],
self.collection.find_one()["sensors"][5]["isVacant"]))
class RoPiDistance:
# initialization
def __init__(self, remote_address):
# set PINs on BOARD
log.debug("Initializing Distance...")
log.debug("> echo 1 pin: " + str(_conf['echo_1_pin']))
log.debug("> trig 1 pin: " + str(_conf['trig_1_pin']))
log.debug("> echo 2 pin: " + str(_conf['echo_2_pin']))
log.debug("> trig 2 pin: " + str(_conf['trig_2_pin']))
# using DistanceSensor
rem_pi = PiGPIOFactory(host=remote_address)
self.distance1 = DistanceSensor(echo=_conf['echo_1_pin'],
trigger=_conf['trig_1_pin'],
pin_factory=rem_pi)
self.distance2 = DistanceSensor(echo=_conf['echo_2_pin'],
trigger=_conf['trig_2_pin'],
pin_factory=rem_pi)
log.debug("...init done!")
# is close
def is_close(self):
distance1 = (self.distance1.distance * 100) < _conf['SAFETY_DISTANCE']
distance2 = (self.distance2.distance * 100) < _conf['SAFETY_DISTANCE']
log.debug("> is close ?" + str(distance1 or distance2))
return distance1 or distance2
# get distance 1
def get_distance1(self):
log.debug("> Distance 1: " + str(self.distance1.distance * 100))
return self.distance1.distance * 100
# get distance 2
def get_distance2(self):
log.debug("> Distance 2: " + str(self.distance2.distance * 100))
return self.distance2.distance * 100
# terminate
def terminate(self):
log.debug("Distance termination...")
self.distance1.close()
self.distance2.close()
def createUS(self, echo, trigger):
'''
Create a sensor for parking lot with echo
and trigger as params.
'''
# Each index in sensorsList will have this object per sensor
info = {'_id': '', 'isVacant': False, 'echo': echo, 'trigger': trigger}
sensor = DistanceSensor(echo=echo,
trigger=trigger,
max_distance=0.05,
threshold_distance=0.005)
# Naming convention is first three chars of lot name and place in sensorsList
info['_id'] = self.lotName[:3] + str(self.countSensors())
print('Sensor %s is initializing' % info['_id'])
self.collection.update_one({'lotName': self.lotName},
{'$push': {
'sensors': info
}})
sensor.close()
def read_distance():
global message
while reading:
message = "Distance: " + '{:1.2f}'.format(sensor.value) + " m"
print(message)
sleep(0.1)
signal(SIGTERM, safe_exit)
signal(SIGHUP, safe_exit)
try:
reader = Thread(target=read_distance, daemon=True)
display = Thread(target=display_distance, daemon=True)
reader.start()
display.start()
pause()
except KeyboardInterrupt:
pass
finally:
reading = False
sleep(0.5)
lcd.clear()
sensor.close()
class RaspberryPi(robots_common.RobotHollow):
def __init__(self):
super().__init__()
self.running = False
self.reload()
self.tts_engine = pyttsx3.init()
self.tts_engine.setProperty('rate', 150)
self.running = True
def reload(self):
super().reload()
self.name = "RaspberryPi"
self.duty = self.config["duty"]
self.block_length = self.config["block_length"]
self.duty_ratio = self.duty_ratio = self.config["raspberrypi"][
"motor"]["duty_ratio"]
self.turn_right_period = 0.005
self.turn_left_period = 0.005
self.turn_right_period = self.config["raspberrypi"]["motor"][
"turn_right_period"]
self.turn_left_period = self.config["raspberrypi"]["motor"][
"turn_left_period"]
self.servo_horizontal = self.config["raspberrypi"]["camera"][
"servo_horizontal"]
self.servo_vertical = self.config["raspberrypi"]["camera"][
"servo_vertical"]
self.grab = True
self.servo_claw = 1
self.servo_claw_angle_open = -60
self.servo_claw_angle_close = 60
self.servo_claw = self.config["raspberrypi"]["claw"]["servo"]
self.servo_claw_angle_open = self.config["raspberrypi"]["claw"][
"angle_open"]
self.servo_claw_angle_close = self.config["raspberrypi"]["claw"][
"angle_close"]
self.servo_horizontal = 2
self.servo_vertical = 3
self.servo_horizontal = self.config["raspberrypi"]["camera"][
"servo_horizontal"]
self.servo_vertical = self.config["raspberrypi"]["camera"][
"servo_vertical"]
self.IN1 = 7
self.IN2 = 8
self.IN3 = 9
self.IN4 = 10
self.IN1 = self.config["raspberrypi"]["motor"]["pinout"]["IN1"]
self.IN2 = self.config["raspberrypi"]["motor"]["pinout"]["IN2"]
self.IN3 = self.config["raspberrypi"]["motor"]["pinout"]["IN3"]
self.IN4 = self.config["raspberrypi"]["motor"]["pinout"]["IN4"]
self.servos = [17, 27, 22]
self.servos = self.config["raspberrypi"]["servos"]
self.trigger = 18
self.echo = 24
self.trigger = self.config["raspberrypi"]["hcsr04"]["trigger"]
self.echo = self.config["raspberrypi"]["hcsr04"]["echo"]
motor_left = (self.IN1, self.IN2)
motor_right = (self.IN3, self.IN4)
self.motors = [
Motor(forward=motor_left[0], backward=motor_left[1]),
Motor(forward=motor_right[0], backward=motor_right[1])
]
self.pi_servos = []
for gpio_servo in self.servos:
self.pi_servos.append(
AngularServo(gpio_servo, min_angle=-90, max_angle=90))
self.hcsr04 = DistanceSensor(echo=self.echo, trigger=self.trigger)
def shutdown(self, quiet=False):
self.print_stdout("shutdown(quiet={})".format(quiet), quiet)
self.running = False
self.hcsr04.close()
for servo in self.pi_servos:
servo.close()
for motor in self.motors:
motor.close()
def sleep(self, seconds=1.0, quiet=False):
self.print_stdout("sleep(seconds={})".format(seconds), quiet)
time.sleep(seconds)
def set_servo(self, i=1, angle=0.0, quiet=False):
self.print_stdout("set_servo(i={}, angle={})".format(i, angle), quiet)
time.sleep(0.2)
if angle > 0 and angle > 90:
angle = 90
if angle < 0 and angle < -90:
angle = -90
self.pi_servos[i - 1].angle = angle
time.sleep(0.2)
def set_motor(self, i=1, duty=0.5, quiet=False):
self.print_stdout("set_motor(i={}, duty={})".format(i, duty), quiet)
motor_index = i - 1
if duty < 0:
duty *= -1
self.motors[motor_index].backward(duty)
else:
self.motors[motor_index].forward(duty)
if -0.01 <= duty <= 0.01:
self.motors[motor_index].stop()
def claw_toggle(self, quiet=False):
if self.grab:
self.claw_close(quiet)
else:
self.claw_open(quiet)
def claw_open(self, quiet=False):
self.print_stdout("claw_open()", quiet)
self.grab = True
self.set_servo(self.servo_claw, self.servo_claw_angle_open, quiet=True)
def claw_close(self, quiet=False):
self.print_stdout("claw_close()", quiet)
self.grab = False
self.set_servo(self.servo_claw,
self.servo_claw_angle_close,
quiet=True)
def read_distance(self, quiet=False):
# Read sonar
distance = self.hcsr04.distance * 100
try:
system = self.config.get("measurement_system").lower()
except:
system = "m"
if system == "r" or system == "i" or system == "b":
distance = distance * 0.393701
self.print_stdout("Distance: [{}] inches.".format(str(distance)),
quiet)
else:
self.print_stdout("Distance: [{}] cm.".format(str(distance)),
quiet)
return distance
def move_forward(self, blocks=1, speed=-1, quiet=False):
if speed < 0:
speed = self.duty
self.print_stdout(
"move_forward(blocks={}, speed={})".format(blocks, speed), quiet)
period = blocks * self.block_length / speed
motor_index = 0
for motor in self.motors:
speed = speed * self.duty_ratio[motor_index]
motor.forward(speed)
motor_index += 1
self.sleep(period, quiet=True)
for motor in self.motors:
motor.stop()
def move_backwards(self, blocks=1, speed=-1, quiet=False):
if speed < 0:
speed = self.duty
self.print_stdout(
"move_backwards(blocks={}, speed={})".format(blocks, speed), quiet)
period = blocks * self.block_length / speed
for motor in self.motors:
motor.backward(speed)
self.sleep(period, quiet=True)
for motor in self.motors:
motor.stop()
def turn_right(self, angle=90, quiet=False):
self.print_stdout("turn_right(angle={})".format(angle), quiet)
duty = 0.5 # Use fixed duty cycle for turning
index = 0
for motor in self.motors:
if index % 2 == 0:
# Left motor
motor.forward(duty)
else:
# Right motor
motor.backward(duty)
index += 1
self.sleep(angle * self.turn_right_period, quiet=True)
for motor in self.motors:
motor.stop()
def turn_left(self, angle=90, quiet=False):
self.print_stdout("turn_left(angle={})".format(angle), quiet)
duty = 0.5 # Use fixed duty cycle for turning
index = 0
for motor in self.motors:
if index % 2 == 0:
# Left motor
motor.backward(duty)
else:
# Right motor
motor.forward(duty)
index += 1
self.sleep(angle * self.turn_left_period, quiet=True)
for motor in self.motors:
motor.stop()
def camera_toggle(self, quiet=False):
self.print_stdout("camera_toggle()", quiet)
max = len(self.camera_toggle_positions)
if self.camera_pos >= max:
self.camera_pos = 0
pos = self.camera_toggle_positions[self.camera_pos]
self.sleep(0.2, quiet=True)
self.set_servo(self.servo_horizontal, pos[0], quiet=True)
self.sleep(0.2, quiet=True)
self.set_servo(self.servo_vertical, pos[1], quiet=True)
self.sleep(0.2, quiet=True)
self.camera_pos += 1
def look_angle(self, angle=90, quiet=False):
self.print_stdout("look_angle(angle={})".format(angle), quiet)
self.sleep(0.2, quiet=True)
if angle > 0 and angle > 90:
angle = 90
if angle < 0 and angle < -90:
angle = -90
position = angle * 0.015
self.set_servo(self.servo_horizontal, position, quiet=True)
self.set_servo(self.servo_vertical, 0, quiet=True)
self.sleep(0.2, quiet=True)
def say_yes(self, quiet=False):
self.print_stdout("say_yes()", quiet)
self.look_angle(0, quiet=True)
self.say("Yes!", quiet)
self.set_servo(self.servo_vertical, 60.0, quiet=True)
self.set_servo(self.servo_vertical, -60.0, quiet=True)
self.set_servo(self.servo_vertical, 60.0, quiet=True)
self.look_angle(0, quiet=True)
def say_no(self, quiet=False):
self.print_stdout("say_no()", quiet)
self.look_angle(0, quiet=True)
self.say("No!", quiet)
self.set_servo(self.servo_horizontal, 60.0, quiet=True)
self.set_servo(self.servo_horizontal, -60.0, quiet=True)
self.set_servo(self.servo_horizontal, 60.0, quiet=True)
self.look_angle(0, quiet=True)
def say_welcome(self, quiet=False):
self.print_stdout("say_welcome()", quiet)
self.say_yes(True)
message = "Welcome to BluPants! My name is {} robot. Are you ready for learning Computer Science with me? " \
"Visit blupants.org to get started.".format(self.name)
self.say(message, quiet)
def say(self, message, quiet=False):
self.print_stdout(message, quiet)
if not quiet:
try:
self.tts_engine.say(message)
self.tts_engine.runAndWait()
except:
pass
from gpiozero import DistanceSensor
from time import sleep
dSensor = DistanceSensor(echo=24, trigger=23)
dSensor.max_distance = 2
try:
while True:
print(dSensor.distance)
sleep(.5)
finally:
dSensor.close()
def stop_sensor(d_sensor: DistanceSensor):
logging.info('Sensor Disabled ....')
d_sensor.close()
