123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649 |
- from multiprocessing import Process
- # from event_storage import EventStorage
- from hard_disk_storage import HardDiskStorage
- from memory_storage import MemoryStorage
- from sonar_socket2 import Receiver
- from sonar_temp_tcp import ClientTemp
- from temp_sonar import Receiver1
- from sonar_udp import sender
- from sonar_command_tcp_once import Client
- import socket
- import struct
- import time
- import construct as c
- import redis
- import logging
- from logging.handlers import TimedRotatingFileHandler
- servo_angle = 'servo1_angle'
- MIN_ANGLE = 1
- MAX_ANGLE = 358
- SYSTEM_MODE = 'once' # once,multiple
- hard_disk_db = HardDiskStorage()
- servo_data = c.Struct(
- header=c.Int16ul, # 固定不便,可能用于区分不同设备
- none_1=c.Int8ub, # 固定不便,可能用于区分不同设备
- servo1_flag=c.Int8ub,
- none_2=c.Int8ub, # 固定不便,可能用于区分不同设备
- servo2_flag=c.Int8ub,
- depth=c.Bytes(4),
- temp=c.Bytes(4),
- yaw=c.Bytes(4), # 航向
- roll=c.Bytes(4), # 横滚
- pitch=c.Bytes(4), # 俯仰
- servo1_angle=c.Bytes(4),
- servo2_angle=c.Bytes(4),
- water_leak1=c.Bytes(4),
- water_leak2=c.Bytes(4),
- )
- def get_program_status():
- sql = "SELECT time_end FROM measurement_data ORDER BY id desc limit 1"
- res = hard_disk_db.execute_sql(sql, None)
- if res:
- if res[0]['time_end']:
- return "off"
- else:
- return "on"
- else:
- return "off"
- class MyProcess(Process): # 继承Process类
- def __init__(self, name, config):
- super(MyProcess, self).__init__()
- self.name = name
- self.__sock = None
- self.__ip = config['ip']
- self.__port = config['port']
- self.__save_run_time = 0
- # self.__converter = converter
- # self.__storager = MemoryStorage({'ip':'127.0.0.1','port':4001})
- # self.__save_frequency = config['save_frequency']
- self.__save_frequency = 300 # 最小值120
- self.redis_db = None
- self.__connected = False
- self.run_count = 0
- self.servo_status = None
- self.proc_status = get_program_status()
- def run(self):
- self.__connect()
- self.__connected = True
- self.redis_db = MemoryStorage()
- self.redis_db.set_value_permanent({'operation_mode': 'auto'})
- self.redis_db.set_value_permanent({'operation_init_move': '0'})
- self.redis_db.set_value_permanent({'operation_move_flag': '0'})
- self.redis_db.set_value_permanent({'operation_move_angel': '0'})
- while True:
- speed = 30
- acceleration = 2000
- operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode']
- operation_init_move = self.redis_db.get_value(['operation_init_move'])['operation_init_move']
- status = self.get_sonar_status()
- # self.redis_db.set_value_permanent({'newset_angel': curr_angel})
- try:
- curr_angel = int(status['servo1_angle'])
- curr_depth = round(status['depth'])
- except:
- print('get sonar status error')
- continue
- info_dict = {'depth': curr_depth, 'servo_angle': curr_angel}
- self.redis_db.set_value(info_dict)
- print('fang --> operation_mode:', operation_mode)
- if operation_mode == 'manual':
- time_flag = 0
- temp_now_time = int(time.time())
- operation_save_time = self.redis_db.get_value(['operation_save_time'])['operation_save_time']
- print('fang --> operation_save_time:', operation_save_time)
- print('fang --> temp_now_time:', temp_now_time)
- print('fang --> operation_init_move:', operation_init_move)
- if operation_save_time != None:
- print('fang --> cha:', temp_now_time - int(operation_save_time))
- if operation_save_time != None and (temp_now_time - int(operation_save_time)) > 300:
- self.redis_db.set_value_permanent({'operation_mode': 'auto'})
- continue
- dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'}
- self.redis_db.set_value_permanent(dict1)
- if operation_init_move == '0':
- self.send_command(location=curr_angel, speed=speed, acceleration=acceleration)
- self.redis_db.set_value_permanent({'operation_init_move': '1'})
- operation_move_angel = self.redis_db.get_value(['operation_move_angel'])['operation_move_angel']
- operation_move_flag = self.redis_db.get_value(['operation_move_flag'])['operation_move_flag']
- print('fang --> operation_move_angel:', operation_move_angel)
- print('fang --> operation_move_flag:', operation_move_flag)
- print('fang --> curr_angel:', curr_angel)
- if operation_move_angel != None and operation_move_flag == '1':
- after_angel = int(operation_move_angel) + curr_angel
- if int(operation_move_angel) > 0:
- after_angel += 1
- if after_angel > 358:
- after_angel = 358
- elif after_angel < 1:
- after_angel = 1
- print('fang --> after_angel:', after_angel)
- self.send_command(location=after_angel, speed=speed, acceleration=acceleration)
- # time.sleep(0.2)
- time_flag = 1
- self.redis_db.set_value_permanent({'operation_move_angel': '0'})
- self.redis_db.set_value_permanent({'operation_move_flag': '0'})
- if time_flag == 0:
- time.sleep(1)
- print('fang --> --> --> --> --> --> --> --> -->')
- self.__save_run_time = 0
- continue
- status = self.get_sonar_status() # 发查询指令,当作心跳包。
- info_dict = {'depth': status['depth'], 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'],
- 'pitch': status['pitch'], 'servo_angle': status[servo_angle],
- 'water_leak1': status['water_leak1'], 'water_leak2': status['water_leak2']}
- self.redis_db.set_value(info_dict)
- now_time = time.time()
- switch = get_program_status()
- print(self.proc_status, "-->", switch)
- if self.proc_status == 'on' and switch == 'off':
- dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'}
- self.redis_db.set_value_permanent(dict1)
- time.sleep(3)
- self.proc_status = switch
- continue
- elif self.proc_status == 'off' and switch == 'off':
- dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'}
- self.redis_db.set_value_permanent(dict1)
- time.sleep(3)
- continue
- elif self.proc_status == 'off' and switch == 'on':
- self.__save_run_time = 0
- self.proc_status = switch
- else:
- pass
- if now_time - self.__save_run_time >= self.__save_frequency - 10: # 提前开启声纳
- print('sonar_begin---------------------------', time.time())
- if SYSTEM_MODE == 'once':
- dict = {'sonar_open': 'open', 'range': 'small', 'flag': '0'}
- elif SYSTEM_MODE == 'multiple':
- dict = {'sonar_open': 'open', 'range': 'small', 'flag': '0'}
- self.redis_db.set_value(dict)
- self.rotate_zero(speed, acceleration)
- print('sonar_status:', self.redis_db.get_value(['sonar_status'])['sonar_status'])
- if now_time - self.__save_run_time >= self.__save_frequency and self.redis_db.get_value(['sonar_status'])[
- 'sonar_status'] == 'open':
- print('zz', self.redis_db.get_value(['sonar_status'])['sonar_status'])
- self.__save_run_time = time.time()
- if SYSTEM_MODE == 'once':
- print('duoji_runing---------------------------', time.time())
- self.rotate_once_small(speed, acceleration, self.redis_db, MAX_ANGLE, self.__save_run_time)
- self.redis_db.set_value({'range': 'big'})
- time.sleep(3)
- self.rotate_once_big(speed, acceleration, self.redis_db, MIN_ANGLE, self.__save_run_time)
- self.run_count = self.run_count + 1
- # print('run_count',self.run_count)
- elif SYSTEM_MODE == 'multiple':
- speed = 30
- angle = 10
- self.rotate_multiple_small(angle, speed, acceleration, self.__save_run_time, 's')
- self.redis_db.set_value({'range': 'big'})
- time.sleep(3)
- self.rotate_multiple_big(angle, speed, acceleration, self.__save_run_time, 'b')
- # 关闭声纳
- dict = {'sonar_open': 'close'}
- self.redis_db.set_value(dict)
- time.sleep(1)
- # data_dict = get_sonar_status(self.__sock)
- # self.__conn.set_value(data_dict)
- # find_data = b'\xF0\x0F\xFF\xFF\xFF\xFF\x03'
- # self.__sock.send(find_data)
- #
- # # 接收服务器返回的数据
- # response = self.__sock.recv(1024)
- # print(response)
- # #res = servo_data.parse(response)
- # while True:
- # if isinstance(self.__command, list):
- # for i in self.__command:
- # command_list = json.loads(i['command'])
- # self.command_polling(command_list=command_list)
- # time.sleep(1)
- # else:
- # self.command_polling()
- # time.sleep(1)
- #
- # if self.__stopped:
- # break
- # 建立socket连接
- def __connect(self):
- if self.__sock:
- self.__sock.close()
- self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # 允许重用本地地址和端口
- self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) # 在客户端开启心跳维护
- self.__sock.settimeout(10) # 设置超时时间3mins
- try:
- self.__sock.connect((self.__ip, self.__port))
- # logger.info(f'Connect to [{self.name}]:[{self.__ip}]:[{self.__port}] success !')
- self.__connected = True
- except Exception as e:
- # logger.info(f'Connect to [{self.name}]:[{self.__ip}]:[{self.__port}] failed:{e} !!!')
- self.__connected = False
- self.__reconnect()
- def __reconnect(self):
- while True:
- try:
- if self.__sock:
- self.__sock.close()
- self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
- self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) # 在客户端开启心跳维护
- self.__sock.settimeout(10) # 设置超时时间,单位:秒
- self.__sock.connect((self.__ip, self.__port))
- self.__connected = True
- # logger.info(f'Reconnect to [{self.name}]:[{self.__ip}]:[{self.__port}] success !')
- break
- except Exception as e:
- # logger.info(f'Reconnect to [{self.name}]:[{self.__ip}]:[{self.__port}] failed:{e} !!! Continue reconnect in 5s..')
- self.__connected = False
- time.sleep(5)
- def __send_command(self, command):
- if self.__connected:
- try:
- # send_data = bytes.fromhex(command)
- self.__sock.send(command)
- response = self.__sock.recv(1024)
- return response
- except Exception as e:
- time.sleep(5)
- self.__reconnect()
- print('send', e)
- return None
- # logger.info(f'Send command to [{self.name}]:[{self.__ip}]:[{self.__port}] error:{e}')
- else:
- self.__reconnect()
- return None
- def calculate_checksum(self, data):
- checksum = 0
- for byte in data:
- checksum += byte
- checksum = (~checksum) & 0xFF
- byte_data = bytes([checksum])
- hex_string = byte_data.hex()
- return hex_string
- def data_converter(self, hex_data):
- servo_data = c.Struct(
- header=c.Int16ul, # 固定不便,可能用于区分不同设备
- none_1=c.Int8ub, # 固定不便,可能用于区分不同设备
- servo1_flag=c.Int8ub,
- none_2=c.Int8ub, # 固定不便,可能用于区分不同设备
- servo2_flag=c.Int8ub,
- depth=c.Bytes(4),
- temp=c.Bytes(4),
- yaw=c.Bytes(4), # 航向
- roll=c.Bytes(4), # 横滚
- pitch=c.Bytes(4), # 俯仰
- servo1_angle=c.Bytes(4),
- servo2_angle=c.Bytes(4),
- )
- res = servo_data.parse(hex_data)
- return res
- def get_sonar_status(self):
- # find_data = b'\xF0\x0F\xFF\xFF\xFF\xFF\x03'
- # find_data = b'\xF0\x0F\xFF\xFF\xFF\xFF\x03'
- while True:
- try:
- find_data = b'\xF0\x0F\xF0\xF0\xF0\xF0\x03' # 增加漏水检测传感器指令
- response = self.__send_command(find_data)
- result = {'depth': None, 'temp': None, 'yaw': None, 'roll': None, 'pitch': None, 'servo1_angle': None,
- 'servo2_angle': None, 'water_leak1': None, 'water_leak2': None}
- if response != None:
- if len(response) == 35:
- res = servo_data.parse(response)
- result['depth'] = struct.unpack('>f', res.depth)[0]
- result['temp'] = struct.unpack('>f', res.temp)[0]
- result['yaw'] = struct.unpack('>f', res.yaw)[0]
- result['roll'] = struct.unpack('>f', res.roll)[0]
- result['pitch'] = struct.unpack('>f', res.pitch)[0]
- result['servo1_angle'] = int(struct.unpack('>f', res.servo1_angle)[0])
- result['servo2_angle'] = int(struct.unpack('>f', res.servo2_angle)[0])
- if len(response) == 43: # 增加漏水检测传感器
- res = servo_data.parse(response)
- result['depth'] = struct.unpack('>f', res.depth)[0]
- result['temp'] = struct.unpack('>f', res.temp)[0]
- result['yaw'] = struct.unpack('>f', res.yaw)[0]
- result['roll'] = struct.unpack('>f', res.roll)[0]
- result['pitch'] = struct.unpack('>f', res.pitch)[0]
- result['servo1_angle'] = int(struct.unpack('>f', res.servo1_angle)[0])
- result['servo2_angle'] = int(struct.unpack('>f', res.servo2_angle)[0])
- result['water_leak1'] = struct.unpack('>f', res.water_leak1)[0]
- result['water_leak2'] = struct.unpack('>f', res.water_leak2)[0]
- return result
- except Exception as e:
- print('fang --> get sonar error :', e)
- time.sleep(2)
- continue
- def send_command(self, location, speed, acceleration):
- header = 'f00f'
- servo1_location = struct.pack('>f', location).hex()
- servo2_location = struct.pack('>f', location).hex()
- servo1_speed = struct.pack('>f', speed).hex()
- servo2_speed = struct.pack('>f', speed).hex()
- servo1_acceleration = struct.pack('>f', acceleration).hex()
- servo2_acceleration = struct.pack('>f', acceleration).hex()
- data = servo1_location + servo2_location + servo1_speed + servo2_speed + servo1_acceleration + servo2_acceleration
- data_hex = bytes.fromhex(data)
- checksum = self.calculate_checksum(data_hex)
- send_data = header + data + checksum
- send_data = bytes.fromhex(send_data)
- # print(send_data) # 输出校验和的十六进制表示
- response = self.__send_command(send_data)
- def rotate_zero(self, speed, acceleration):
- while True:
- status = self.get_sonar_status()
- if status[servo_angle] != None:
- if status[servo_angle] > MIN_ANGLE + 5:
- print('rotate_zero', status[servo_angle])
- self.send_command(location=MIN_ANGLE, speed=speed, acceleration=acceleration)
- else:
- break
- def rotate_once_small(self, speed, acceleration, save_data, location, time_start):
- count = 0
- num = 0 # 图像编号
- angel = None
- save_flag = 0
- send_data = True
- while True:
- status = self.get_sonar_status()
- # print(status[servo_angle])
- operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode']
- if operation_mode == 'manual':
- dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'}
- self.redis_db.set_value_permanent(dict1)
- break
- if status[servo_angle] != None:
- if send_data == True:
- # time_start = time.time() # 记录开始时间
- save_flag = 1
- self.send_command(location=location, speed=speed, acceleration=acceleration)
- send_data = False
- if save_flag == 1:
- info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'],
- 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'],
- 'pitch': status['pitch'], 'img_type': 's',
- 'servo_angle': status[servo_angle],
- 'water_leak1': status['water_leak1'],
- 'water_leak2': status['water_leak2']}
- save_data.set_value(info_dict)
- num += 1
- if angel != status[servo_angle]:
- angel = status[servo_angle]
- count = 0
- else:
- count = count + 1
- if status[servo_angle] >= MAX_ANGLE - 1 or count > 4:
- save_data.set_value({'flag': '0'})
- logger.warning('angel error, save image stop , curr range: s . ')
- break
- def rotate_once_big(self, speed, acceleration, save_data, location, time_start):
- count = 0
- num = 0 # 图像编号
- angel = None
- save_flag = 0
- send_data = True
- while True:
- status = self.get_sonar_status()
- print(status[servo_angle])
- operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode']
- print('fang --> rotate --> model :', operation_mode)
- if operation_mode == 'manual':
- dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'}
- self.redis_db.set_value_permanent(dict1)
- break
- if status[servo_angle] != None:
- if send_data == True:
- # time_start = time.time() # 记录开始时间
- save_flag = 1
- self.send_command(location=location, speed=speed, acceleration=acceleration)
- send_data = False
- if save_flag == 1:
- info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'],
- 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'],
- 'pitch': status['pitch'], 'img_type': 'b',
- 'servo_angle': status[servo_angle],
- 'water_leak1': status['water_leak1'],
- 'water_leak2': status['water_leak2']
- }
- save_data.set_value(info_dict)
- num += 1
- if angel != status[servo_angle]:
- angel = status[servo_angle]
- count = 0
- else:
- count = count + 1
- if status[servo_angle] <= MIN_ANGLE + 0.5 or count > 4:
- save_data.set_value({'flag': '0'})
- logger.warning('angel error, save image stop , curr range: b . ')
- break
- def rotate_once_little(self, speed, acceleration, save_data, location, time_start, mode):
- count = 0
- angel = None
- while True:
- status = self.get_sonar_status()
- if status[servo_angle] != None:
- if location >= MIN_ANGLE and location <= MAX_ANGLE:
- self.send_command(location=location, speed=speed, acceleration=acceleration)
- if angel != status[servo_angle]:
- angel = status[servo_angle]
- count = 0
- else:
- count = count + 1
- if count > 2:
- info_dict = {'flag': '1', 'timestamp': time_start, 'num': status[servo_angle],
- 'depth': status['depth'],
- 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'],
- 'pitch': status['pitch'], 'img_type': mode,
- 'servo_angle': status[servo_angle],
- 'water_leak1': status['water_leak1'],
- 'water_leak2': status['water_leak2']
- }
- save_data.set_value(info_dict)
- break
- # def rotate_once(self,speed, acceleration,save_data):
- # status = self.get_sonar_status()
- # if status[servo_angle] != None:
- # if status[servo_angle] > 0.5:
- # self.send_command(location=0, speed=speed, acceleration=acceleration)
- # else:
- # count = 0
- # num = 0 # 图像编号
- # angel = None
- # save_image_flag = 0
- # while count < 5:
- # status = self.get_sonar_status()
- # if status[servo_angle] != None and save_image_flag == 0:
- # if status[servo_angle] < 0.5:
- # time_start = time.time() # 记录开始时间
- # save_image_flag = 1
- # self.send_command(location=359, speed=speed, acceleration=acceleration)
- # #print(angel,status[servo_angle])
- # if angel != status[servo_angle]:
- # angel = status[servo_angle]
- # count = 0
- # else:
- # count = count + 1
- # if save_image_flag == 2:
- # count = count + 2
- # if status[servo_angle] != None:
- # if count > 2 or status[servo_angle] >= 358:
- # save_image_flag = 0
- # save_data.set_value({'range':'big'})
- # if status[servo_angle] != None:
- # if count > 2 or status[servo_angle] >= 358:
- # save_image_flag = 2
- # time.sleep(5)
- # self.send_command(location=0, speed=speed, acceleration=acceleration)
- # time_now = time.time()
- # if save_image_flag == 1:
- # info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'],
- # 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'],
- # 'pitch': status['pitch'],
- # 'servo_angle': status[servo_angle]}
- # save_data.set_value(info_dict)
- # num += 1
- # print('asdasd',111111111111111111111)
- # elif save_image_flag == 2:
- # info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'],
- # 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'],
- # 'pitch': status['pitch'],
- # 'servo_angle': status[servo_angle]}
- # save_data.set_value(info_dict)
- # num += 1
- # print('asdasd', 22222222222222222222222222222222)
- # else:
- # info_dict = {'flag': '0', 'timestamp': time_start, 'num': num, 'depth': status['depth'],
- # 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'],
- # 'pitch': status['pitch'],
- # 'servo_angle': status[servo_angle]}
- # save_data.set_value(info_dict)
- # # time_sum = time_end - time_start # 计算的时间差为程序的执行时间,单位为秒/s
- # # print(time_sum)
- def rotate_multiple_small(self, angel, speed, acceleration, time_start, mode):
- t = 0
- while True:
- operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode']
- if operation_mode == 'manual':
- dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'}
- self.redis_db.set_value_permanent(dict1)
- break
- save_status = self.redis_db.get_value(['flag'])['flag']
- if save_status == '0':
- location_angel = MIN_ANGLE + angel * t
- if location_angel > MAX_ANGLE:
- break
- else:
- self.rotate_once_little(speed, acceleration, self.redis_db, location_angel, time_start, mode)
- t = t + 1
- def rotate_multiple_big(self, angel, speed, acceleration, time_start, mode):
- t = 0
- while True:
- operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode']
- if operation_mode == 'manual':
- dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'}
- self.redis_db.set_value_permanent(dict1)
- break
- save_status = self.redis_db.get_value(['flag'])['flag']
- if save_status == '0':
- location_angel = MAX_ANGLE - angel * t - 10
- if location_angel < MIN_ANGLE:
- break
- else:
- self.rotate_once_little(speed, acceleration, self.redis_db, location_angel, time_start, mode)
- t = t + 1
- def rotate_multiple(self, client_socket, angel, speed, acceleration):
- # 模式二,指定间隔,达到,停顿,继续,完成,停止
- status = self.get_sonar_status()
- if status[servo_angle] != None:
- if status[servo_angle] > 0.5:
- self.send_command(location=0, speed=speed, acceleration=acceleration)
- else:
- i = int(360 / angel)
- t = 0
- time_start = time.time() # 记录开始时间
- while t < i + 1:
- count = 0
- speed = 300
- acceleration = 2000
- last_angel = None
- while count < 4:
- status = self.get_sonar_status()
- location_angel = min(angel * t, 359)
- self.send_command(location=location_angel, speed=speed, acceleration=acceleration)
- if last_angel != status[servo_angle]:
- last_angel = status[servo_angle]
- count = 0
- else:
- print(t, last_angel)
- count = count + 1
- if count > 3:
- t = t + 1
- self.send_command(location=0, speed=speed, acceleration=acceleration)
- if __name__ == '__main__':
- servo_config = {'ip': '192.168.1.61', 'port': 4001, 'save_frequency': 0}
- log_handler = TimedRotatingFileHandler(filename='/home/sencott/sonar/log/test_tcp.log', when='midnight', interval=30, backupCount=5,
- encoding="utf8")
- # 配置日志记录器
- logging.basicConfig(level=logging.WARNING, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s",
- handlers=[log_handler])
- # 获取日志记录器
- logger = logging.getLogger()
- awaken_gls10 = Process(target=sender, name='awaken_gls10', daemon=True)
- command_p = Client()
- temp = ClientTemp()
- save_image = Process(target=Receiver, name='save_image', daemon=True)
- save_temp = Process(target=Receiver1, name='save_temp', daemon=True)
- p = MyProcess(name='servo', config=servo_config) # 实例化进程对象
- awaken_gls10.start()
- command_p.daemon = True
- command_p.start()
- temp.daemon = True
- temp.start()
- save_image.start()
- save_temp.start()
- p.daemon = True
- p.start()
- while True:
- time.sleep(1)
- # print('main..............................', time.time())
|