Robot Floor Navigation Control Loop with Vision Sensors - PI Control Algorithm

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.