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)