AbstractLink.java

Go to the documentation of this file.

package com.neuronrobotics.sdk.addons.kinematics;
 
import java.util.ArrayList;
 
 
import com.neuronrobotics.sdk.addons.kinematics.gcodebridge.IGcodeExecuter;
import com.neuronrobotics.sdk.addons.kinematics.imu.IMU;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.common.IFlushable;
import com.neuronrobotics.sdk.common.Log;
import com.neuronrobotics.sdk.common.TickToc;
import com.neuronrobotics.sdk.pid.PIDLimitEvent;
import com.neuronrobotics.sdk.pid.PIDLimitEventType;
 
import javafx.scene.transform.Affine;
 
// TODO: Auto-generated Javadoc
// Kevin Shouldn't the Link's channel be kept in this level of Abstraction? The way I designg AbstractCartesianPositonDevice  Requires this
public abstract class AbstractLink implements  IFlushable{
 
    private double targetValue=0;
    
    private double targetEngineeringUnits=0;
    
    private ArrayList<ILinkListener> links = new ArrayList<ILinkListener>();
    
    private LinkConfiguration conf =null;
    private ArrayList<LinkConfiguration> slaveLinks;
    private LinkFactory slaveFactory = new LinkFactory();
    private boolean useLimits=true;
 
    private Object linksLocation=null;
    
    private IMU imu = new IMU();
    public double getDeviceMaximumValue() {
        return conf.getDeviceTheoreticalMax();
    }
    public double getDeviceMinimumValue() {
        return conf.getDeviceTheoreticalMin();
    }
    public void setDeviceMaximumValue(double max) {
         conf.setDeviceTheoreticalMax(max);
    }
    public void setDeviceMinimumValue(double min) {
         conf.setDeviceTheoreticalMin(min);
    }
    public double getDeviceMaxEngineeringUnits() {
        if(conf.getScale()>0)
            return toEngineeringUnits(getDeviceMaximumValue());
        else
            return toEngineeringUnits(getDeviceMinimumValue());
    }
    
    public double getDeviceMinEngineeringUnits() {
        if(conf.getScale()>0)
            return toEngineeringUnits(getDeviceMinimumValue());
        else
            return toEngineeringUnits(getDeviceMaximumValue());
    }
    public AbstractLink(LinkConfiguration conf){
        this.conf=conf;
        slaveLinks = conf.getSlaveLinks();
        if(slaveLinks.size()>0)
            System.out.println(conf.getName()+" has slaves: "+slaveLinks.size());
        for(LinkConfiguration c:slaveLinks){
            //generate the links
            getSlaveFactory().getLink(c);
        }
    }
    public void cacheTargetValue(){
        for(LinkConfiguration c:slaveLinks){
            //generate the links
            AbstractLink link = getSlaveFactory().getLink(c);
            link.cacheTargetValue();
        }
        cacheTargetValueDevice();
    }
    
    public abstract void cacheTargetValueDevice();
    
    public void flush(double time){
        for(LinkConfiguration c:slaveLinks){
            //generate the links
            AbstractLink link = getSlaveFactory().getLink(c);
            link.flush(time);
        }
        flushDevice(time);
    }
    
    public void flushAll(double time){
        for(LinkConfiguration c:slaveLinks){
            //generate the links
            AbstractLink link = getSlaveFactory().getLink(c);
            link.flushAll(time);
        }
        flushAllDevice(time);
    }
    
    public abstract void flushDevice(double time);
    
    public abstract void flushAllDevice(double time);
    
    public abstract double getCurrentPosition();
    
    public double toEngineeringUnits(double value){
        return ((value-getHome())*getScale());
    }
    
    public double toLinkUnits(double euValue){
        return  (euValue/getScale())+getHome();
    }
    
    public void addLinkListener(ILinkListener l){
        //Log.info("Adding link listener: "+l);
        if(getLinks().contains(l))
            return;
        getLinks().add(l);
    }
    
    public void removeLinkListener(ILinkListener l){
        //Log.info("Removing link listener: "+l);
        if(getLinks().contains(l))
            getLinks().remove(l);
        //throw new RuntimeException();
    }
    
    public void fireLinkListener(double linkUnitsValue){
        ArrayList<ILinkListener> links2 = getLinks();
        for (int i = 0; i < links2.size(); i++) {
            ILinkListener l = links2.get(i);
            //Log.info("Link Event, RAW="+linkUnitsValue);
            try {
                l.onLinkPositionUpdate(this,toEngineeringUnits(linkUnitsValue));
            }catch( Throwable t) {
                t.printStackTrace(System.out);
            }
        }
    }
    
    public void fireLinkLimitEvent(PIDLimitEvent e){
        for(ILinkListener l:getLinks()){
            //Log.info("Link Event, RAW="+linkUnitsValue);
            l.onLinkLimit(this, e);
        }
    }
    
    public void Home(){
        setTargetValue(getHome());
        cacheTargetValue();
    }
    
    public void incrementEngineeringUnits(double inc){
        setTargetEngineeringUnits(targetEngineeringUnits+inc);
    }
    
    public void setTargetEngineeringUnits(double pos) {
        //TickToc.tic("Nan check link "+getLinkConfiguration().getLinkIndex());
        if(new Double(pos).isNaN()) {
            new RuntimeException("Setpopint in setTargetEngineeringUnits can not be set to nan").printStackTrace();
            return;
        }
        //TickToc.tic("Nan check link done");
        targetEngineeringUnits = pos;
        double linkUnits = toLinkUnits(targetEngineeringUnits);
        //TickToc.tic("to link units");
        setPosition(linkUnits);
    }
 
    public void setCurrentEngineeringUnits(double angle) {
        double current = (double)(getCurrentPosition()-getHome());
        if(current != 0)
            conf.setScale(angle/current);
    }
    
    public double getCurrentEngineeringUnits(){
        double link = getCurrentPosition();
        if(new Double(link).isNaN())
            link=0;
        double back = toEngineeringUnits(link);
        //Log.info("Link space: "+link+" Joint space: "+back);
        return back;
    }
    
    public double getTargetEngineeringUnits() {
        return toEngineeringUnits(getTargetValue());
    }
    
    public double getMaxEngineeringUnits() {
        if(conf.getScale()>0)
            return toEngineeringUnits(getUpperLimit());
        else
            return toEngineeringUnits(getLowerLimit());
    }
    
    public double getMinEngineeringUnits() {
        if(conf.getScale()>0)
            return toEngineeringUnits(getLowerLimit());
        else
            return toEngineeringUnits(getUpperLimit());
    }
    
    public void setMinEngineeringUnits(double minLimit ) {
        if(conf.getScale()>0)
            conf.setLowerLimit( toLinkUnits(minLimit));
        else
            conf.setUpperLimit( toLinkUnits(minLimit));
    }
    
    public void setMaxEngineeringUnits(double maxLimit) {
        if(conf.getScale()>0)
            conf.setUpperLimit( toLinkUnits(maxLimit));
        else
            conf.setLowerLimit( toLinkUnits(maxLimit));
    }
    
    
    public double getMaxVelocityEngineeringUnits() {
        return Math.abs(toEngineeringUnits(conf.getUpperVelocity()));
    }
    public void setMaxVelocityEngineeringUnits(double max) {
        conf.setUpperVelocity(toLinkUnits(max)); 
    }
    
    public boolean isMaxEngineeringUnits() {
        if(getTargetValue() == getUpperLimit()) {
            return true;
        }
        return false;
    }
    
    public boolean isMinEngineeringUnits() {
        if(getTargetValue() == getLowerLimit()) {
            return true;
        }
        return false;
    }
    
    protected void setPosition(double val) {
        //if(getTargetValue() != val){
            setTargetValue(val);
        //}
        for(LinkConfiguration c:slaveLinks){
            //generate the links
            AbstractLink link = getSlaveFactory().getLink(c);
            link.cacheTargetValue();
        }
        cacheTargetValue();
    }
    
 
    
    protected void setTargetValue(double val) {
        //TickToc.tic("setTargetValue nan check");
 
        if(new Double(val).isNaN()) {
            new RuntimeException("Setpopint in virtual device can not be set to nan").printStackTrace();
            return;
        }
        //TickToc.tic("setTargetValue nan check done ");
        Log.info("Setting cached value :"+val);
        this.targetValue = val;
        for(LinkConfiguration c:slaveLinks){
            //generate the links
            AbstractLink link = getSlaveFactory().getLink(c);
            link.setTargetValue(targetValue);
        }
        //TickToc.tic("followers set ");
    
        double ub = getMaxEngineeringUnits();
        double lb = getMinEngineeringUnits();
        boolean flip = getScale()<0;
        boolean belowLower = targetValue<getLowerLimit();
        boolean aboveUpper = targetValue>getUpperLimit();
        String execpt = "Attempted="+toEngineeringUnits(targetValue)+" (engineering units) Device Units="+targetValue
                +" \nUpper Bound="+ub+" (engineering units) Device Units="+getUpperLimit()
                + "\nLower Bound="+lb+" (engineering units) Device Units="+getLowerLimit();
        if(flip?belowLower:aboveUpper){
            this.targetValue = flip?getLowerLimit():getUpperLimit();
            for(LinkConfiguration c:slaveLinks){
                //generate the links
                AbstractLink link = getSlaveFactory().getLink(c);
                link.setTargetValue(targetValue);
            }
            cacheTargetValue();
            fireLinkLimitEvent(
                    new PIDLimitEvent(
                            conf.getHardwareIndex(),
                            targetValue ,
                            PIDLimitEventType.UPPERLIMIT,
                            System.currentTimeMillis()
                            )
                    );
            if(isUseLimits())throw new RuntimeException("Joint hit Upper software bound\n"+execpt);
        }
        if(flip?aboveUpper:belowLower) {
            this.targetValue =flip?getUpperLimit():getLowerLimit();
            for(LinkConfiguration c:slaveLinks){
                //generate the links
                AbstractLink link = getSlaveFactory().getLink(c);
                link.setTargetValue(targetValue);
            }
            cacheTargetValue();
            fireLinkLimitEvent(
                    new PIDLimitEvent(
                            conf.getHardwareIndex(),
                            targetValue ,
                            PIDLimitEventType.LOWERLIMIT,
                            System.currentTimeMillis()
                            )
                    );
            if(isUseLimits())throw new RuntimeException("Joint hit Lower software bound\n"+execpt);
            
        }else{
            Log.info("Abstract Link: limits disabled");
        }
        //TickToc.tic("link bound set done");
    }
 
    public double getTargetValue() {
        return targetValue;
    }
    
    public void setUpperLimit(int upperLimit) {
        conf.setUpperLimit(upperLimit);
    }
    
    public void setLowerLimit(int lowerLimit) {
        conf.setLowerLimit(lowerLimit);
    }
    
    public void setHome(int home) {
        conf.setStaticOffset(home);
    }
 
    public void setScale(double d) {
        conf.setScale(d);
    }
 
    public double getScale() {
        return conf.getScale();
    }   
 
    public double getUpperLimit() {
        return (double) conf.getUpperLimit();
    }
 
    public double getLowerLimit() {
        return  conf.getLowerLimit();
    }
 
    public double getHome() {
        return  conf.getStaticOffset();
    }
    
    public void setCurrentAsUpperLimit() {
        conf.setUpperLimit(getCurrentPosition());
    }
    
    public void setCurrentAsLowerLimit() {
        conf.setLowerLimit(getCurrentPosition());
    }
 
    public void setUseLimits(boolean useLimits) {
        this.useLimits = useLimits;
    }
 
    public boolean isUseLimits() {
        return useLimits;
    }
 
    public void setLinkConfiguration(LinkConfiguration conf) {
        this.conf = conf;
    }
 
    public LinkConfiguration getLinkConfiguration() {
        return conf;
    }
 
    public ArrayList<ILinkListener> getLinks() {
        return links;
    }
 
    public void setLinks(ArrayList<ILinkListener> links) {
        this.links = links;
    }
 
    public void removeAllLinkListener() {
        links.clear();
    }
 
    public void setGlobalPositionListener(Object Object) {
//      if(!Affine.class.isInstance(Object)) {
//          RuntimeException runtimeException = new RuntimeException("Must be an Affine");
//          runtimeException.printStackTrace();
//          throw runtimeException;
//      }
        this.linksLocation = Object;
    }
 
    public Object getGlobalPositionListener() {
        return linksLocation;
    }
    public LinkFactory getSlaveFactory() {
        return slaveFactory;
    }
    public void setSlaveFactory(LinkFactory slaveFactory) {
        this.slaveFactory = slaveFactory;
    }
 
    public IMU getImu() {
        return imu;
    }
    public void addChangeListener(ILinkConfigurationChangeListener l) {
        conf.addChangeListener(l);
    }
    public void removeChangeListener(ILinkConfigurationChangeListener l) {
        conf.removeChangeListener(l);
    }
    public void clearChangeListener() {
        conf.clearChangeListener();
    }
}