SmartMotor.java

package robotCore;
 
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
 
import robotCore.Encoder.EncoderType;
 
// import javax.management.RuntimeErrorException;
 
// import robotCore.TwoWire.HardwareException;

public class SmartMotor extends MotorBase {
    public enum SmartMotorType {
        Unknown,
        PWM,
        Servo
    }

    public enum SmartMotorMode {
        Power,
        Speed,
        Position
    }
 
    private static int k_maxSpeed = 10000;  // Maximum allowable speed
 
    private class Processor {
        private int m_addr;
        private int m_nextMotor = 0;
        private SmartMotor[] m_motors = null;
        // private long m_validPins = 0;
        // private long m_validPWMPins = 0;
        // private int m_usedPins = 0;
        private int m_maxMotors = 0;
 
        Processor(int addr) {
            m_addr = addr;
        }
 
        void periodic() {
            // Logger.log("SmartMotor", 1, "periodic()");
            if (Device.m_boardVersion == 4) {
                // Does not support setting multiple values
                for (SmartMotor motor : m_motors) {
                    if ((motor != null) && motor.m_changed) {
                        // Logger.log("SmartMotor", 1, String.format("setMotor %d=%d", motor.m_motorNo, motor.m_currentValue));
                        m_device.sendCommand(m_i2cAddr, TwoWire.k_setMotor, (byte) motor.m_motorNo, (short) motor.m_currentValue);
                        motor.m_changed = false;
                    }
                }
            }
            else {
                int count = 0;
                for (SmartMotor motor : m_motors) {
                    if ((motor != null) && motor.m_changed) {
                        if (Math.abs(motor.m_currentValue) < 32000) {
                            count++;
                        }
                        else {
                            // Must send a 32 bit value
                            if (Device.m_boardSubVersion == 0) {
                                throw new TwoWire.HardwareException("Operation not supported by hardware: update firmware");
                            }
                            m_device.sendCommand(m_i2cAddr, TwoWire.k_setMotor32, (byte) motor.m_motorNo, motor.m_currentValue);
                        }
                    }
                }
 
                if (count != 0) {
                    byte[] data = new byte[(3 * count) + 1];
                    ByteBuffer buffer = ByteBuffer.wrap(data, 0, data.length);
                    buffer.order(ByteOrder.LITTLE_ENDIAN);
 
                    buffer.put((byte) count);
                    for (SmartMotor motor : m_motors) {
                        if ((motor != null) && motor.m_changed) {
                            // Logger.log("SmartMotor", 1, String.format("periodic: %d", motor.m_currentValue));
                            buffer.put((byte) motor.m_motorNo);
                            buffer.putShort((short) motor.m_currentValue);
 
                            motor.m_changed = false;
                        }
                    }
 
                    // Logger.log("SmartMotor", 1, String.format("Combine: count=%d, len=%d", count, data.length));
 
                    m_device.sendCommand(m_addr, TwoWire.k_setMultipleMotors, data);
                }
            }
        }
 
        private int getMaxMotors() {
            if (m_maxMotors == 0) {
                m_maxMotors = m_device.getByte(m_addr, TwoWire.k_getMaxMotors);
            }
 
            // System.out.println(String.format("maxMotors = %d", m_maxMotors));
 
            return (m_maxMotors);
        }
 
        private void verifyPin(int pin, boolean pwm) {
            // long validPins = pwm ? getValidPWMPins() : getValidOutputPins();
 
            // Logger.log("SmartMotor", 1, String.format("p=%d,v=%x,b=%x", pin, validPins, 1L << pin));
 
            // /*
            //  * Check if pin is valid for this processor
            //  */
            // if ((pin < 0) || (pin >= 64) || (((1L << pin) & validPins) == 0)) {
            //  throw new TwoWire.HardwareException(
            //          String.format("SmartMotor: Invalid%s pin: %d", pwm ? " PWM" : "", pin));
            // }
            if (!m_device.isPinValid(m_addr, pin, pwm ? Device.PinType.PWMPin : Device.PinType.OutputPin)) {
                throw new TwoWire.HardwareException(
                        String.format("SmartMotor: Invalid %s pin: %d", pwm ? "PWM" : "DIR", pin));
        }
 
            /*
             * Check if pin is currently in use
             */
            if (m_device.checkUsedPin(m_addr, pin)) {
                throw new TwoWire.HardwareException(String.format("SmartMotor: Pin %d is in use", pin));
            }
        }
    }
 
    static ArrayList<Processor> m_processors = new ArrayList<Processor>();
 
    static void periodic() {
        for (Processor processor : m_processors) {
            processor.periodic();
        }
    }
 
    Processor getProcessor(int addr) {
        Processor processor;
 
        for (int i = 0; i < m_processors.size(); i++) {
            processor = m_processors.get(i);
 
            if (processor.m_addr == addr) {
                return (processor);
            }
        }
 
        processor = new Processor(addr);
        m_processors.add(processor);
 
        return (processor);
    }
 
    private Processor m_processor;
    private Device m_device = null;
    // private ArduinoConnection m_arduino = null;
//  private SmartMotorType m_type = SmartMotorType.Unknown;
    private SmartMotorMode m_mode = SmartMotorMode.Power;
    private int m_motorNo;
    private int m_currentValue;
    private int m_i2cAddr = 0;
    private double m_maxSpeed = 0;
    private boolean m_changed = false;
 
    private Encoder m_encoder = null;
 
    private void init(SmartMotorType type, int pwmPin, int dirPin, int min, int zero, int max, int i2cAddr) {
        // if (RobotBase.TW)
        // {
        initTW(type, pwmPin, dirPin, min, zero, max, i2cAddr);
        // }
        // else
        // {
        // initArduino(type, pwmPin, dirPin, min, zero, max);
        // }
    }
 
    private void initTW(SmartMotorType type, int pwmPin, int dirPin, int min, int zero, int max, int i2cAddr) {
        if (pwmPin == dirPin) {
            throw new TwoWire.HardwareException(String.format("SmartMotor: The pwmPin and dirPin must be different"));
        }
 
        m_device = Device.getInstance();
        m_processor = getProcessor(i2cAddr);
        int maxMotors = m_processor.getMaxMotors();
 
        // System.out.println(String.format("SmartMotor init: addr = %d", i2cAddr));
 
        m_processor.verifyPin(pwmPin, true);
        if (dirPin != 0) {
            m_processor.verifyPin(dirPin, false);
        }
 
        if (((type != SmartMotorType.PWM) && (type != SmartMotorType.Servo))
                || (m_processor.m_nextMotor >= maxMotors)) {
            throw new TwoWire.HardwareException("Invalid motor param");
        }
 
        if (m_processor.m_motors == null) {
            m_processor.m_motors = new SmartMotor[maxMotors];
        }
 
//      m_type = type;
        m_motorNo = m_processor.m_nextMotor++;
        m_processor.m_motors[m_motorNo] = this;
        m_i2cAddr = i2cAddr;
 
        m_device.setUsedPin(m_i2cAddr, pwmPin);
 
        if (dirPin != 0) {
            m_device.setUsedPin(m_i2cAddr, dirPin);
        }
 
        switch (type) {
            case PWM:
                // SendCommand(String.format("m%dcp %d %d", m_motorNo, pwmPin, dirPin));
                m_device.sendCommand(m_i2cAddr, TwoWire.k_configureMotor, (byte) m_motorNo,
                        (byte) SmartMotorType.PWM.ordinal(), (byte) pwmPin, (byte) dirPin);
                break;
 
            // case Servo:
            // sendCommand(String.format("m%dcs %d %d %d %d", m_motorNo, pwmPin, min, zero,
            // max));
            // break;
 
            default:
                break;
        }
    }

    public SmartMotor(SmartMotorType type, int pwmPin, int dirPin) {
        init(type, pwmPin, dirPin, 1000, 1500, 2000, Device.k_mainTwoWireId);
    }
 
    // Used for creating DummyMotor
    public SmartMotor() {
 
    }

    public SmartMotor(SmartMotorType type, int pwmPin, int dirPin, int i2cAddr) {
        init(type, pwmPin, dirPin, 1000, 1500, 2000, i2cAddr);
    }

    public SmartMotor(int pin, int min, int zero, int max) {
        init(SmartMotorType.Servo, pin, 0, min, zero, max, Device.k_mainTwoWireId);
    }

    public SmartMotor(int pin, int min, int zero, int max, int i2cAddr) {
        init(SmartMotorType.Servo, pin, 0, min, zero, max, i2cAddr);
    }

    public void setMaxSpeed(double maxSpeed) {
        m_maxSpeed = maxSpeed;
    }

    public void set(double value) {
        int send = 0;
 
        switch (m_mode) {
            case Power:
                if (value > 1.0) {
                    value = 1.0;
                } else if (value < -1.0) {
                    value = -1.0;
                }
 
                send = (int) (value * 1000);
                break;
 
            case Speed:
                if (m_encoder == null) {
                    throw new TwoWire.HardwareException("Setting Speed with no encoder specified");
                }
                if (m_maxSpeed != 0) {
                    value *= m_maxSpeed;
                }
                // Since we are sending a 16 bit value we must make sure that the speed is not too large
                send = (int) ((value > k_maxSpeed) ? k_maxSpeed * Math.signum(value) : value);
                break;
 
            case Position:
                if (m_encoder == null) {
                    throw new TwoWire.HardwareException("Setting Position with no encoder specified");
                }
                if (m_encoder.getType() == EncoderType.AnalogRotational) {
                    value = Gyro.normalize(value + m_encoder.getZero(), m_encoder.getRange());
                }
                else {
                    value += m_encoder.getZero();
                }
                send = (int) (value * 10); // TODO - really should get rid of this x10
                break;
 
            default:
                Logger.log("SmartMotor", 3, "Invalid motor type: " + m_mode);
                return;
        }
 
        if (send != m_currentValue) {
            // Logger.log("SmartMotor", 1, String.format("setPower: %d", send));
            m_currentValue = send;
            m_changed = true;
        }
    }

    public void setFeedbackDevice(Encoder encoder) {
        m_encoder = encoder;
 
        // SendCommand(String.format("m%dd %d", m_motorNo, encoder.getEncoderNo()));
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setFeedbackDevice, (byte) m_motorNo, (byte) encoder.getEncoderNo());
    }

    public Encoder getFeedbackDevice() {
        return (m_encoder);
    }

    public void setControlMode(SmartMotorMode mode) {
        if (mode != m_mode) {
            m_mode = mode;
            m_currentValue = 10000;
            // SendCommand(String.format("m%dm %d", m_motorNo, mode.ordinal()));
            // Logger.log("SmartMotor", 1, String.format("m_mode = %d", m_mode.ordinal()));
            m_device.sendCommand(m_i2cAddr, TwoWire.k_setMotorMode, (byte) m_motorNo, (byte) m_mode.ordinal());
        }
    }

    public void setFTerm(double f) {
        // FloatParams params = new FloatParams(f);
 
        // SendCommand(String.format("m%df %d %d", m_motorNo, params.value,
        // params.divisor));
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDFTerm, (byte) m_motorNo, (float) f);
    }

    public void setITerm(double i) {
        // FloatParams params = new FloatParams(i);
 
        // SendCommand(String.format("m%di %d %d", m_motorNo, params.value,
        // params.divisor));
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDITerm, (byte) m_motorNo, (float) i);
    }

    public void setPTerm(double p) {
        // FloatParams params = new FloatParams(p);
 
        // SendCommand(String.format("m%dp %d %d", m_motorNo, params.value,
        // params.divisor));
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDPTerm, (byte) m_motorNo, (float) p);
    }

    public void setIZone(double z) {
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDIZone, (byte) m_motorNo, (float) z);
    }

    public void setInverted(boolean invert) {
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setMotorInverted, (byte) m_motorNo, (byte) (invert ? 1 : 0));
    }

    public void setDTerm(double d) {
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDDTerm, (byte) m_motorNo, (float) d);
    }
 
    // /**
    // *
    // * @param wrap - When using position control this specifies that the position
    // * wraps modulo wrap
    // */
    // public void setWrap(double wrap) {
    // m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDWrap, (byte) m_motorNo,
    // (float) wrap);
    // }

    public void setMinPower(double minPower) {
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDMinPower, (byte) m_motorNo, (float) minPower);
    }

    public void setDeadZone(double deadZone) {
        m_device.sendCommand(m_i2cAddr, TwoWire.k_setPIDDeadZone, (byte) m_motorNo, (float) deadZone);
    }
}