• 日期: 2024-06-28
  • 标签: 常规

该代码为一个机器人在环形路径上运动，通过光电传感器读取地面的颜色信息来进行路径跟踪。

以下是每一句的作用：

  1. function run(P,D): 定义一个函数run，该函数接受两个参数P和D，用于控制机器人的速度。
  2. data2={}: 定义一个空table data2，用于存储光电传感器读取的数据。
  3. PD=0; error=0: 初始化PD和error为0，分别代表比例增益和当前误差。
  4. for i=1,5,1 do res,data=sim.readVisionSensor(floorSensorHandles[i]) if(res>=0)then data2[i]=data[11] end end: 通过循环读取5个光电传感器的数据，并将第11个数据存储在data2中。
  5. if(data2[4]<0.73) then data2[3]=0 end: 如果第4个传感器的读数小于0.73，则将第3个传感器的读数设为0，表示检测到路径边界。
  6. if(data2[2]<0.73) then data2[1]=0 end: 如果第2个传感器的读数小于0.73，则将第1个传感器的读数设为0，表示检测到路径边界。
  7. if(data2[2]<data2[4]) then if(data2[3]<data2[1]) then error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[2]-0.85) else error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[2]-0.85) end else if(data2[3]<data2[1]) then error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[4]-0.85) else error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[4]-0.85) end end: 根据data2的值计算error，error是目标位置和当前位置之间的距离。
  8. PD=P*error+D*(error-previousError): 根据比例增益P和微分增益D，以及当前误差和前一个误差，计算PD控制值。
  9. previousError=error; previousdata=data2: 将previousError和previousdata更新为当前的error和data2，用于下一次计算PD控制值。
  10. if error<0.2 then rightV=speed leftV=speed previousError=0 end: 如果error小于0.2，则直接让机器人前进，并重置previousError。
  11. elseif data2[3]<0.75 or data2[4]<0.75 then rightV=speed+PD*speed leftV=speed-PD*speed end: 如果第3或4个传感器的读数小于0.75，则机器人向左转，左轮速度减小，右轮速度增加。
  12. elseif data2[1]<0.75 or data2[2]<0.75 then rightV=speed-PD*speed leftV=speed+PD*speed end: 如果第1或2个传感器的读数小于0.75，则机器人向右转，右轮速度减小，左轮速度增加。
  13. sim.setJointTargetVelocity(leftmotor,leftV): 设置左轮的目标速度。
  14. sim.setJointTargetVelocity(rightmotor,rightV): 设置右轮的目标速度。
  15. function sysCall_threadmain(): 定义一个函数sysCall_threadmain，作为主线程函数。
  16. leftmotor=sim.getObjectHandle('Pioneer_p3dx_leftMotor'); rightmotor=sim.getObjectHandle('Pioneer_p3dx_rightMotor'); car=sim.getObjectHandle('P1'); floorSensorHandles={-1,-1,-1,-1,-1}: 获取机器人的左右轮控制句柄和地面传感器句柄。
  17. floorSensorHandles[1]=sim.getObjectHandle('l'); floorSensorHandles[2]=sim.getObjectHandle('m'); floorSensorHandles[3]=sim.getObjectHandle('r'); floorSensorHandles[4]=sim.getObjectHandle('m0'); floorSensorHandles[5]=sim.getObjectHandle('m2': 将光电传感器的句柄存储到floorSensorHandles中。
  18. flag={0,0,0,0,0,0,0,0}: 定义一个flag数组来标记机器人是否到达每个停靠点。
  19. speed=-5; back=-1: 设置机器人的初始速度和方向。
  20. stop={{-0.1,0.099},{-0.731,0.098},{-1.302,0.104},{-1.469,1.263},{-0.782,1.299},{0.238,1.296},{0.822,1.284},{0.6,0.131}}: 定义一个stop数组，存储机器人需要停靠的位置。
  21. sim.setIntegerSignal('occupy2',1): 设置信号occupy2为1，表示机器人占用第二个停靠点。
  22. sim.setJointTargetVelocity(leftmotor,0); sim.setJointTargetVelocity(rightmotor,0): 停止机器人的运动。
  23. sim.setIntegerSignal('arrive2',1): 设置信号arrive2为1，表示机器人到达第二个停靠点。
  24. sim.waitForSignal('finish2'): 等待信号finish2，表示其他机器人允许机器人离开。
  25. sim.clearIntegerSignal('finish2'): 清除信号finish2。
  26. while true do if(sim.getIntegerSignal('occupy3')==nil) then break end end: 循环等待直到信号occupy3消失，表示第三个停靠点不再被占用。
  27. sim.clearIntegerSignal('occupy2'): 清除信号occupy2。
  28. flag={0,1,0,0,0,0,0,0}: 设置flag数组，表示机器人已经到达第二个停靠点。
  29. while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do: 循环执行，直到模拟结束。
  30. position=sim.getObjectPosition(car,-1): 获取机器人当前位置。
  31. if (math.abs(position[1]-stop[1][1])<0.05)and(math.abs(position[2]-stop[1][2])<0.05) and (flag[1]==0) then sim.setIntegerSignal('occupy1',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive1',1) sim.waitForSignal('finish1') sim.clearIntegerSignal('finish1') while true do if(sim.getIntegerSignal('occupy2')==nil) then break end end sim.clearIntegerSignal('occupy1') flag={1,0,0,0,0,0,0,0} end: 如果机器人到达第一个停靠点，则设置信号occupy1为1，停止运动，等待信号finish1，然后清除信号occupy1，并等待信号occupy2。
  32. if (math.abs(position[1]-stop[2][1])<0.05)and(math.abs(position[2]-stop[2][2])<0.05) and (flag[2]==0) then sim.setIntegerSignal('occupy2',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive2',1) sim.waitForSignal('finish2') sim.clearIntegerSignal('finish2') while true do if(sim.getIntegerSignal('occupy3')==nil) then break end end sim.clearIntegerSignal('occupy2') flag={0,1,0,0,0,0,0,0} end: 如果机器人到达第二个停靠点，则设置信号occupy2为1，停止运动，等待信号finish2，然后清除信号occupy2，并等待信号occupy3。
  33. if (math.abs(position[1]-stop[5][1])<0.05)and(math.abs(position[2]-stop[5][2])<0.05) and (flag[5]==0) then sim.setIntegerSignal('occupy5',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive5',1) sim.waitForSignal('finish5') sim.clearIntegerSignal('finish5') while true do if(sim.getIntegerSignal('occupy6')==nil) then break end end sim.clearIntegerSignal('occupy5') flag={0,0,0,0,1,0,0,0} end: 如果机器人到达第五个停靠点，则设置信号occupy5为1，停止运动，等待信号finish5，然后清除信号occupy5，并等待信号occupy6。
  34. if (math.abs(position[1]-stop[6][1])<0.05)and(math.abs(position[2]-stop[6][2])<0.05) and (flag[6]==0) then sim.setIntegerSignal('occupy6',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive6',1) sim.waitForSignal('finish6') sim.clearIntegerSignal('finish6') while true do if(sim.getIntegerSignal('occupy7')==nil) then break end end sim.clearIntegerSignal('occupy6') flag={0,0,0,0,0,1,0,0} end: 如果机器人到达第六个停靠点，则设置信号occupy6为1，停止运动，等待信号finish6，然后清除信号occupy6，并等待信号occupy7。
  35. if (math.abs(position[1]-stop[3][1])<0.05)and(math.abs(position[2]-stop[3][2])<0.05) and (flag[3]==0) then sim.setIntegerSignal('occupy3',1) if(sim.getIntegerSignal('occupy4')==nil) then sim.clearIntegerSignal('occupy3') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy4')==nil) then break end end sim.clearIntegerSignal('occupy3') end flag={0,0,1,0,0,0,0,0} end: 如果机器人到达第三个停靠点，如果没有其他机器人占用，则设置信号occupy3为1，否则等待直到占用机器人离开。
  36. if (math.abs(position[1]-stop[4][1])<0.05)and(math.abs(position[2]-stop[4][2])<0.05) and (flag[4]==0) then sim.setIntegerSignal('occupy4',1) if(sim.getIntegerSignal('occupy5')==nil) then sim.clearIntegerSignal('occupy4') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy5')==nil) then break end end sim.clearIntegerSignal('occupy4') end flag={0,0,0,1,0,0,0,0} end: 如果机器人到达第四个停靠点，如果没有其他机器人占用，则设置信号occupy4为1，否则等待直到占用机器人离开。
  37. if (math.abs(position[1]-stop[7][1])<0.05)and(math.abs(position[2]-stop[7][2])<0.05) and (flag[7]==0) then sim.setIntegerSignal('occupy7',1) if(sim.getIntegerSignal('occupy8')==nil) then sim.clearIntegerSignal('occupy7') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy8')==nil) then break end end sim.clearIntegerSignal('occupy7') end flag={0,0,0,0,0,0,1,0} end: 如果机器人到达第七个停靠点，如果没有其他机器人占用，则设置信号occupy7为1，否则等待直到占用机器人离开。
  38. if (math.abs(position[1]-stop[8][1])<0.05)and(math.abs(position[2]-stop[8][2])<0.05) and (flag[8]==0) then sim.setIntegerSignal('occupy8',1) if(sim.getIntegerSignal('occupy1')==nil) then sim.clearIntegerSignal('occupy8') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy1')==nil) then break end end sim.clearIntegerSignal('occupy8') end flag={0,0,0,0,0,0,0,1} end: 如果机器人到达第八个停靠点，如果没有其他机器人占用，则设置信号occupy8为1，否则等待直到占用机器人离开。
  39. run(0.22,0.06): 调用run函数，使用参数0.22和0.06来控制机器人的速度。
  40. end: 代码执行完毕，退出。

原文地址: https://www.cveoy.top/t/topic/oVCV 著作权归作者所有。请勿转载和采集!

免费AI点我,无需注册和登录