Перемещение робота в позицию, введенную пользователем, с помощью датчиков расстояния ⇐ Python

[Гость]

Итак, я только что начал проект для своей выпускной стажировки. Одна из вещей, которую они хотят, — это робот, который работает на Python (через Raspberry Pi на роботе) и которым можно управлять из любого места, используя подключение к Интернету и ноутбук. До меня они наняли еще одного человека, который убедил компанию купить DJI Robomaster EP с ядром THE ROBOT. Этот робот запрограммирован в приложении robomasters, которое вы можете загрузить с их веб-сайта, но для загрузки кода также можно использовать Raspberry Pi.
Роботу необходимо:

• Переместитесь в введенное положение X и Y на тестовом поле U-образной формы, сохраняя при этом переднюю часть прямо.

< p>Последний человек купил для робота лидар, в написанном им коде он использует лидар для измерения положения X до стены. Для позиции Y он использовал инфракрасный датчик, который идет в комплекте с роботом (бюджет закончился). В определенной степени это работает, но он рекомендовал мне заменить инфракрасный датчик на другой лидар. Роботу также необходимо сохранять прямой угол, теперь это делается с помощью камеры (которая часто выходит из строя и нарушает код), которая ведет камеру на робота. Затем этот сигнал фильтруется с помощью ПИД-регулятора, и когда он срабатывает, робот выпрямляется с ошибкой 0,0. Проблема в том, что камера, которая включена слишком долго, приводит к сбою всего кода и заставляет робота врезаться в стену.
Поэтому предыдущий человек сказал мне, что лучше купить 2 недорогих лидарных датчика, работающих с Raspberry Pi. На данный момент в системе уже есть один лидар для измерения расстояния X. Два лидара, которые мне нужно будет купить, будут размещены под углом в задней части робота, чтобы измерить угол и расстояние по оси Y.
У меня нет никаких знаний в программировании, поэтому я действительно не знаю, что делать, но у меня есть код от последнего человека, который я вроде как получил.
Мой вопрос:

• Как мне реализовать 2 новых лидара и удалить камеру и инфракрасный датчик

ПИД-регулятор

Код: Выделить всё

#class voor de PID controller die op basis van vision de robot stuurt
class PIDController:
def __init__(self, kp, ki, kd, setpoint,kpz, kiz, kdz, setpointz):
self.kp = kp
self.ki = ki
self.kd = kd
self.kpz = kpz
self.kiz = kiz
self.kdz = kdz
#center van de camera is 0.5
self.setpoint = 0.5
#de gewenste hoekverdraaing ten opzichte van zijn doel is 0 graden
self.setpointz = 0
self.prev_error = 0
self.integral = 0
self.prev_errorz = 0
self.integralz = 0
#regelaar berekening voor zijwaartse translatie om zichzelf te centeren ten opzichte van de blauwe lijn
def compute(self, xc):
global error
error = self.setpoint - xc
#print (error)
#wanneer de lijn niet meer zichtbaar is zal het resultaat van de bovenstaande berekening 0.5 - niks zijn. In dit geval maak je van error 0 zodat de robot stil staat.
if error == 0.5:
error = 0
self.integral += error
derivative = error - self.prev_error
# waarden voor kp, ki en kd kunnen aangepast worden aan het begin van het main programma
output = (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)
self.prev_error = error

#print("succes")
return output
#regelaar berekening voor rotatie van de robot. Dit is voor het uitlijnen met de blauwe lijn.
def computez(self, thetac):
global errorz
errorz = self.setpointz - thetac
self.integralz += errorz
derivativez = errorz - self.prev_errorz
#waarden voor kp, ki en kd kunnen aangepast worden aan het begin van het main programma
outputz = (self.kpz * errorz) + (self.kiz * self.integralz) + (self.kdz * derivativez)
#afzwakking van de output van de regelaar.  Pas de waarden voor de regelaar aan om dit netjes te doen.
outputz = outputz/10

self.prev_errorz = errorz

#print("succesz")
return outputz

Код определения расстояния (я думаю):

Код: Выделить всё

#functie voor het afhandelen van afstandinformatie van de IR-distance sensor
def sub_data_handler(sub_info):
global pos_x
distance = sub_info
#print("tof1:{0}  tof2:{1}  tof3:{2}  tof4:{3}".format(distance[0], distance[1], distance[2], distance[3]))
pos_x = distance [0]
pos_x = float (pos_x)

#functie die eenmaal de bovenstaande functie aanroept
def get_position():
#subscribe op sensor informatie met een frequentie van 1
ep_sensor.sub_distance(freq =1, callback=sub_data_handler)
#het programma krijgt iets meer dan een seconde de tijd om eenmalig een afstand in te meten.
time.sleep(1.1)
#unsubscribe
ep_sensor.unsub_distance()

Код, использующий камеру:

Код: Выделить всё

def cameraregeling(xc, thetac):
if plan == "INIT": #ga naar de init positie toe ongeacht van het vasthebben van een object
#beweeg langs de y en z-as tot de robot genult is.
ep_chassis.drive_speed(x=0, y=pid.compute(xc), z=-pid.compute(thetac), timeout=5)
#-pid.compute(xc)
#pid.computez(thetac)
if plan == "MAGAZIJN_GO":
#sturen van P controllers voor zijwaards bewegen en draaien richting de lijn
ep_chassis.drive_speed(x=0.1, y=pid.compute(xc), z=-pid.computez(thetac), timeout=5)
#pid.compute(xc)
#-pid.computez(thetac)
print(pid.compute(xc))
print(-pid.computez(thetac))
if plan == "STA_STIL":
ep_chassis.drive_speed(x=0, y=0, z=0, timeout=1)
if plan == "RETURN":
ep_chassis.drive_speed(x=-0.1, y=pid.compute(xc), z=-pid.computez(thetac), timeout=5)

Я не знаю, правильный ли это код для получения помощи. В качестве контекста я также добавлю основной код, где все используется.

Код: Выделить всё

#start main programma
if __name__ == '__main__':
#initialiseren van robot resources
ep_robot = robot.Robot()
ep_robot.initialize(conn_type='rndis') #rndis voor het verbinden van de robot met een Raspberry Pi via USB
ep_chassis = ep_robot.chassis
ep_arm = ep_robot.robotic_arm
ep_vision = ep_robot.vision
ep_camera = ep_robot.camera
ep_gripper = ep_robot.gripper
ep_robot.set_robot_mode(mode=robot.CHASSIS_LEAD)
ep_arm = ep_robot.robotic_arm
ep_sensor = ep_robot.sensor

ep_version = ep_robot.get_version()#vraag versie op

ep_battery = ep_robot.battery
ep_battery.sub_battery_info(5, sub_info_handler, ep_robot)
time.sleep(1)
ep_battery.unsub_battery_info()

#vraag de y-positie van de IR sensor
get_position()
pos_y = lidar.getDistance()

# Define PID constants
kp = 3
ki = 0
kd = 0
kpz = 2.5
kiz = 0.002
kdz = 0

# Setpoint (0.5 voor translatie, 0 voor rotatie)
setpoint = 0.5
setpointz = 0

# Create PID controller
pid = PIDController(kp, ki, kd, setpoint,kpz, kiz, kdz, setpointz)

#start hoofdprogramma
#luister naar berichten op de TCP server voor instructies
try:
while True:
status = "WACHTEN_OP_BERICHT"
pos_y = lidar.getDistance()
get_position()
opgehaald = 0
check = 0
message_constructor()
# Receive data from the client
print("listening")
data = client_socket.recv(1024)

if not data:
continue
# Split the received string into variables
variables = data.decode().split(';')
# Assuming the correct order:  variable1, variable2, variable3
status = variables [0]
print (status)
doel_x = variables [1]
print (doel_x)
doel_y = variables [2]
print (doel_y)
doel_z = variables [3]
print (doel_z)
objectkeuze = variables[4]
print (objectkeuze)
snelheid = variables [5]
print (snelheid)

doel_x = float(doel_x)
doel_y = float(doel_y)
init_y = doel_y
doel_z = float(doel_z)

print (status, doel_x, doel_y, doel_z)
#geef de status terug aan de gebruiker
message_constructor()
#einde van het behandelen van messages

if status == "INFO":
#vraag positie op
pos_y = lidar.getDistance()
get_position()
#vraag batterij info op
ep_battery.sub_battery_info(5, sub_info_handler, ep_robot)
time.sleep(1)
ep_battery.unsub_battery_info()
#vraag object informatie op
input_state = GPIO.input(pin_number)
if input_state == GPIO.HIGH:
RFIDreader()
print (text.strip())
#print (text, type(text))
colorline = text.strip()
else:
text = "0"
print("geen object aanwezig")
message_constructor()

#wanneer de robot mag rijden naar een opgegeven coordinaat
if status == "GO":
move_until_close_enough(doel_x, doel_y)
#als de positie bereikt is verander de status van GO naar BEREIKT
status = "BEREIKT"
print("bereikt")
message_constructor()

#programma voor terug gaan naar start
if status == "INIT":
plan = "INIT"
status = "ONDERWEG"
#doel_x = 1000 # beweeg tot 1 meter van de achterste muur vandaan
#doel_y = lidar.getDistance()
#move_until_close_enough(doel_x, doel_y)
#doel_y = 630 #beweeg tot ongeveer 6 meter van de linkermuur vandaan zodat de magazijnlijnen zichtbaar zijn
#move_until_close_enough(doel_x, doel_y)
#start camera feed voor controleren ten opzichte van de lijn
StartCamera()
time.sleep(3)#geef de camera tijd om de queue te vullen met informatie over de lijn
ReadCamera()
time.sleep(1)
while True:
cameraregeling(xc, thetac)
#print(error,"and", errorz)
if abs(error) = 25 and input_state != GPIO.HIGH:
print("error in object oppakken")
status = "ERROR"
plan = "STA_STIL"
cameraregeling(xc, thetac)
time.sleep(3)
movement_handlerstil()
cv2.destroyAllWindows()
result = ep_vision.unsub_detect_info(name="line")
cv2.destroyAllWindows()
ep_camera.stop_video_stream()
break
time.sleep(0.1)

#leg tijd vast voor het terugrijden
currenttime = time.time()

while True:
cameraregeling(xc, thetac)
if time.time() - currenttime >5:
plan = "INIT"
print (abs(error),abs(errorz))
if abs(error) = 1:
status = "OBJECT_AFZETTEN"
plan = "STA_STIL"
cameraregeling(xc, thetac)
time.sleep(3)
pos_y = lidar.getDistance()
get_position()
message_constructor()
Gripperopen()
plan = "RETURN"
currenttime = time.time()
break
time.sleep(0.1)
while True:
cameraregeling(xc, thetac)
if time.time()-currenttime >= 2:
plan = "INIT"
print (abs(error),abs(errorz))
if abs(error) 

Источник: [url]https://stackoverflow.com/questions/78152637/having-a-robot-move-to-a-user-input-position-using-distance-sensors[/url]