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 __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):
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,
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,
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, *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
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 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
# 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 = ''
def sonar_test():
indicators = {b'L': "Left Sonar", b'R': "Right Sonar"}
device = Sonar(device="COM10")
while True:
device.pretty_measure()
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
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)
def test_sonar():
sonar = Sonar()
while True:
distance = sonar.get_distance()
print(distance)
