TwinCAT and Unity – Part #3

After a long break we are back in 3rd part of my TwinCAT and Unity series. Once again we start this part with some small refactoring. The main goal of this part is to create a working simulation of a simple belt conveyor with one sensor. As an additional bonus we will animate the belt movement as well.

Simulation Object

When creating new simulation classes we are repeating the same procedure. We implement the IRequireTwinCatHandler interface and we create two variables – one for POU name and one variable name. All of them are also inheriting MonoBehavior. We can safely assume that most of our future simulation elements will share this pattern. All of them will require TwinCAT handler, all of them will have to inherit MonoBehavior and all of them will have at least one variable to either write to or read from. Going forward we may find that we also doing more common things between the objects.

In the Unity project I have created Simulation Base folder and in that folder a new base class SimulationObject.

public class SimulationObject : MonoBehaviour, IRequireTwinCatHandler
{
    public ITwinCatHandler TwinCatHandler { get; set; }
    [SerializeField]
    public string pouName;
    public string varName;
}

Now in each simulation class (Sim_ProxySensor, FB_SquareWave) we can change our declaration to inherit SimulationObject. We also have to remove TwinCat handler, pouName and varName variables from each class. Another important change that need to be done is the modification of dependency injection service that we wrote in the last part. We have to change the Linq query to look for SimulationObject implementations.

// Find all elements that require TwinCat handler
var elements = FindObjectsOfType<SimulationObject()>
                       .OfType<IRequireTwinCatHandler>();
foreach(var element in elements)
{
    element.TwinCatHandler = twinCatHandler;
}

Conveyor class

Once we have refactoring out of the way, we can focus on creating a new class for our conveyor. From functional perspective – the conveyor will run constantly unless we will detect an object in front of a sensor. We will use the sensor created in the last part. The principle of operation of our conveyor will be very simple – we will have a single output which will indicate that the conveyor motor is either on or off. Our conveyor simulation class will only worry about the conveyor. The sensor signal will be sent to the PLC by a different simulation element. Let’s create Sim_Conveyor class in Unity.

public class Sim_Conveyor : SimulationObject
{
    [SerializeField]
    public float conveyorSpeed = 0.5f;
    public bool direction;
    public float fbkOffset;
    public float fbkPosition;

    private float offset = 0;
    private Renderer rend;
    private bool motorEnabled;

    // Start is called before the first frame update
    void Start()
    {
        rend = GetComponent<MeshRenderer>();
    }

    // Update is called once per frame
    void Update()
    {
        motorEnabled = TwinCatHandler.ReadBool(pouName, varName);
    }

    private void OnCollisionStay(Collision collidingObject)
    {
        float modifier = direction ? 1 : -1;

        Vector3 beltVelocity = transform.forward * modifier * conveyorSpeed 
              * Time.deltaTime;        

        Rigidbody rb = collidingObject.gameObject.GetComponent<Rigidbody>();

        if (motorEnabled)
        {
            rb.position += beltVelocity;
            rb.MovePosition(rb.position + beltVelocity);            
            fbkPosition += beltVelocity.x;            
        }
    }
}

The TwinCAT related code is pretty straight forward, thanks to our SimulationObject a lot of things is happening in the background. All we need is to read one variable and perform some action depending on its state. Let’s leave it for now and start building the Unity scene. I’ll explain the details of each element as we go along

Conveyor simulation

The biggest question we have to ask ourselves is how we would like to simulate the movement of the object on the conveyor? Should we simulate the belt itself and its physical properties and behavior? That would be possible, but it also would be complicated in implementation and to be fair, do we even need this level of accuracy/complexity? When we consider the purpose of this simulation system – testing PLC logic, we can simply say no, we don’t need to go that deep. We will take the following approach – if the object is in contact with the conveyor belt surface we will begin to move this object with the linear speed that real conveyor would have. To simulate movement of the object on the conveyor we have to detect object collision in Unity. The collision can be only detected between Rigid Body components and Collider boxes.

When collision event is detected we need to decide what should happen – as in real world the conveyor would move the box in direction of belt travel. Same will happen here

In this line we are calculating the displacement (conventionally named “velocity”) that we are going to apply to item(s) on the belt.

Vector3 beltVelocity = transform.forward * modifier * conveyorSpeed 
              * Time.deltaTime; 

Here we are picking up the RigidBody component of colliding object and apply calculated displacement:

Rigidbody rb = collidingObject.gameObject.GetComponent<Rigidbody>();
if (motorEnabled)
{
   rb.position += beltVelocity;
   rb.MovePosition(rb.position + beltVelocity);            
   fbkPosition += beltVelocity.x;            
}

On my Github page you can find a 3D model of a conveyor and a small tote (box). In addition there is a texture to allow us to portray the conveyor movement. When everything is put together it should look like this.

The conveyor model is divided into two elements – Belt and Structure. The Belt is the important one. We have to add BoxCollider to it and Sim_Conveyor class we have created. As an addition we can add Belt shader to simulate the movement of belt surface. If we completed the refactoring task correctly the pouName and varName fields should be available automatically – we don’t have them in our Sim_Conveyor class – they reside in the base object – SimulationObject

The tote model has to have a BoxCollider added to it as well as RigidBody

PLC Code

The scene is set. Now let’s write a simple function block to control the conveyor. Our functional description is as follow – enable conveyor until object is detected at the sensor position.

FUNCTION_BLOCK FB_Conveyor
VAR_INPUT
	Occupied	: BOOL;
END_VAR
VAR_OUTPUT
	EnableMotor	: BOOL;
END_VAR
VAR
END_VAR

//-------------------------------------------------------------------

IF NOT Occupied THEN
	EnableMotor := TRUE;
ELSE
	EnableMotor := FALSE;
END_IF

I doubt, that we can have a simpler conveyor control. Let’s extend our DataExchange GVL to the following:

VAR_GLOBAL
	bVar1	: BOOL;
	iVar1	: DINT;
	button1 : BOOL;
	status1 : BOOL;
	
	sensor1 : BOOL;
	motor1	: BOOL;
	
END_VAR

And let’s instantiate the conveyor FB in our main program

PROGRAM MAIN
VAR
	fbConveyor		: FB_Conveyor;
END_VAR

//------------------------------------------------------------------

fbConveyor(
	Occupied := 	DataExchange.sensor1,
	EnableMotor =>	DataExchange.motor1
);

That’s it – the PLC code is complete. Back in unity we have to populate the variable names and POU names in our simulation objects – sensor and conveyor. In both cases the POU name is DataExchange and Variable names are sensor1 and motor1 as defined in our GVL.

Testing

If everything has been setup correctly when we start the PLC ad the Unity the tote should start moving towards the sensor and stop once it hits the sensor beam. If the tote goes through the sensor beam and falls of the conveyor then the sensor variable is probably set incorrectly. If the tote just drops down without touching the belt – the box collider for the belt is missing. Expected result should look like this

Stopped box at the end of conveyor

Belt animation

This is a nice addition however it doesn’t affect the simulation itself. The belt animation is a trick that produces an impression of belt movement. To achieve that we are going to use texture offset. When we click on the texture of the belt in Unity we can see that one of the properties there is called X and Y offset. If we modify them when editing the scene we can see that the belt texture position is changing. Now if we put that behavior into our class we will get a nice smooth motion. Small modification to Update method and new AnimateTexture method.

void Update()
{
   motorEnabled = TwinCatHandler.ReadBool(pouName, varName);
   if (motorEnabled)
   {
      AnimateTexture();
   }
}
void AnimateTexture()
{
    offset += Time.deltaTime * conveyorSpeed * 1.01f;
    fbkOffset = offset;
    Vector2 currentOffset = rend.material.mainTextureOffset;
    rend.material.mainTextureOffset = new Vector2(offset, currentOffset.y);
}

The “magic number” of 1.01 is an arbitrary value that allows to align the texture movement with belt speed. The requirement of that number comes from the fact that there is a small discrepancy between belt surface animation and tote movement. It is likely caused by errors in UV mapping in the conveyor model. I wasn’t able to pinpoint the root cause of that phenomenon, maybe someone will be able to explain it?

Yet another feature has been implemented! We have a fully working conveyor belt that behaves as expected. See you soon in the next part where we are going to implement some pneumatic actuators – another very common automation component.

Leave a Reply

Your email address will not be published. Required fields are marked *