Tool rotation in movel bugged?

Hello all,

I have been making an application that takes bottle caps and screws it onto bottles using the sixth axis.

I have a CS8C controller with TX60 arm.

The application is connected to a Profibus master PLC which tells the robot the order of several positions as well as the positions themselves and triggers the motion.

When the cap arrives the PLC sends signals to the controller and the arm picks up the cap, no problems. Then the arm returns to the waiting position.

When the bottle arrives, if the arm has the cap, the PLC sends signal to the robot and robot arm first goes above the bottle, to the position let's call it aboveNeckPos, then puts cap on as far as possible onto the bottle neck without screwing and then the idea is to screw it on.

The problem is that almost all these motions have to be linear so the bottle does not break and so I use movel. However, the angle needed to screw the cap on is 120 degrees. Everything is vertical so the z axis is along the bottle.

My idea was to put the screwing position, let's call it screwedPos, the same as the "push cap on" position, let's call it neckPos, except with .trsf.rz=120.00 instead of .trsf.rz=0.00.

The idea is the following movement sequence:

...->aboveNeckPos->neckPos->screwedPos->(gripper release)->neckPos->aboveNeckPos->...

The bug is the following:

1. After arriving at screwedPos the gripper has rotated only 60.0 degrees, and no matter which number below 180.0 degrees I put the gripper rotates only half the amount (so if we want 170.0 the gripper rotates 85.0).
2. If the next position after gripper release is not neckPos but aboveNeckPos the gripper first screws the rest of the way after releasing and then unscrews while lifting back to aboveNeckPos.

I am at a loss as to what is going on.

The PLC sends the correct information and that same information arrives to the CS8C, that is confirmed.

Any help would be appreciated.

Hello TygerDawg, thanks for your reply. Yes, I did not provide the code, because it is rudimentary.

So here are more details. I have several positions. DefaultPos, CapPosAbove, CapPos, BottlePosAbove, NeckPos, ScrewedPos. I also have a frame, fFrame which is defined relative to the world as some rotations of the axes such that the z axis points down and other axes are slightly rotated around z axis by a few degrees so that all my movement is in x direction and z direction, the y is always constant. All the points are defined relative to this frame. Now, the only difference between ScrewedPos and NeckPos is:

NeckPos.trsf.rz = 0.0
NeckPos.trsf.z = 200.0
ScrewedPos.trsf.rz = 120.0
ScrewedPos.trsf.z = 200.2

Now, the code itself is divided into 5 tasks. Communication of inputs (PLC->CS8C), logic of inputs (extracting information from the input bits and floats such as arm configuration etc.), logic of outputs (based on current arm position and forces etc. extract information to be sent to PLC), Communication of outputs (CS8C->PLC) and movement task.

The communication of inputs task has, somewhere in there, the following lines:

NeckPos.trsf.y = aioGet(io:BLOCKA_IF20)
ScrewedPos.trsf.y = NeckPos.trsf.y
NeckPos.trsf.x = aioGet(io:BLOCKA_IF21)
ScrewedPos.trsf.x = NeckPos.trsf.x
NeckPos.trsf.z = aioGet(io:BLOCKA_IF22)
ScrewedPos.trsf.z = aioGet(io:BLOCKA_IF23)
ScrewedPos.trsf.rz = aioGet(io:BLOCKA_IF24)
posChoice = aioGet(io:BLOCKA_IW0)

I have confirmed that what the PLC sends is what the CS8C receives.

The posChoice is the position PLC sends to the robot.

The movement task is structured as follows:

begin
while(true)
hereDist = distance(currentPos, lcPos)
switch posChoice
case 0
...
break
...
case 2,3
posDist = distance(NeckPos, lcPos)
if (posDist>posDistTrig) or (isEmpty() and hereDist>posDistTrig)
movel(NeckPos, tTool, mNomSpeed)
lcPos = NeckPos
endIf
break
case 4,5
posDist = distance(ScrewedPos, lcPos)
if (posDist>posDistTrig) or (isEmpty() and hereDist>posDistTrig)
movel(ScrewedPos, tTool, mNomSpeed)
lcPos = ScrewedPos
endIf
break
case 6,7
posDist = distance(NeckPos, lcPos)
if (posDist>posDistTrig) or (isEmpty() and hereDist>posDistTrig)
movel(NeckPos, tTool, mNomSpeed)
lcPos = NeckPos
endIf
break
...
endSwitch
donePos = posChoice
delay(0)
endWhile
end

Basically, what this does is that it calculates the movement only if the movement is big enough (I put the posDistTrig to 0.1mm) and then confirms the chosen position by sending the donePos back to PLC. The PLC sequence is 3->5->7 or 2->4->6 depending on the starting position (1 or 2) but that does not matter for this part of the sequence. I have confirmed on the PLC that this sequence is in fact the one.

I honestly don't know what could be the problem that it halves the angle. What is interesting if I put an angle greater than 180.0 than the agripper rotates in the other direction (the negative angle rotation) but the angle is still halved. So ScrewedPos.trsf.rz in [0, 180.0) amounts to rotation of [0, 90.0), but ScrewedPos.trsf.rz in (180.0, 360.0] amounts to the rotation of (-90.0, 0.0].

In my attempt to get the robot working I have switched to joint movement. So the movement task movements are as follows:

case 4,5
posDist = distance(ScrewedPos, lcPos)
if (posDist>posDistTrig) or (isEmpty() and hereDist>posDistTrig)
rotationAng = ScrewPos.trsf.rz
ScrewPos.trsf.rz = 0.0
pointToJoint(tTool, lastKnownJ, ScrewPos, jJoint)
jJoint.j6 = jJoint.j6 + rotationAng
movej(jJoint, tTool, mNomSpeed)
lcPos = ScrewedPos
endIf
break

Similarly for other points.

This did not solve the problem. The angle is still only half the wanted angle. The only benefit is that now it does not suffer for angles greater than 180.0 degrees.

Hello Galet, thanks for the response.

Quote

- When the gripper is above the bottle, the Tool (tTool) is positionned in the center of the bottle. You can turn with Appro function (probably arround Rz). In this case, be carreful that the blend of the movement is OFF. The movement need to be a MoveL.

- When the gripper is above the bottle, the Tool (tTool), is on the edge of the bottle. You can turn with MoveC function.

- You align the Axe6 of the robot with the axe of the bottle. You can turn directly with moveJ on axe 6. (Convert the reference point and change the .j6 data)

I have not tried Appro so far. The closest to what I am doing is your third solution. In fact it is exactly what I am doing with movej.

Quote

When you use a TRSF, the rotated values are limited to [-180°,180°], not to [0°,360°] !

I know, but I have to send a positive number from the PLC so for me it is [0.0, oo) with keeping in the back of the mind that after crossing 180.0 the movel rotates in negative direction. That is why for my newest code the rotation is using movej and j6 axis for rotations with more than 180.0 degrees of movement.

After having played for a while I noticed something. Apparently the 0.2mm separation between the points was somehow messing the movement up. After increasing the ScrewedPos.trsf.z to 235.3 (so 0.3mm separation between screwed and neck positions) the movel fully completed the movement of 179.99 degrees. I do not have an explanation as to why, it might have something to do with the fact that I use blend with both leave and reach set to 0.1, but any help would be appreciated.

For movej I am currently doing similar thing, it does not work that simply. I put wanted rotation to 180.01 degrees. For some reason the robot simply stops, no error and motion buffer is not empty, if the ScrewedPos.trsf.z >= 235.2, so I put it to 235.1mm and am gradually decreasing NeckPos.trsf.z. The more I decrease it the more of rotation the robot does. If I put NeckPos.trsf.z = 234.9 (so 0.2mm separation between screwed and neck positions) I get rotation of 91.33, if I put it to NeckPos.trsf.z = 234.7 (so 0.4mm separation) I get 135.67 degrees of rotation. Keep in mind that the wanted j6 angle is 180.01 for all of them.

While the blending problem might be responsible for movel, after all 0.3mm separation is 0.1mm blended from starting side, 0.1 blended from the other side so that leaves 0.1 mm of actual movement and that would explain why I needed 0.3mm for movel to rotate fully, but it fails to explain why movej would behave weirdly.

Thanks both of you. The problem was in fact that blending was enabled. When I put blending distances to 0.0mm both movel and movej rotated the full wanted amount.

The only problem left is that if movej is used and the distance between screwed and neck positions is < 0.4mm the robot randomly stops with no errors, and buffer is not empty. That is, if NeckPos.trsf.z = 235.0mm then robot arm unexpectedly stops if 234.6mm < ScrewedPos.trsf.z < 235.4mm.

Quote

What's append ?

I don't understand this question.

append -

1. attach, affix

2. to add as a supplement or appendix (as in a book)

Quote

Rather than set the blending values to 0, can you try :

- to set the mNomSpeed.blend to OFF before the movement.

or

- to add a WaitEndMove() after the Move_()

I basically did a WaitEndMove, before sending next command from the PLC I waited for motion buffer to be empty and for the arm to be settled. The arm still did not rotate the full amount.

As for setting the blend to OFF, it is easier for me to change blending distance as I already change them for some points to speed up the process where the movement to or from a certain position (like default position) need not be exact.

I have a question, is there a maximum blending distance? Does controller ignore leave and reach (or set them to predefined values) if I put in a very large number, say 1000.0 for blending?

Quote

You can use different mDesc...

...

Of course, there are many different solutions to the same problem.

Quote

When the robot is moved from A to B, the Reach from A + Leave from B is reduced to the distance [A,B]. So, if [A,B] = 10 mm, the blending is automatically reduce to 5 mm even if the value is bigger !

But if the tool is already in the sphere, and you send a new point in the same sphere, without waitEndMove(), I think the robot don't move, because it is already in final position.

Thanks for the explanation. Judging by my experience here the robot moves half the distance if the tool is already in the sphere.