def __init__(self, parent_robot, sensor_map, offset_x, offset_y,
sensor_rot, min_range, max_range, beam_angle):
self.parent_robot = parent_robot
self.sensor = Sonar(sensor_map, min_range, max_range, beam_angle)
self.sensor_offset_x = offset_x
self.sensor_offset_y = offset_y
self.sensor_rotation = sensor_rot
self.sensor_range = 0
self.sensor_x = 0
self.sensor_y = 0
def set_mode_sonar(self, topic, payload):
"""
This method sets the trigger and echo pins for sonar operation.
:param topic: message topic
:param payload: {"command": "set_mode_sonar", "trigger_pin": “PIN”, "tag":”TAG”
"echo_pin": “PIN”"tag":”TAG” }
"""
trigger = payload['trigger']
echo = payload['echo']
self.sonar = Sonar(self.pi, trigger, echo)
self.receive_loop_idle_addition = self.read_sonar
def __init__(self,
threshold=0.1,
buffer_size=5,
max_speed=480,
decel_rate=0.05,
turn_time=1,
reverse_time=1,
turn_dist=1000,
reverse_dist=500,
offset=0.8,
debug=False):
"""
Constructs an Obstacle_Avoidance object using a series of overridable default parameters.
Keyword Arguments:
threshold (float): What threshold value to pass to the Sonar object.
buffer_size (int): What buffer_size value to pass to the Sonar object.
max_speed (int): Refer to the max_speed class attribute.
decel_rate (float): Refer to the decel_rate class attribute.
turn_time (float): Refer to the turn_time class attribute.
reverse_time (float): Refer to the reverse_time class attribute.
turn_dist (int): Refer to the turn_dist class attribute.
reverse_dist (int): Refer to the reverse_dist class attribute.
offset (float): Refer to the offset class attribute.
debug (bool): Flag that controls whether or not to allow remote debugging.
"""
# If debug is enabled, wait for the remote debugger to attach before continuing.
if debug:
ptvsd.enable_attach("rover_senpai")
print("Waiting for debugger to attach...", file=stderr)
ptvsd.wait_for_attach()
print("Debugger attached, continuing...", file=stderr)
dual_mc33926.io_init_motor_drive() # Initializes the motor driver.
self.max_speed = max_speed
self.decel_rate = decel_rate
self.turn_time = turn_time
self.reverse_time = reverse_time
self.turn_dist = turn_dist
self.reverse_dist = reverse_dist
self.offset = offset
self._sonar_data = (
0, b'') # Distance and the sonar indicator (b'L' or b'R').
self._stop_flag = False # Causes the threads to stop when set to true.
self._condition = Condition(
) # Condition object protects _sonar_data and _stop_flag.
self._sonar = Sonar(threshold=threshold, buffer_size=buffer_size)
self._motor = dual_mc33926.MotorDriver()
self._sonar_thread = Thread(target=self._run_sonar, daemon=True)
self._motor_thread = Thread(target=self._run_motor, daemon=True)
def __init__(self, leftMotorPort=None, rightMotorPort=None, leftTouchPort=None, rightTouchPort=None, sonarPort=None): BrickPiSetup() # setup the serial port for communication if(sonarPort is not None): self.sonar = Sonar(sonarPort) if(leftTouchPort is not None): self.leftTouch = Touch(leftTouchPort) if(rightTouchPort is not None): self.rightTouch = Touch(rightTouchPort) if(leftMotorPort is not None and rightMotorPort is not None): self.motors = Motors(leftMotorPort, rightMotorPort) BrickPiSetupSensors() #Send the properties of sensors to BrickPi
def __init__(
self,
num_sonar,
gpio_trigger_list, #-- list of all the trigger pins, starting from left
gpio_echo_list, #-- list of all the echo pins, starting from left
range_min, #- [m]
range_max, #- [m]
angle_min_deg, #- [deg]
angle_max_deg):
self.sonar_array = []
self.pub_array = []
self.num_sonar = num_sonar
delta_angle_deg = (angle_max_deg - angle_min_deg) / float(num_sonar -
1)
rospy.loginfo("Initializing the arrays")
#--- Create an array and expand the object with its angle
for i in range(num_sonar):
sonar = Sonar(gpio_trigger_list[i],
gpio_echo_list[i],
range_min=range_min * 100,
range_max=range_max * 100)
angle_deg = angle_min_deg + delta_angle_deg * i
sonar.angle = math.radians(angle_deg)
self.sonar_array.append(sonar)
rospy.loginfo("Sonar %d set" % i)
#--- Publishers
topic_name = "/dkcar/sonar/%d" % i
pub = rospy.Publisher(topic_name, Range, queue_size=5)
self.pub_array.append(pub)
rospy.loginfo("Publisher %d set with topic %s" % (i, topic_name))
#--- Default message
message = Range()
# message.ULTRASOUND = 1
# message.INFRARED = 0
message.radiation_type = 0
message.min_range = range_min
message.max_range = range_max
self._message = message
def __init__(self):
self._board = Board()
self._speed = 50
self.motor = Motor(self._speed)
self.button = Button(self)
self.leds = Leds()
self.lcd = Lcd7segment()
self.ir = InfraRed(self._board)
self.sonar = Sonar()
print('Robot is ready !')
def __init__(self,
amybot=True,
cambot=False,
fourtronix=False,
straight=False):
Sonar.__init__(self)
# Rainbow.__init__(self)
print("Hello!")
self.last_text = "Bleurgh!"
self.joystick = Joystick()
# if elsing this because the init methods of both classes do stuff with hardware, so best to only intiailise deliberately
if amybot:
self.bot = AmyBot()
LOGGER.info('Enable AmyBot')
elif cambot:
self.bot = CamJamBot()
LOGGER.info('Enable CamJamBot')
elif fourtronix:
self.bot = FourTronix()
LOGGER.info('Enable FourTronixBot')
else:
print("Unknown Robot Type")
sys.exit(0)
self.straight_line_start = False
if straight:
self.mode = R2_BUTTON
self.straight_line_start = True
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr
interval = 0.0
self.modes = {
# L1_BUTTON: self.rainbow,
R1_BUTTON: self.remote,
L2_BUTTON: self.maze,
R2_BUTTON: self.straight
}
super().__init__()
def __init__(self, *args, **kwargs):
batch = kwargs.pop('batch')
robot = kwargs.pop('robot')
sonar_map = kwargs.pop('sonar_map')
offset_x = kwargs.pop('offset_x')
min_range = kwargs.pop('min_range')
max_range = kwargs.pop('max_range')
beam_angle = kwargs.pop('beam_angle')
sonar_group = pyglet.graphics.OrderedGroup(2)
super(PanningDistanceSensor, self).__init__(src.resources.sonar_image,
0, 0, batch, sonar_group)
self.parent_robot = robot
self.sonar_offset_x = offset_x
self.sonar_angle_max = 90
self.sonar_angle_min = -90
self.sonar_angle = 0
self.sonar_angle_target = 0
self.sonar_sensor = Sonar(sonar_map, min_range, max_range, beam_angle)
self.sensor_offset_x = self.width - 8
self.sensor_offset_y = 0
self.sonar_range = 0.0
self.sensor_x = 0
self.sensor_y = 0
class FixedTransformDistanceSensor(object):
def __init__(self, parent_robot, sensor_map, offset_x, offset_y,
sensor_rot, min_range, max_range, beam_angle):
self.parent_robot = parent_robot
self.sensor = Sonar(sensor_map, min_range, max_range, beam_angle)
self.sensor_offset_x = offset_x
self.sensor_offset_y = offset_y
self.sensor_rotation = sensor_rot
self.sensor_range = 0
self.sensor_x = 0
self.sensor_y = 0
def update_sensor(self):
"""Calculates the XY position of the sensor origin based on the current position of the robot and
then takes a reading."""
angle_radians = -math.radians(self.parent_robot.rotation)
beam_angle = angle_radians + self.sensor_rotation
beam_angle = src.util.wrap_angle(beam_angle)
self.sensor_x = self.parent_robot.x + (
self.sensor_offset_x * math.cos(angle_radians) -
(self.sensor_offset_y * math.sin(angle_radians)))
self.sensor_y = self.parent_robot.y + (
self.sensor_offset_x * math.sin(angle_radians) +
(self.sensor_offset_y * math.cos(angle_radians)))
self.sensor_range = self.sensor.update_sonar(self.sensor_x,
self.sensor_y, beam_angle)
def get_distance(self):
"""Returns the last reading taken by this sensor."""
return self.sensor_range
def get_fixed_triggered(self, trigger_distance):
"""Returns true is the last reading is less than or equal to trigger_distance"""
if self.sensor_range < trigger_distance:
return True
else:
return False
def draw_sensor_position(self):
"""Draws a circle at the origin of the sensor"""
src.util.circle(self.sensor_x, self.sensor_y, 5)
class Robot: def __init__(self, leftMotorPort=None, rightMotorPort=None, leftTouchPort=None, rightTouchPort=None, sonarPort=None): BrickPiSetup() # setup the serial port for communication if(sonarPort is not None): self.sonar = Sonar(sonarPort) if(leftTouchPort is not None): self.leftTouch = Touch(leftTouchPort) if(rightTouchPort is not None): self.rightTouch = Touch(rightTouchPort) if(leftMotorPort is not None and rightMotorPort is not None): self.motors = Motors(leftMotorPort, rightMotorPort) BrickPiSetupSensors() #Send the properties of sensors to BrickPi def followWall(self, prefer_wall_distance): """ Follow wall within 'distance' cm """ print "Follow wall within ", prefer_wall_distance, " cm." acc_err = 0 last_err = 0 last_dist = 0 while(True): acc_err, last_err, last_diff_vel = self.followWallStep(prefer_wall_distance, acc_err, last_err, last_dist) def followWallStep(self, prefer_wall_distance, acc_err=0, last_err=0, last_z = 0): # read sonar z = self.sonar.getSmoothSonarDistance(0.05) print "Distance from the wall ", z err = (prefer_wall_distance - z) err = limitTo(err, -10, 10) derror = err - last_err acc_err += err #acc_err = limitTo(acc_err, -10, 10) # HACK: shouldn't need to do this if(err < 0): kp = FOLLOW_WALL_KP_FAR else: kp = FOLLOW_WALL_KP_NEAR I_adjust = limitTo(FOLLOW_WALL_KI*acc_err, -5, 5) diff_vel = kp*err + I_adjust + FOLLOW_WALL_KD * derror print "err : ", err, acc_err if(err*last_err <= 0): acc_err = 0 print kp*err, I_adjust, FOLLOW_WALL_KD * derror print "RERR : ", abs(err) - abs(last_err) ERR_T = 3 #if(abs(last_err) - abs(err) > ERR_T): # diff_vel = 0 # set moving speed new_left_speed = FWD_VEL + FWD_SIGN*int(round(diff_vel)) new_right_speed = FWD_VEL - FWD_SIGN*int(round(diff_vel)) self.motors.setSpeed(new_left_speed, new_right_speed) time.sleep(0.01) last_z = z return (acc_err, err, last_z) def keepDistanceFront(self, distance): """ Keeps the robot at some distance from the wall """ print "Keep distance : ", distance acc_err = 0 last_err = 0 while True: # get sonar measurement for 0.05 seconds z = self.sonar.getSmoothSonarDistance(0.05) # set the speed of the robot err = z - distance acc_err += err derror = err - last_err print "Current distance : ", z speed = KEEP_DISTANCE_FRONT_KP * err + KEEP_DISTANCE_FRONT_KI * acc_err + KEEP_DISTANCE_FRONT_KD * derror print speed, err, acc_err, derror #if(abs(err) > 3 and speed != 0 and abs(speed) < LOWEST_VEL): # direction = speed/abs(speed) # speed = direction*LOWEST_VEL if(err*last_err < 0): acc_err = 0 print "mul", err*last_err self.motors.setSpeed(speed) last_err = err time.sleep(0.01) # def stop(self): # self.motors.stop() # def forward(self, *args, **kwargs): # self.motors.forward(*args, **kwargs) # def turn(self, *args, **kwargs): # self.motors.turn(*args, **kwargs) # def left90deg(self): # self.motors.left90deg() # def right90deg(self): # self.motors.right90deg() # def setSpeed(self, *args, **kwargs): # self.motors.setSpeed(*args, **kwargs) def __getattr__(self, name): """ If a method like stop does not exist, try it on the motors """ def _missing(*args, **kwargs): if self.motors is not None: func = getattr(self.motors, name) func(*args, **kwargs) return _missing
IP = '192.168.2.2'
PORT = 20002
server = Server(IP, PORT)
server.start()
server.receiveConnection()
print('Connection Received')
# ----------------- Initialize Sonar -----------------
if sonars_activated:
s_front = Sonar(6, 18)
s_left = Sonar(5, 17)
s_right = Sonar(12, 27)
#s_backright = Sonar(13, 22)
#s_backleft = Sonar(16, 23)
def drive(turn):
#print(turn)
if turn == 'left':
pass
# ------------------ Initialize IMU ------------------
def run_main_program():
Printer.verbose = Parser.verbose
Printer.debug = Parser.debug
Printer.log_filename = Parser.log_filename
Printer.log_level = Parser.log_level
Printer.log_max_bytes_per_file = Parser.log_max_bytes_per_file
Printer.log_max_number_log_files = Parser.log_max_number_log_files
Printer.log_format = Parser.log_format
Printer.enable_logging()
if Parser.show_examples:
Printer.print_example_usage()
exit(0)
lSonar = Sonar(p_parser=Parser)
if Parser.test_connectivity:
lSonar.test_connectivity()
if Parser.check_quota:
lSonar.check_quota()
if Parser.list_studies:
lSonar.list_studies()
if Parser.list_unparsed_files:
lSonar.list_unparsed_files()
if Parser.update_studies:
lSonar.update_studies()
if Parser.export_data:
lSonar.export_dataset()
def main():
#camera = Camera()
#camera.camera()
#driver.__init__()
#servo.__init__()
sonar = Sonar()
try:
while True:
motion = 0
servo = Servo()
front_center_obs = sonar.sonar(5, 15)
front_right_obs = sonar.sonar(7, 19)
front_left_obs = sonar.sonar(3, 13)
print(front_left_obs, front_center_obs, front_right_obs)
if front_center_obs > 100:
if front_right_obs > 75:
if front_left_obs > 75:
motion = 1
servo.SetAngle(120)
else:
servo = Servo()
servo.SetAngle(135)
motion = 1
else:
if front_left_obs > 75:
servo.SetAngle(105)
motion = 1
else:
back_center_obs = sonar.sonar(18, 26)
back_right_obs = sonar.sonar(16, 24)
back_left_obs = sonar.sonar(38, 40)
print("back", back_left_obs, back_center_obs,
back_right_obs)
if back_center_obs > 75:
if back_right_obs > 75:
if back_left_obs > 75:
servo.SetAngle(105)
motion = 2
else:
servo.SetAngle(135)
motion = 2
else:
if back_left_obs > 75:
servo.SetAngle(105)
motion = 2
else:
motion = 0
else:
motion = 0
else:
servo = Servo()
back_center_obs = sonar.sonar(18, 26)
back_right_obs = sonar.sonar(16, 24)
back_left_obs = sonar.sonar(38, 40)
print("back", back_left_obs, back_center_obs, back_right_obs)
if back_center_obs > 100:
if back_right_obs > 75:
if back_left_obs > 75:
if front_right_obs > 75:
servo.SetAngle(135)
else:
servo.SetAngle(105)
motion = 2
else:
servo.SetAngle(135)
motion = 2
else:
if back_left_obs > 75:
servo.SetAngle(105)
motion = 2
else:
motion = 0
else:
motion = 0
print(motion)
if motion == 1:
driver = Driver()
driver.left()
time.sleep(3)
driver.cleanup()
motion = 0
elif motion == 0:
print("report HQ")
motion = 0
elif motion == 2:
print("entered back command")
driver = Driver()
driver.right()
time.sleep(3)
driver.cleanup()
motion = 0
else:
print("motion undefined")
except KeyboardInterrupt:
driver = Driver()
driver.cleanup()
print("Interrupted")
motion = 0
class Obstacle_Avoidance:
"""
A class for performing obstacle avoidance by running a sonar thread and a motor control thread
in parallel.
Attributes:
max_speed (int): The maximum speed value for the motor driver.
decel_rate (float): How quickly (from 0 to 1) the rover should decelerate.
turn_time (float): How long (in seconds) the rover should turn left or right.
reverse_time (float): How long (in seconds) the rover should reverse.
turn_dist (int): How close an obstacle must be (in mm) before the rover turns.
reverse_dist (int): How close an obstacle must be (in mm) before the rover reverses.
offset (float): Since the wheels aren't straight, one wheel will turn slower.
"""
class Direction(Enum):
"""This enumerates the possible directions the rover can move/turn."""
FORWARD = 0
BACKWARD = 1
RIGHT = 2
LEFT = 3
def __init__(self,
threshold=0.1,
buffer_size=5,
max_speed=480,
decel_rate=0.05,
turn_time=1,
reverse_time=1,
turn_dist=1000,
reverse_dist=500,
offset=0.8,
debug=False):
"""
Constructs an Obstacle_Avoidance object using a series of overridable default parameters.
Keyword Arguments:
threshold (float): What threshold value to pass to the Sonar object.
buffer_size (int): What buffer_size value to pass to the Sonar object.
max_speed (int): Refer to the max_speed class attribute.
decel_rate (float): Refer to the decel_rate class attribute.
turn_time (float): Refer to the turn_time class attribute.
reverse_time (float): Refer to the reverse_time class attribute.
turn_dist (int): Refer to the turn_dist class attribute.
reverse_dist (int): Refer to the reverse_dist class attribute.
offset (float): Refer to the offset class attribute.
debug (bool): Flag that controls whether or not to allow remote debugging.
"""
# If debug is enabled, wait for the remote debugger to attach before continuing.
if debug:
ptvsd.enable_attach("rover_senpai")
print("Waiting for debugger to attach...", file=stderr)
ptvsd.wait_for_attach()
print("Debugger attached, continuing...", file=stderr)
dual_mc33926.io_init_motor_drive() # Initializes the motor driver.
self.max_speed = max_speed
self.decel_rate = decel_rate
self.turn_time = turn_time
self.reverse_time = reverse_time
self.turn_dist = turn_dist
self.reverse_dist = reverse_dist
self.offset = offset
self._sonar_data = (
0, b'') # Distance and the sonar indicator (b'L' or b'R').
self._stop_flag = False # Causes the threads to stop when set to true.
self._condition = Condition(
) # Condition object protects _sonar_data and _stop_flag.
self._sonar = Sonar(threshold=threshold, buffer_size=buffer_size)
self._motor = dual_mc33926.MotorDriver()
self._sonar_thread = Thread(target=self._run_sonar, daemon=True)
self._motor_thread = Thread(target=self._run_motor, daemon=True)
def _run_sonar(self):
"""Continously runs the sonars and notifies the motor thread whenever an object is close."""
while True:
# Gets a measurement from the sonars.
dist, dir = self._sonar.measure()
# If the measured sonar distance is less than our turning threshold...
if dist < self.turn_dist:
self._condition.acquire()
# If the stop flag has been set by stop(), release the lock and break out.
if self._stop_flag:
self._condition.release()
break
# Otherwise, publish the distance and which sonar detected it.
self._sonar_data = (dist, dir)
self._condition.notify_all()
self._condition.release()
def _run_motor(self):
"""Continuously runs the motors and turns away from any obstacles deteced by the sonars."""
self._motor.enable()
def move(direction=Direction.FORWARD, duration=None):
"""
Helper method which simply moves/turns the rover in a given direction.
Keyword Arguments:
direction (Direction): The direction the rover should turn/move.
duration (float): How long the rover should move in this direction.
"""
# First, gradually stop whatever the motors were doing before.
for i in range(1 - self.decel_rate, 0, -self.decel_rate):
self._motor.set_speeds(self._spd1 * i, self._dir1,
self._spd2 * i, self._dir2)
sleep(0.05)
# Determine speed and direction of motors based on the desired direction.
if direction == Direction.FORWARD:
self._spd1 = self.max_speed
self._spd2 = self.max_speed * self.offset
self._dir1 = 1
self._dir2 = 1
elif direction == Direction.BACKWARD:
self._spd1 = self.max_speed
self._spd2 = self.max_speed * self.offset
self._dir1 = 0
self._dir2 = 0
elif direction == Direction.RIGHT:
self._spd1 = 0
self._spd2 = self.max_speed * self.offset
self._dir1 = 1
self._dir2 = 1
else:
self._spd1 = self.max_speed
self._spd2 = 0
self._dir1 = 1
self._dir2 = 1
# Start moving.
self._motor.set_speeds(self._spd1, self._dir1, self._spd2,
self._dir2)
# Wait for a duration (if specified) before accepting another move command.
if (type(duration) in [int, float]) and (duration > 0):
sleep(duration)
# The rover continuously moves forward, reversing and turning away from detected obstacles.
while True:
# Move forward until sonar publishes data about a detected obstacle.
move()
self._condition.acquire()
self._condition.wait()
# If the stop flag has been set by stop(), release the lock and break out.
if self._stop_flag:
self._condition.release()
break
# Otherwise, save the distance from the obstacle (in mm) and which sonar detected it.
dist, dir = self._sonar_data.deepcopy()
self._condition.release()
# If the object is very close, first reverse from it.
if dist < self.reverse_dist:
move(direction=Direction.BACKWARD, duration=self.reverse_time)
# Turn right away from objects detected by the left sonar.
if dir == b'R':
move(direction=Direction.LEFT, duration=self.turn_time)
# Turn left away from objects detected by the right sonar.
elif dir == b'L':
move(direction=Direction.RIGHT, duration=self.turn_time)
# Raise an exception if we see an unrecognized sonar label.
else:
raise ValueError("Invalid sonar label")
# Disable the motors once we break out of the loop.
self._motor.disable()
def start(self):
"""This method starts the obstacle avoidance algorithm."""
if not (self._motor_thread.is_alive()
and self._sonar_thread.is_alive()):
self._stop_flag = False
self._sonar_thread.start()
self._motor_thread.start()
print("Obstacle avoidance is now running.", file=stderr)
else:
print("Obstacle avoidance is already running.", file=stderr)
def stop(self):
"""This method stops the obstacle avoidance algorithm."""
if self._motor_thread.is_alive() and self._sonar_thread.is_alive():
self._condition.acquire()
self._stop_flag = True
self._condition.notify_all()
self._condition.release()
self._sonar_thread.join()
self._motor_thread.join()
print("Obstacle avoidance has stopped.", file=stderr)
else:
print("Obstacle avoidance is not currently running.", file=stderr)
class RpiGateway(GatewayBase):
"""
This class implements a Banyan gateway for the Raspberry Pi
GPIO pins. It implements the Common Unified GPIO Message
Specification.
If pipgiod is not currently running, it will start it, and
no backplane ip address was specified, a local backplane
will be automatically started. If you specify a backplane
ip address, you will need to start that backplane manually.
"""
def __init__(self, *subscriber_list, **kwargs):
"""
Initialize the class for operation
:param subscriber_list: A list of subscription topics
:param kwargs: Command line arguments - see bg4rpi()
at the bottom of this file.
"""
# attempt to instantiate pigpio
self.pi = pigpio.pi()
# if pigpiod is not running, try to start it
#
if not self.pi.connected:
print('\nAttempting to start pigpiod...')
run(['sudo', 'pigpiod'])
time.sleep(.5)
self.pi = pigpio.pi()
if not self.pi.connected:
print('pigpiod did not start - goodbye.')
sys.exit()
else:
print('pigpiod successfully started!')
print('pigpiod is running version: ', self.pi.get_pigpio_version())
# variables to hold instances of sonar and stepper
self.sonar = None
self.stepper = None
# a list of valid pin numbers
# using a list comprehension to create the list
self.gpio_pins = [pin for pin in range(2, 28)]
# i2c handle supplied by pigpio when i2c open is called
self.i2c_handle = None
# initialize the parent
super(RpiGateway, self).__init__(
subscriber_list=subscriber_list,
back_plane_ip_address=kwargs['back_plane_ip_address'],
subscriber_port=kwargs['subscriber_port'],
publisher_port=kwargs['publisher_port'],
process_name=kwargs['process_name'],
)
# start the banyan receive loop
try:
self.receive_loop()
except KeyboardInterrupt:
self.pi.stop()
self.clean_up()
sys.exit(0)
def init_pins_dictionary(self):
"""
This method is called by the gateway_base parent class
when it is initializing.
"""
# build a status table for the pins
for x in self.gpio_pins:
entry = {'mode': None, 'duty': None, 'freq': None, 'value': 0}
self.pins_dictionary[x] = entry
def digital_write(self, topic, payload):
"""
This method performs a digital write
:param topic: message topic
:param payload: {"command": "digital_write", "pin": “PIN”, "value": “VALUE”}
"""
self.pi.write(payload['pin'], payload['value'])
def disable_digital_reporting(self, topic, payload):
"""
This method disables digital input reporting for the selected pin.
:param topic: message topic
:param payload: {"command": "disable_digital_reporting", "pin": “PIN”, "tag": "TAG"}
"""
entry = self.pins_dictionary[payload['pin']]
entry['mode'] = None
def i2c_read(self, topic, payload):
"""
This method will perform an i2c read by specifying the i2c
device address, i2c device register and the number of bytes
to read.
Call set_mode_i2c first to establish the pins for i2c operation.
:param topic: message topic
:param payload: {"command": "i2c_read", "pin": “PIN”, "tag": "TAG",
"addr": “I2C ADDRESS, "register": “I2C REGISTER”,
"number_of_bytes": “NUMBER OF BYTES”}
:return via the i2c_callback method
"""
# if no i2c handle has been assigned, get one
if self.i2c_handle is None:
self.i2c_handle = self.pi.i2c_open(1, payload['addr'], 0)
# using the handle do the i2c read and retrieve the data
value = self.pi.i2c_read_i2c_block_data(self.i2c_handle,
payload['register'],
payload['number_of_bytes'])
# value index 0 is the number of bytes read.
# the remainder of value is the a byte array of data returned
# convert the data to a list
value = list(value[1])
# format the data for report to be published containing
# the data received
report = ', '.join([str(elem) for elem in value])
payload = {'report': 'i2c_data', 'value': report}
self.publish_payload(payload, 'from_rpi_gateway')
def i2c_write(self, topic, payload):
"""
This method will perform an i2c write for the i2c device with
the specified i2c device address, i2c register and a list of byte
to write.
Call set_mode_i2c first to establish the pins for i2c operation.
:param topic: message topic
:param payload: {"command": "i2c_write", "pin": “PIN”, "tag": "TAG",
"addr": “I2C ADDRESS, "register": “I2C REGISTER”,
"data": [“DATA IN LIST FORM”]}
"""
if self.i2c_handle is None:
self.i2c_handle = self.pi.i2c_open(1, payload['addr'], 0)
data = payload['data']
self.pi.i2c_write_device(self.i2c_handle, data)
# give the i2c device some time to process the write request
time.sleep(.4)
def play_tone(self, topic, payload):
"""
This method plays a tone on a piezo device connected to the selected
pin at the frequency and duration requested.
Frequency is in hz and duration in milliseconds.
Call set_mode_tone before using this method.
:param topic: message topic
:param payload: {"command": "play_tone", "pin": “PIN”, "tag": "TAG",
“freq”: ”FREQUENCY”, duration: “DURATION”}
"""
pin = int(payload['pin'])
self.pi.set_mode(pin, pigpio.OUTPUT)
frequency = int(payload['freq'])
frequency = int((1000 / frequency) * 1000)
tone = [
pigpio.pulse(1 << pin, 0, frequency),
pigpio.pulse(0, 1 << pin, frequency)
]
self.pi.wave_add_generic(tone)
wid = self.pi.wave_create()
if wid >= 0:
self.pi.wave_send_repeat(wid)
time.sleep(payload['duration'] / 1000)
self.pi.wave_tx_stop()
self.pi.wave_delete(wid)
def pwm_write(self, topic, payload):
"""
This method sets the pwm value for the selected pin.
Call set_mode_pwm before calling this method.
:param topic: message topic
:param payload: {“command”: “pwm_write”, "pin": “PIN”,
"tag":”TAG”,
“value”: “VALUE”}
"""
self.pi.set_PWM_dutycycle(payload['pin'], payload['value'])
def servo_position(self, topic, payload):
"""
This method will set a servo's position in degrees.
Call set_mode_servo first to activate the pin for
servo operation.
:param topic: message topic
:param payload: {'command': 'servo_position',
"pin": “PIN”,'tag': 'servo',
“position”: “POSITION”}
"""
pin = payload['pin']
pulse_width = (payload['position'] * 10) + 500
self.pi.set_servo_pulsewidth(pin, pulse_width)
def set_mode_analog_input(self, topic, payload):
"""
This method programs a PCF8591 AD/DA for analog input.
:param topic: message topic
:param payload: {"command": "set_mode_analog_input",
"pin": “PIN”, "tag":”TAG” }
"""
# pin is used as channel number
value = None
i2c_handle = self.pi.i2c_open(1, 72, 0)
pin = payload['pin']
self.pi.i2c_write_byte_data(i2c_handle, 64 | (pin & 0x03), 0)
time.sleep(0.1)
for i in range(3):
value = self.pi.i2c_read_byte(i2c_handle)
self.pi.i2c_close(i2c_handle)
# publish an analog input report
payload = {'report': 'analog_input', 'pin': pin, 'value': value}
self.publish_payload(payload, 'from_rpi_gateway')
def set_mode_digital_input(self, topic, payload):
"""
This method sets a pin as digital input.
:param topic: message topic
:param payload: {"command": "set_mode_digital_input", "pin": “PIN”, "tag":”TAG” }
"""
pin = payload['pin']
entry = self.pins_dictionary[pin]
entry['mode'] = self.DIGITAL_INPUT_MODE
self.pi.set_glitch_filter(pin, 20000)
self.pi.set_mode(pin, pigpio.INPUT)
self.pi.set_pull_up_down(pin, pigpio.PUD_DOWN)
self.pi.callback(pin, pigpio.EITHER_EDGE, self.input_callback)
def set_mode_digital_output(self, topic, payload):
"""
This method sets a pin as a digital output pin.
:param topic: message topic
:param payload: {"command": "set_mode_digital_output",
"pin": PIN, "tag":”TAG” }
"""
self.pi.set_mode(payload['pin'], pigpio.OUTPUT)
def set_mode_pwm(self, topic, payload):
"""
This method sets a GPIO pin capable of PWM for PWM operation.
:param topic: message topic
:param payload: {"command": "set_mode_pwm", "pin": “PIN”, "tag":”TAG” }
"""
self.pi.set_mode(payload['pin'], pigpio.OUTPUT)
def set_mode_servo(self, topic, payload):
"""
This method establishes a GPIO pin for servo operation.
:param topic: message topic
:param payload: {"command": "set_mode_servo", "pin": “PIN”, "tag":”TAG” }
"""
pass
def set_mode_sonar(self, topic, payload):
"""
This method sets the trigger and echo pins for sonar operation.
:param topic: message topic
:param payload: {"command": "set_mode_sonar", "trigger_pin": “PIN”, "tag":”TAG”
"echo_pin": “PIN”"tag":”TAG” }
"""
trigger = payload['trigger']
echo = payload['echo']
self.sonar = Sonar(self.pi, trigger, echo)
self.receive_loop_idle_addition = self.read_sonar
def read_sonar(self):
"""
Read the sonar device and convert value to
centimeters. The value is then published as a report.
"""
sonar_time = self.sonar.read()
distance = sonar_time / 29 / 2
distance = round(distance, 2)
payload = {'report': 'sonar_data', 'value': distance}
self.publish_payload(payload, 'from_rpi_gateway')
def set_mode_stepper(self, topic, payload):
"""
This method establishes either 2 or 4 GPIO pins to be used in stepper
motor operation.
:param topic:
:param payload:{"command": "set_mode_stepper", "pins": [“PINS”],
"steps_per_revolution": “NUMBER OF STEPS”}
"""
self.stepper = StepperMotor(self.pi, payload['pins'][0],
payload['pins'][1], payload['pins'][2],
payload['pins'][3])
def stepper_write(self, topic, payload):
"""
Move a stepper motor for the specified number of steps.
:param topic:
:param payload: {"command": "stepper_write", "motor_speed": “SPEED”,
"number_of_steps":”NUMBER OF STEPS” }
"""
if not self.stepper:
raise RuntimeError('Stepper was not initialized')
number_of_steps = payload['number_of_steps']
if number_of_steps >= 0:
for i in range(number_of_steps):
self.stepper.do_clockwise_step()
else:
for i in range(abs(number_of_steps)):
self.stepper.do_counterclockwise_step()
def input_callback(self, pin, level, tick):
"""
This method is called by pigpio when it detects a change for
a digital input pin. A report is published reflecting
the change of pin state for the pin.
:param pin:
:param level:
:param tick:
:return:
"""
# payload = {'report': 'digital_input_change', 'pin': str(pin), 'level': str(level)}
entry = self.pins_dictionary[pin]
if entry['mode'] == self.DIGITAL_INPUT_MODE:
payload = {
'report': 'digital_input',
'pin': pin,
'value': level,
'timestamp': time.time()
}
self.publish_payload(payload, 'from_rpi_gateway')
class PanningDistanceSensor(src.sprites.basicsprite.BasicSprite):
def __init__(self, *args, **kwargs):
batch = kwargs.pop('batch')
robot = kwargs.pop('robot')
sonar_map = kwargs.pop('sonar_map')
offset_x = kwargs.pop('offset_x')
min_range = kwargs.pop('min_range')
max_range = kwargs.pop('max_range')
beam_angle = kwargs.pop('beam_angle')
sonar_group = pyglet.graphics.OrderedGroup(2)
super(PanningDistanceSensor, self).__init__(src.resources.sonar_image,
0, 0, batch, sonar_group)
self.parent_robot = robot
self.sonar_offset_x = offset_x
self.sonar_angle_max = 90
self.sonar_angle_min = -90
self.sonar_angle = 0
self.sonar_angle_target = 0
self.sonar_sensor = Sonar(sonar_map, min_range, max_range, beam_angle)
self.sensor_offset_x = self.width - 8
self.sensor_offset_y = 0
self.sonar_range = 0.0
self.sensor_x = 0
self.sensor_y = 0
def set_target(self, target):
"""Set the target angle for the panning sonar."""
if target >= self.sonar_angle_max:
target = self.sonar_angle_max
elif target <= self.sonar_angle_min:
target = self.sonar_angle_min
self.sonar_angle_target = target
def update_sensor(self):
"""Calculates the XY position of the sensor origin based on the current position of the robot and
then takes a reading."""
angle_radians = -math.radians(self.parent_robot.rotation)
self.sensor_x = self.parent_robot.x + (
self.sensor_offset_x * math.cos(angle_radians) -
(self.sensor_offset_y * math.sin(angle_radians)))
self.sensor_y = self.parent_robot.y + (
self.sensor_offset_x * math.sin(angle_radians) +
(self.sensor_offset_y * math.cos(angle_radians)))
self.x = self.sensor_x
self.y = self.sensor_y
self.sonar_range = self.sonar_sensor.update_sonar(
self.sensor_x, self.sensor_y, angle_radians)
def get_distance(self):
"""Returns the last reading taken by this sensor."""
return self.sonar_range
def update(self, dt):
"""Updates the position of the sprite representing the panning servo head."""
angle_radians = -math.radians(self.parent_robot.rotation)
self.sensor_x = self.parent_robot.x + (self.sonar_offset_x *
math.cos(angle_radians))
self.sensor_y = self.parent_robot.y + (self.sonar_offset_x *
math.sin(angle_radians))
self.x = self.sensor_x
self.y = self.sensor_y
if (self.sonar_angle_target - self.sonar_angle) > 0:
self.sonar_angle += 5
elif (self.sonar_angle_target - self.sonar_angle) < 0:
self.sonar_angle -= 5
self.rotation = self.parent_robot.rotation - self.sonar_angle
self.update_sensor()
def draw_sensor_position(self):
"""Draws a circle at the origin of the sensor."""
src.util.circle(self.sensor_x, self.sensor_y, 5)
CommandHandler('video', capture_video_command))
updater.dispatcher.add_handler(CommandHandler('mode', set_mode_command))
updater.start_polling()
if __name__ == '__main__':
light_controller_thread = threading.Thread(
target=light_controller_driver_thread)
light_controller_thread.start()
bot_thread = threading.Thread(target=bot_driver_thread)
bot_thread.start()
left_sonar = Sonar(LEFT_TRIGGER, ECHO, READING_TIMEOUT_MS)
middle_sonar = Sonar(MIDDLE_TRIGGER, ECHO, READING_TIMEOUT_MS)
right_sonar = Sonar(RIGHT_TRIGGER, ECHO, READING_TIMEOUT_MS)
LED_OUT = []
for pin in LED_PINS:
LED_OUT.append(PWMOutputDevice(pin))
try:
while True:
left_dist = left_sonar.distance()
time.sleep(.100)
middle_dist = middle_sonar.distance()
time.sleep(.100)
right_dist = right_sonar.distance()
time.sleep(.100)
angle = d * 100 # cela permet d'appliquer un pourcentage de rotation
robot.set_speed(SPEED)
return(je tourne de +str(angle) )
else :
# La distance est de moins de 5 cm
angle = 0
robot.set_speed(-SPEED)
return(je recule)
while 1: # Boucle infinie
try:
sonar1 = Sonar(23, 24)
sonar2 = Sonar(22,10)
sonar3 = Sonar(9,11)
sonar4 = Sonar(8,7)
infra1 = Infra(2)
infra2 = Infra(3)
distance1 = sonar1.avg_mesure()
distance2 = sonar2.avg_mesure()
distance3 = sonar3.avg_mesure()
distance4 = sonar4.avg_mesure()
print(distance1,distance2,distance3,distance4)
if infra1.is_near_obstacle():
angle = 0
def sonar_test():
indicators = {b'L': "Left Sonar", b'R': "Right Sonar"}
device = Sonar(device="COM10")
while True:
device.pretty_measure()
# kit.motor3.throttle = 0
# kit.motor4.throttle = 0
kit.motor3.throttle = 1
kit.motor4.throttle = 1
time.sleep(4)
kit.motor3.throttle = 0
kit.motor4.throttle = 0
def stop():
kit.motor3.throttle = 0
kit.motor4.throttle = 0
s_front = Sonar(6, 18)
s_left = Sonar(5, 17)
s_right = Sonar(12, 27)
s_backright = Sonar(13, 22)
s_backleft = Sonar(16, 23)
distances = [1000.0, 1000.0, 1000.0, 1000.0, 1000.0]
front_dist = '0'
backleft_dist = '0'
backright_dist = '0'
left_dist = '0'
right_dist = '0'
turn = ''
#!/usr/bin/python2.7
from time import sleep
from sys import exit
from meow import Meow
from sonar import Sonar
from debug import *
if __name__ == "__main__":
meow = Meow()
sonar = Sonar()
while 1:
sleep(.1)
distance = sonar.readValue()
infoMessage("[+] distance: ", distance)
if distance < 100:
meow.playNextSound()
sleep(3)
def test_sonar():
sonar = Sonar()
while True:
distance = sonar.get_distance()
print(distance)
