p2ctl() /************* AXIS 2 POSITION CONTROL CODE *******/
 { static double v2spd = 0.0;
 /** LEAD LAG CONTROLLER **/
 v2spd = p2bl v2spd + p2al (double)p2er + p2a2 (double)p2erl;
 v2sp = (int)v2spd;
 }


Figure 8.15. Major loop position control subroutine for joint 2.


Table 8.3. Position control variables as used in Figures 8.14 and 8.15.

 Joint 0
 Variables Description
 v2spd velocity set point
 p2sp position set point
 p2max maximum position allowed
 p2min minimum position allowed
 p2erl previous position error value
 p2er current position error value
 p2 current position value
 p2bl coefficient from equation 8-8
 p2al coefficient from equation 8-9
 p2a2 coefficient from equation 8-10
 v2sp integer value of velocity set point


Vision Controllers

 Control of the manipulator based on the information from the robot vision system was

similar to the position control method. However, for the vision controller, the input to the

controller was the position of the centroid of a targeted fruit in the image plane of the CCD

camera rather than the position of the joint. The output from the controller was a servo valve

control signal (D/A word). Therefore, a feedback loop was created as shown in the block

diagram (Figure 8.16). In this feedback loop, the vision block was essentially a position

measurement device with a variable gain. The vision system determined the position of the

targeted fruit in the image and the gain was a function of the position of the sliding joint. The

vision system did not contain any dynamic attributes which affected the system.