Encoder.java

/*
 *    Copyright (C) 2016  John H. Gaby
 *
 *    This program is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, version 3 of the License.
 *
 *    This program is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *    
 *    Contact: robotics@gabysoft.com
 */
 
package robotCore;
 
import java.nio.ByteBuffer;
 
import robotCore.Device.Processor;
import robotCore.Device.RequestType;
 
//import robot.Robot;

public class Encoder {
    private Object m_lock = new Object();

 
    public enum EncoderType {
        Analog,

        Quadrature,

        Simple,

        PWMRotational,

        AnalogRotational,
        
        /*
         * The encoder is quadrature but measures position rather than speed
         */
        QuadraturePosition,
        
        /*
         * The encoder is attached to the microcontroller via i2c
         */
        I2CRotational
    }
 
    private Device m_device = null;
    private int m_encoderNo = -1;
    private int m_zeroPosition = 0;
    private int m_i2cAddr = 0;
    private Processor m_processor;
    private boolean m_group = false;
    private EncoderType m_type;
    private int m_range = 0;
 
    private Encoder m_encoder = null;
 
    private void getIntCallback(ByteBuffer buffer, Object object) {
        // Logger.log("Encoder", 1, "getIntCallback()");
 
        Encoder encoder = (Encoder) object;
        int pos = buffer.getInt();
 
        synchronized (m_lock) {
            encoder.m_lastPos = pos;
        }
    }
    
    public EncoderType getType() {
        return m_type;
    }
 
    // This function will be called from a different thread
    void groupCallback(ByteBuffer data, Object object) {
        Encoder encoder = (Encoder) object;
        byte pos = data.get();
 
        if (pos == -128) {
            // delta is too large, get full value
            // Logger.log("Encoder", 1, String.format("Encoder %d overflow", encoder.m_encoderNo));
            m_device.getInt(m_i2cAddr, TwoWire.k_getEncoderPos, (byte) encoder.m_encoderNo,
                    (b, o) -> getIntCallback(b, o), encoder);
        } else {
            synchronized (m_lock) {
                encoder.m_lastPos += pos;
            }
        }
    }
 
    private void verifyPin(int pin, Device.PinType type) {
        if (!m_device.isPinValid(m_i2cAddr, pin, type)) {
            throw new TwoWire.HardwareException(
                    String.format("Encoder: Invalid %s pin: %d", type.toString(), pin));
        }
 
        /*
         * Check if pin is currently in use
         */
        if (m_device.checkUsedPin(m_i2cAddr, pin)) {
            throw new TwoWire.HardwareException(String.format("Encoder: Pin %d is in use", pin));
        }
    }
 
    private void init(EncoderType type, int pin1, int pin2, int i2cAddr, boolean group) {
        if (pin1 == pin2) {
            throw new TwoWire.HardwareException(String.format("Encoder: The two pins must be different"));
        }
        
        if (type == EncoderType.QuadraturePosition && Device.m_boardSubVersion == 0) {
            throw new TwoWire.HardwareException("The hardware does not support positional quadrature encoders, update firmware");
        }
 
        m_device = Device.getInstance();
 
        m_processor = m_device.getProcessor(i2cAddr);
 
        m_i2cAddr = i2cAddr;
        m_group = group;
        m_type = type;
 
        /*
         * MUSTFIX - Probably need to get range after encoder has been initialized?
         */
        if ((type == EncoderType.AnalogRotational) || (type == EncoderType.I2CRotational)) {
            m_range = m_device.getEncoderRange(m_i2cAddr);
        }
 
        /*
         * For I2C encoders, pin1 is the i2c address. For all others, we must verify pin numbers
         */
        if (type != EncoderType.I2CRotational) {
            verifyPin(pin1, (type == EncoderType.AnalogRotational) ? Device.PinType.AnalogPin : Device.PinType.InterruptPin);
            m_device.setUsedPin(m_i2cAddr, pin1);
    
            if (pin2 >= 0) {
                // m_processor.verifyPin(pin2, false); // type == EncoderType.Analog);
                verifyPin(pin2, (type == EncoderType.AnalogRotational) ? Device.PinType.AnalogPin : Device.PinType.InputPin);
                m_device.setUsedPin(m_i2cAddr, pin2);
            }
        }
 
        m_encoderNo = m_processor.getNextEncoderNo();
 
        m_device.sendCommand(m_i2cAddr, TwoWire.k_configureEncoder, (byte) m_encoderNo, (byte) type.ordinal(),
                (byte) pin1, (byte) pin2);
 
        if (group) {
            m_processor.addGroupRequest(RequestType.EncoderRelativePosition, m_encoderNo, (d, c) -> groupCallback(d, c),
                    this);
            Device.startGroupRequests();
        }
    }
 
    // To support DummyEncoder
    public Encoder() {
    }

    public Encoder(EncoderType type, int pin1, int pin2, int i2cAddr) {
        init(type, pin1, pin2, i2cAddr, false);
    }

    public Encoder(EncoderType type, int pin1, int pin2, int i2cAddr, boolean group) {
        init(type, pin1, pin2, i2cAddr, group);
    }

    public Encoder(EncoderType type, int pin1, int pin2) {
        init(type, pin1, pin2, Device.k_mainTwoWireId, false);
    }

    public Encoder(EncoderType type, int pin1, int pin2, boolean group) {
        init(type, pin1, pin2, Device.k_mainTwoWireId, group);
    }

    public int getRange() {
        return m_range;
    }
 
    // ! @cond PRIVATE
    public int getEncoderNo() {
        return (m_encoderNo);
    }
    // ! @endcond

    public int get() {
        return getPosition();
    }
 
    int m_lastPos = 0;
 
    private int getRawPosition() {
        if (m_encoder != null) {
            return (m_encoder.getRawPosition());
        }
 
        if (!m_group) {
            m_lastPos = m_device.getInt(m_i2cAddr, TwoWire.k_getEncoderPos, (byte) m_encoderNo);
        }
 
        synchronized (m_lock) {
            return (m_lastPos);
        }
    }

    public int getPosition() {
        int pos = getRawPosition() - m_zeroPosition;
 
        if (m_range != 0) {
            pos = (int) Gyro.normalize(pos, m_range);
        }
 
        return pos;
    }

    public int getSpeed() {
        return getSpeedTW();
    }
 
    private int getSpeedTW() {
        if (m_encoder != null) {
            return (m_encoder.getSpeed());
        }
 
        return (m_device.getShort(m_i2cAddr, TwoWire.k_getEncoderSpeed, (byte) m_encoderNo));
    }

    public void reset() {
        m_zeroPosition = getRawPosition();
    }

    public void reset(int position) {
        m_zeroPosition = getRawPosition() - position;
    }

    public void setZero(int zero) {
        m_zeroPosition = zero;
    }
 
    public int getZero() {
        return m_zeroPosition;
    }

    public void setInverted(boolean invert) {
        Logger.log("Encoder", 1, String.format("setInverted(%b)", invert));
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setEncoderInverted, (byte) m_encoderNo, (byte) (invert ? 1 : 0));
    }
}