This function is a control loop for a robot that uses vision sensors to navigate a floor. The function takes two parameters, P and I, which are coefficients used in the control algorithm.

The function starts by reading the vision sensor data from five different sensors and storing it in the data2 table. If a valid reading is obtained, the 11th element of the data table is stored in the corresponding index of the data2 table.

Next, the function checks if the readings from the 4th and 2nd sensors are less than 0.73. If so, the 3rd and 1st readings are set to 0, respectively.

The function then calculates the error based on the values in the data2 table. The error calculation depends on the relationship between the 2nd and 4th readings, and the 3rd and 1st readings. The error is the sum of the absolute differences between the target values (0.095, 0.85, and 0.85) and the corresponding readings.

The function then calculates the proportional-integral (PI) term using the P and I coefficients and the error. The previousError variable is used to calculate the integral term. The previousError and previousdata variables are updated for the next iteration.

Next, the function checks if the error is less than 0.2. If so, both the rightV and leftV variables are set to the speed variable, and the previousError is reset to 0.

If the error is not less than 0.2, the function checks if the 3rd or 4th readings are less than 0.75. If so, the rightV is set to the speed plus the PI term multiplied by the speed, and the leftV is set to the speed minus the PI term multiplied by the speed.

If the 3rd or 4th readings are not less than 0.75, the function checks if the 1st or 2nd readings are less than 0.75. If so, the rightV is set to the speed minus the PI term multiplied by the speed, and the leftV is set to the speed plus the PI term multiplied by the speed.

Finally, the function sets the target velocities of the left and right motors using the sim.setJointTargetVelocity function.

function runPI data2= PI=0 error=0 for i=151 do resdata=simreadVisionSensorfloorSensorHandlesi ifres=0then data2i=data11 end end ifdata24073 then

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

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