LinkFactory.java

Go to the documentation of this file.

package com.neuronrobotics.sdk.addons.kinematics;
 
import java.util.ArrayList;
import java.util.HashMap;
 
import com.neuronrobotics.sdk.addons.kinematics.gcodebridge.GcodeDevice;
import com.neuronrobotics.sdk.addons.kinematics.gcodebridge.GcodePrismatic;
import com.neuronrobotics.sdk.common.BowlerAbstractDevice;
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.common.IFlushable;
import com.neuronrobotics.sdk.common.Log;
import com.neuronrobotics.sdk.common.TickToc;
import com.neuronrobotics.sdk.dyio.DyIO;
import com.neuronrobotics.sdk.dyio.peripherals.AnalogInputChannel;
import com.neuronrobotics.sdk.dyio.peripherals.CounterOutputChannel;
import com.neuronrobotics.sdk.dyio.peripherals.ServoChannel;
import com.neuronrobotics.sdk.namespace.bcs.pid.IExtendedPIDControl;
import com.neuronrobotics.sdk.namespace.bcs.pid.IPidControlNamespace;
import com.neuronrobotics.sdk.pid.GenericPIDDevice;
import com.neuronrobotics.sdk.pid.ILinkFactoryProvider;
import com.neuronrobotics.sdk.pid.VirtualGenericPIDDevice;
 
// TODO: Auto-generated Javadoc
public class LinkFactory implements IHardwareSyncPulseReciver, IHardwareSyncPulseProvider {
    private static HashMap<String, INewLinkProvider> userLinkProviders = new HashMap<String, INewLinkProvider>();
 
    private ArrayList<AbstractLink> links = new ArrayList<AbstractLink>();
 
    private ArrayList<LinkConfiguration> linkConfigurations = null;
 
    public static void addLinkProvider(String typeTag, INewLinkProvider provider) {
        userLinkProviders.put(typeTag, provider);
        LinkType.addType(typeTag);
    }
 
    public static boolean linkProviderExists(String typeTag) {
        return userLinkProviders.get(typeTag) != null;
    }
 
    public LinkFactory() {
        this(null);
    }
 
    public LinkFactory(BowlerAbstractDevice bad) {
        if (bad != null)
            DeviceManager.addConnection(bad, bad.getScriptingName());
    }
 
    public LinkFactory(ILinkFactoryProvider connection, IExtendedPIDControl d) {
        this(null);
        // Log.enableInfoPrint();
        // TODO fill in the auto link configuration
        LinkConfiguration first = connection.requestLinkConfiguration(0);
        first.setPidConfiguration(d);
        getLink(first);
 
        for (int i = 1; i < first.getTotlaNumberOfLinks(); i++) {
            LinkConfiguration tmp = connection.requestLinkConfiguration(i);
            tmp.setPidConfiguration(d);
            getLink(tmp);
        }
 
    }
 
    public AbstractLink getLink(String name) {
        for (AbstractLink l : links) {
            if (l.getLinkConfiguration().getName().equalsIgnoreCase(name))
                return l;
        }
        String data = "No link of name '" + name + "' exists";
        for (AbstractLink l : links) {
            data += "\n" + l.getLinkConfiguration().getName();
        }
        throw new RuntimeException(data);
    }
 
    public AbstractLink getLink(LinkConfiguration c) {
        for (AbstractLink l : links) {
            if (l.getLinkConfiguration() == c)
                return l;
        }
        return getLinkLocal(c);
    }
 
    public void refreshHardwareLayer(LinkConfiguration c) {
        // retreive the old link
        AbstractLink oldLink = getLink(c);
        links.remove(oldLink);
        AbstractLink newLink = getLinkLocal(c);
        for (ILinkListener l : oldLink.getLinks()) {
            newLink.addLinkListener(l);
        }
        oldLink.removeAllLinkListener();
 
    }
 
    private AbstractLink getLinkLocal(LinkConfiguration c) {
 
        AbstractLink tmp = null;
        // Log.info("Loading link: "+c.getName()+" type = "+c.getTypeEnum()+" device=
        // "+c.getDeviceScriptingName());
        try {
            switch (c.getTypeEnum()) {
 
            case ANALOG_PRISMATIC:
                if (getDyio(c) != null) {
                    tmp = new AnalogPrismaticLink(new AnalogInputChannel(getDyio(c).getChannel(c.getHardwareIndex())),
                            c);
                    tmp.setUseLimits(false);
                }
                break;
            case ANALOG_ROTORY:
                if (getDyio(c) != null) {
                    tmp = new AnalogRotoryLink(new AnalogInputChannel(getDyio(c).getChannel(c.getHardwareIndex())), c);
                    tmp.setUseLimits(false);
                }
                break;
            case PID_TOOL:
            case PID:
                if (getPid(c) != null) {
                    tmp = new PidRotoryLink(getPid(c).getPIDChannel(c.getHardwareIndex()), c, false);
                }
                break;
            case PID_PRISMATIC:
                if (getPid(c) != null) {
                    tmp = new PidPrismaticLink(getPid(c).getPIDChannel(c.getHardwareIndex()), c, false);
                }
                break;
            case DUMMY:
            case VIRTUAL:
                String myVirtualDevName = c.getDeviceScriptingName();
                tmp = new PidRotoryLink(getVirtual(myVirtualDevName).getPIDChannel(c.getHardwareIndex()), c, true);
                break;
            case GCODE_HEATER_TOOL:
                if (getGCODE(c) != null) {
                    tmp = getGCODE(c).getHeater(c);
                }
                break;
            case GCODE_STEPPER_PRISMATIC:
            case GCODE_STEPPER_ROTORY:
            case GCODE_STEPPER_TOOL:
                if (getGCODE(c) != null) {
                    tmp = getGCODE(c).getLink(c);
                }
                break;
            case USERDEFINED:
                if (userLinkProviders.containsKey(c.getTypeString())) {
                    INewLinkProvider iNewLinkProvider = userLinkProviders.get(c.getTypeString());
                    tmp = iNewLinkProvider.generate(c);
                    if (IHardwareSyncPulseProvider.class.isInstance(iNewLinkProvider)) {
                        IHardwareSyncPulseProvider r = (IHardwareSyncPulseProvider) iNewLinkProvider;
                        r.addIHardwareSyncPulseReciver(this);
                    }
                }
                break;
            default:
                break;
            }
        } catch (Throwable t) {
            t.printStackTrace();
        }
        if (tmp == null) {
            String myVirtualDevName = c.getDeviceScriptingName();
            if (!c.isPrismatic()) {
                tmp = new PidRotoryLink(getVirtual(myVirtualDevName).getPIDChannel(c.getHardwareIndex()), c, true);
            } else {
                tmp = new PidPrismaticLink(getVirtual(myVirtualDevName).getPIDChannel(c.getHardwareIndex()), c, true);
            }
        }
        tmp.setLinkConfiguration(c);
        addLink(tmp);
        return tmp;
    }
 
    public void addLink(AbstractLink link) {
        links.add(link);
        if (!getLinkConfigurations().contains(link.getLinkConfiguration()))
            getLinkConfigurations().add(link.getLinkConfiguration());
    }
 
    public double[] getLowerLimits() {
        double[] up = new double[links.size()];
        for (int i = 0; i < up.length; i++) {
            up[i] = links.get(i).getMinEngineeringUnits();
        }
        return up;
    }
 
    public double[] getUpperLimits() {
        double[] up = new double[links.size()];
        for (int i = 0; i < up.length; i++) {
            up[i] = links.get(i).getMaxEngineeringUnits();
        }
        return up;
    }
 
    public void addLinkListener(ILinkListener l) {
        for (AbstractLink lin : links) {
            lin.addLinkListener(l);
        }
    }
 
    public void flush(final double seconds) {
        HashMap<String, Boolean> flushed = new HashMap<String, Boolean>();
        for (LinkConfiguration c : getLinkConfigurations()) {
            String name = c.getDeviceScriptingName();
            // TickToc.tic("Checking "+name+" for flush ");
            // if a device is disconnected it is removed from the device manager. the
            // factory should check all devices
            Object specificDevice = DeviceManager.getSpecificDevice(IFlushable.class, name);
            if (specificDevice == null) {
                getLink(c).flush(seconds);// links flushed directly because there is no flushable device
            } else {
                if (flushed.get(name) == null) {
                    flushed.put(name, true);
                    IFlushable flushDevice = (IFlushable) specificDevice;
 
                    flushDevice.flush(seconds);
                    // TickToc.tic("Flushed "+name);
                }
            }
            // TickToc.tic("Done Checking "+name+" for flush ");
 
        }
        // System.out.println("Flush Took "+(System.currentTimeMillis()-time)+"ms");
    }
 
    public IPidControlNamespace getPid(LinkConfiguration c) {
 
        return (IPidControlNamespace) DeviceManager.getSpecificDevice(IPidControlNamespace.class,
                c.getDeviceScriptingName());
 
    }
 
    public DyIO getDyio(LinkConfiguration c) {
 
        return (DyIO) DeviceManager.getSpecificDevice(DyIO.class, c.getDeviceScriptingName());
 
    }
 
    public GcodeDevice getGCODE(LinkConfiguration c) {
 
        return (GcodeDevice) DeviceManager.getSpecificDevice(GcodeDevice.class, c.getDeviceScriptingName());
 
    }
 
    public void setCachedTargets(double[] jointSpaceVect) {
        if (jointSpaceVect.length != links.size())
            throw new IndexOutOfBoundsException("Expected " + links.size() + " links, got " + jointSpaceVect.length);
        int i = 0;
        for (AbstractLink lin : links) {
 
            try {
                lin.setTargetEngineeringUnits(jointSpaceVect[i]);
            } catch (Exception ee) {
                throw new RuntimeException("Joint " + i + " failed\n" + ee.getMessage());
            }
            i++;
        }
    }
 
    public boolean isConnected() {
        for (LinkConfiguration c : getLinkConfigurations()) {
            // if a device is disconnected it is removed from the device manager. the
            // factory should check all devices
            if (DeviceManager.getSpecificDevice(null, c.getDeviceScriptingName()) == null) {
                return false;
            }
        }
        return true;
    }
 
    public ArrayList<LinkConfiguration> getLinkConfigurations() {
        if (linkConfigurations == null) {
            linkConfigurations = new ArrayList<LinkConfiguration>();
        }
        return linkConfigurations;
    }
 
    public void removeLinkListener(AbstractKinematicsNR l) {
        // TODO Auto-generated method stub
        for (AbstractLink lin : links) {
            lin.removeLinkListener(l);
        }
    }
 
    public void deleteLink(int i) {
        links.remove(i);
        getLinkConfigurations().remove(i);
    }
 
    @Override
    public void sync() {
        doSync();
    }
 
    public VirtualGenericPIDDevice getVirtual(String myVirtualDevName) {
        return (VirtualGenericPIDDevice) DeviceManager.getSpecificDevice(myVirtualDevName, () -> {
            VirtualGenericPIDDevice virtualGenericPIDDevice = new VirtualGenericPIDDevice(myVirtualDevName);
            virtualGenericPIDDevice.addIHardwareSyncPulseReciver(this);
            return virtualGenericPIDDevice;
        });
    }
 
}