uraxisdaemon/src/main/java/at/acdp/urweb/fhpp/FHMaster.java

package at.acdp.urweb.fhpp;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.*;

import at.acdp.urweb.Params;
import com.digitalpetri.modbus.codec.Modbus;
import com.digitalpetri.modbus.master.ModbusTcpMaster;
import com.digitalpetri.modbus.master.ModbusTcpMasterConfig;
import com.digitalpetri.modbus.requests.*;
import com.digitalpetri.modbus.responses.ReadHoldingRegistersResponse;
import io.netty.util.ReferenceCountUtil;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class FHMaster implements IFHMaster {
    private final Logger logger = LoggerFactory.getLogger(getClass());
    public static int posfaktorNum=1;
    public static int posfaktorDiv=1;
    private ModbusTcpMaster master;

    private final ScheduledExecutorService scheduler = Executors.newSingleThreadScheduledExecutor();
    private volatile boolean started = false;
    public FHMaster() {
    }

    @Override
    public void start(Params app) throws InterruptedException {
        started = true;
        ModbusTcpMasterConfig config = new ModbusTcpMasterConfig.Builder(app.axisIP)
                .setPort(app.axisPort)
                .build();
        master = new ModbusTcpMaster(config);
        while(true) {
            Thread.sleep(1000);
            CompletableFuture<Status> s = readStatus();
            s.join();
        }
    }

    private void sendRequests(ModbusTcpMaster master, List<ModbusRequest> reqs) {
        for(var req: reqs) {
            var future=master.sendRequest(req, 0);
            future.whenCompleteAsync((response, ex) -> {
                if (response != null) {
                    ReferenceCountUtil.release(response);
                }
                else {
                    logger.error("Completed exceptionally, message={}", ex.getMessage(), ex);
                }
            }, Modbus.sharedExecutor());
        }
    }

    private void send(byte[] bytes, int quantity) {
        CompletableFuture<ReadHoldingRegistersResponse> f =
                master.sendRequest(new WriteMultipleRegistersRequest(0, quantity, bytes), 0);
        f.join();
    }

    @Override
    public void direktAuftrag(int pos, int speed, int start, boolean relative, boolean stop) {
        byte ccon=0x0;
        ccon|=(1<<0); // Enable drive
        ccon|=((stop?0:1)<<1); // !Stop
        ccon|=(1<<2); // Release Brake
        ccon|=(0<<3); //Reset fault
        ccon|=(0<<4); //Reserved
        ccon|=(0<<5); //lock software
        ccon|=(1<<6 |0 <<7); //lock software
                            // 0,0 Satzselektion
                            // 1,0 Direktauftrag
                            // 0,1 Reserviert
                            // 1,1 Reserviert

        byte cpos=0x0;
        cpos|=(1<<0); // !Halt
        cpos|=(start<<1); // start fahrauftrag
        cpos|=(0<<2); // start homing
        cpos|=(0<<3); // jog pos
        cpos|=(0<<4); // jog neg
        cpos|=(0<<5); //  teach value
        cpos|=(0<<6); //  clear remaining pos.
        cpos|=(0<<7); //  Reserved

        byte cdir=0x0;
        cdir|=((relative==true?1:0) <<0); //  relative(1), absolute(0)
        cdir|= (0<<1| 0<<2); //absolute/relative
                                // 0,0 Positionsregelung
                                // 0,1 Kraftbetrieb
                                // 1,0 Geschwindigkeitsregelung
                                // 1,1 Reserviert
        cdir|=(0<<3);   // Function Number Kurvenscheibenfunktion (0=nein)
        cdir|=(0<<4);   // Function Number Kurvenscheibenfunktion (0=nein)
        cdir|=(0<<5);   // Function Group Kurvenscheibenfunktion (0=nein)
        cdir|=(0<<6);   // Function Group Kurvenscheibenfunktion (0=nein)
        cdir|=(0<<7);   // 0 = normal; 1=kurvenscheibenfunktion

        byte sbyte4= (byte) speed; // Geschw, in % vom Basiswert (PNU540
        byte sbyte8= (byte)(pos& 0xff);
        byte sbyte7= (byte)((pos>>8) & 0xff);
        byte sbyte6= (byte)((pos>>16) & 0xff);
        byte sbyte5= (byte)((pos>>24) & 0xff);

        send(new byte[]{ccon,
                        cpos,
                        cdir,
                        sbyte4,
                        sbyte5,
                        sbyte6,
                        sbyte7,
                        sbyte8,
                },4);
    }

    @Override
    public CompletableFuture<Status> readStatus() {
        if(master==null)
            return null;
        CompletableFuture<ReadHoldingRegistersResponse> f=master.sendRequest(new ReadHoldingRegistersRequest(0,4), 0);

        CompletableFuture<Status> fc= f.thenApply((response) -> {
            if (response != null) {
                var bb=response.getRegisters();
                try {
                    byte[] bytes = new byte[bb.readableBytes()];
                    bb.readBytes(bytes);
                    Status s=new Status();
                    s.read(bytes);
                    return s;
                }
                catch (Exception e) {
                    e.printStackTrace();
                }
                ReferenceCountUtil.release(response);
            }
            else {
                logger.error("Completed exceptionally, message={}", "null");
            }
            return null;
        });
        return  fc;
    }

    private void trySleep(int ms) {
        try {
            Thread.sleep(ms);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    private void sendReq(byte[] what) {
        List<ModbusRequest> blist = new ArrayList<>();
        blist.add(new WriteMultipleRegistersRequest(0,4, what));
        trySleep(10);
        sendRequests(master, blist);
        trySleep(10);

    }

    @Override
    public void bereitschaft() {

        //S. 135 "Betriebsbereitschaft
        sendReq(new byte[]{0b00000001,0,0,0,0,0,0,0});
        sendReq(new byte[]{0b00000011,0,0,0,0,0,0,0});


        //S. 140 Betriebsbereischaft - direktauftrag
       // sendReq(new byte[]{0b00000100,0,0,0,0,0,0,0});
       // sendReq(new byte[]{0b01000111,0,0,0,0,0,0,0});
    }

    @Override
    public void stop() {
        started = false;
    }
}