Oprogramowanie – Robot RFID

The software implementing the multitasking system distributed across various execution units was developed in the CPDev environment [1] (specifically in the iConfPLC version).

The source codes for the CPDEV1, CPDEV2, and CPDEV3 programs executed by the virtual machines are presented below. These machines feature an identical set of global variables, a portion of which is exchanged between them.

PROGRAM CPDEV1
 VAR_EXTERNAL (*$AUTO*) END_VAR
 VAR
  ID_OK_TRIG : R_TRIG;
  TMP : ARRAY[0..1] OF INT;
  KOM : KOMENDY;
  X : INT := 0;
  Y : INT := 1;
 END_VAR
 
 ID_OK_TRIG(CLK:=ID_OK);
 
 IF ID_OK_TRIG.Q THEN
   IF KOMENDA = KOM.OBR_LEWO THEN
      KIERUNEK := (KIERUNEK + 3) MOD 4;
   ELSIF KOMENDA = KOM.OBR_PRAWO THEN
      KIERUNEK := (KIERUNEK + 1) MOD 4;
   ELSIF KOMENDA = KOM.NA_WPROST THEN
      TMP := NOWA_POZYCJA(XY, KIERUNEK, 
                         KOM.NA_WPROST);
      XY[X] := TMP[X];
      XY[Y] := TMP[Y]; 
      MAPA_RFID[XY[0],XY[1]] := ID_RFID;
   ELSIF KOMENDA = KOM.STOP THEN 
      MAPA_RFID[XY[0],XY[1]] := ID_RFID;
   END_IF
  END_IF 
END_PROGRAM

PROGRAM CPDEV2
VAR_EXTERNAL (*$AUTO*) END_VAR
VAR
   PUSTE: LWORD := 0;
   X : INT := 0;
   Y : INT := 1;
   KOM : KOMENDY;
   OBR : OBROTY;
   TMP: ARRAY[0..1] OF INT; 
END_VAR                           

IF MAPA_RFID[XY[X], XY[Y]] <> PUSTE THEN
   KOMENDA := KOM.STOP;

   TMP := NOWA_POZYCJA(XY, KIERUNEK, KOM.NA_WPROST);

   IF (TMP[X] >= 0 AND TMP[X] <= MAPA_ROZMIAR) 
              AND (TMP[Y] >= 0 AND TMP[Y] <= MAPA_ROZMIAR) THEN
      IF MAPA_RFID[TMP[X], TMP[Y]] = PUSTE THEN
         KOMENDA := KOM.NA_WPROST; // Na wprost, kierunek bez zmian
      END_IF
   END_IF
   
   IF KOMENDA = KOM.STOP THEN
      IF OBROT = OBR.LEWO THEN
           TMP := NOWA_POZYCJA(XY, KIERUNEK, KOM.OBR_LEWO);
              
           IF (TMP[X] >= 0 AND TMP[X] <= MAPA_ROZMIAR) 
                      AND (TMP[Y] >= 0 AND TMP[X] <= MAPA_ROZMIAR) THEN
              IF MAPA_RFID[TMP[X], TMP[Y]] = PUSTE THEN
                 KOMENDA := KOM.OBR_LEWO; // Obrót w lewo w miejscu
              END_IF
           END_IF  
      ELSE
           TMP := NOWA_POZYCJA(XY, KIERUNEK, KOM.OBR_PRAWO);
           
           IF (TMP[X] >= 0 AND TMP[X] <= MAPA_ROZMIAR) 
                      AND (TMP[Y] >= 0 AND TMP[X] <= MAPA_ROZMIAR) THEN
              IF MAPA_RFID[TMP[X], TMP[Y]] = PUSTE THEN
                 KOMENDA := KOM.OBR_PRAWO; // Obrót w prawo w miejscu
              END_IF   
           END_IF
      END_IF
   END_IF 
END_IF
END_PROGRAM

PROGRAM CPDEV3
 VAR_EXTERNAL (*$AUTO*) END_VAR
 VAR
    KOM : KOMENDY;
    ID_OK_TRIG : F_TRIG;
 END_VAR
 
 ID_OK_TRIG(CLK:=ID_OK);
 
 IF ID_OK_TRIG.Q THEN
    IF KOMENDA = KOM.NA_WPROST THEN
       ROBOT_POLECENIE := 1;
       ROBOT_WARTOSC := MAPA_ROZSTAW;  
    ELSIF KOMENDA = KOM.OBR_LEWO THEN
       ROBOT_POLECENIE := 2;
       ROBOT_WARTOSC := 90;   
    ELSIF KOMENDA = KOM.OBR_PRAWO THEN
       ROBOT_POLECENIE := 3;
       ROBOT_WARTOSC := -90;
    ELSE
       ROBOT_POLECENIE := 0;
    END_IF
 END_IF
END_PROGRAM

To supplement the project, a special empty program named TYPY was developed to store custom variable types, along with a NOWA_POZYCJA function that implements the search for a new position serving as the base for exploration.

PROGRAM TYPY
END_PROGRAM

TYPE KOMENDY : STRUCT
   STOP : INT := 0;
   NA_WPROST : INT := 1;
   OBR_LEWO : INT := 2;
   OBR_PRAWO : INT := 3;
   BRAK : INT := 4;
END_STRUCT;
END_TYPE

TYPE OBROTY : STRUCT
   LEWO : INT := 1;
   PRAWO : INT := 2;
END_STRUCT;
END_TYPE

FUNCTION NOWA_POZYCJA :  ARRAY[0..1] OF INT (*RET_TYPE*)
VAR_INPUT
   POS : ARRAY[0..1] OF INT; //aktualna pozycja
   KIER : INT;               //aktualny kierunek
   KOM : INT;                //komenda
END_VAR
   IF KOM >= 1 AND KOM <= 3 THEN
      CASE KIER OF
           0: // N
           IF KOM = 1 THEN POS[1] := POS[1] + 1;  
           ELSIF KOM = 2 THEN POS[0] := POS[0] - 1;
           ELSIF KOM = 3 THEN POS[0] := POS[0] + 1;
           END_IF  
           1: // E
           IF KOM = 1 THEN POS[0] := POS[0] + 1;  
           ELSIF KOM = 2 THEN POS[1] := POS[1] + 1;
           ELSIF KOM = 3 THEN POS[1] := POS[1] - 1;
           END_IF 
           2: //S
           IF KOM = 1 THEN POS[1] := POS[1] - 1;  
           ELSIF KOM = 2 THEN POS[0] := POS[0] + 1;
           ELSIF KOM = 3 THEN POS[0] := POS[0] - 1;
           END_IF  
           3: // W
           IF KOM = 1 THEN POS[0] := POS[0] - 1;  
           ELSIF KOM = 2 THEN POS[1] := POS[1] - 1;
           ELSIF KOM = 3 THEN POS[1] := POS[1] + 1;
           END_IF 
         END_CASE
      END_IF        
   NOWA_POZYCJA := POS;  (*RET_VALUE*) 
END_FUNCTION

[1] CPDev-ControlProgramDeveloper/VirtualMachine: The repository contains public sources of the CPDev (Control Program Developer) virtual machine runtime (VM). The environment is available for download at https://cpdev.kia.prz.edu.pl

Software