Example Source Code:
RoboJDE Java Test Program
For the Unicoder ME-210


Description

This example shows a layered approach to controlling a Unicoder. At the lowest level, the Intellibrain I2CMaster class is used to send and receive byte arrays. These byte arrays in turn are filled and emptied with methods such as buildByteCmd(), buildWordCmd(), extractByteParam(), and extractWordParam(). At the next level, methods such as doSetWordCmd() and doGetWordCmd() actually perform the command specified in the cmdChar parameter. These methods are then used by the next level, the specific command handlers supported by the Unicoder. These include commands such as doSync(), getVelocity(), and so forth.

Finally, the main routine uses these to establish and confirm communication with the Unicoder, and demonstrate monitoring of distance, velocity, and angle.

 

Download

UnicoderTest.java - Java source
com/nubotics/ndi/Unicoder.java - Java source
Unicoder-RoboJDE.zip - RoboJDE Archive

 

Source Code


UnicoderTest.java


/*
 * Unicoder Test
 *
 * Copyright 2009 by Noetic Design, Inc.
 * www.nubotics.com
 *
 * Noetic Design grants you the right to use, modify, make derivative works and
 * redistribute this source file provided you do not remove this copyright notice.
 */

import java.lang.System;
import java.lang.Integer;
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.io.Display;
import com.ridgesoft.io.I2CMaster;
import com.nubotics.ndi.*;
import com.ridgesoft.robotics.*;
import com.ridgesoft.intellibrain.*;

/**
 * This class provides an example of the Nubotics Unicoder
 * encoder attached to the IntelliBrain.
 */
public class UnicoderTest {
    Unicoder iuc;
    BinaryCommandInterface bci;
    I2CSlaveDevice id;
    int slave_address;
    Display display;
    boolean short_mode;

/*
 * UnicoderTest constructor
 */

    UnicoderTest()
    {
        Log.log("---------------------------");
        Log.log("Unicoder Test");
        Log.log("Connecting to Unicoder at address 0x30");
        slave_address = (byte)0x30;
        try {
            id = new I2CSlaveDevice(slave_address, 400); // create I2C connection
            bci = new BinaryCommandInterface(id);
            iuc = new Unicoder(bci); // create I2C slave Unicoder
        } catch (Exception e) {
            e.fillInStackTrace();
            e.printStackTrace();
        }
    }

//********************************************
// Main Entry Point
//
//********************************************
    public static void main(String args[]) {

        try {
            long t;
            boolean st;
            UnicoderTest uct = new UnicoderTest();
            Unicoder uc = uct.iuc;

// connect to the Unicoder
            Log.log("Trying sync:");
            if (uc.doSync()) {
                Log.log("Got sync; trying echo:");

// confirm communications
                if (uc.doEcho((byte)0x5b)) {
                    Log.log("Echo worked.  Connected!");

// check device type
                    String name = uc.doName();
                    Log.log("Device name and version: " + name);
                    if (!name.equals(new String("NUc0E"))) {
                        Log.log("Wrong firmware version FAIL!");
                        System.exit(1);
                    }

// display useful info
                    byte mode = uc.doGetConstant((byte)2);
                    Log.log("Current mode = " + Integer.toHexString(mode & 0x0ff));

                    if ((mode & 0xfe) != 0x30) {
                        st = uc.doSetConstant((byte)2, (byte)0x31);
                        if (!st) {
                            Log.log("Failed to write mode!");
                            System.exit(1);
                        }
                        st = uc.doStoreConstants();
                        if (!st) {
                            Log.log("Failed to store constants!");
                            System.exit(1);
                        }
                        Log.log("Reset...");
                        uc.doReset();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1000)) {
                        }
                        uc.doSync();
                        mode = uc.doGetConstant((byte)2);
                        if (mode != 0x30) {
                            Log.log("Wrong mode FAIL!");
                            System.exit(1);
                        }
                        Log.log("Changed mode to 0x30 and stored.");
                    }
                    uc.doSync();

                    int status;
                    status = uc.getStatus() & 0x1f;
                    Log.log("Status: 0x" + Integer.toHexString(status));

                    Log.log("Getting AS5040 query register");
                    uc.getAS5040(uc.a);
                    Log.log("AS5040 P=" + uc.a.parity + " D=" + uc.a.magnitude_decrease + " I=" + uc.a.magnitude_increase + " L=" + 
                            uc.a.linearity_alarm + " C=" + uc.a.cordic_overflow + " O=" + uc.a.offset_compensation_finished + " A=" + uc.a.angle + " F=0x" + Integer.toHexString(uc.a.factory));
                    if ((uc.a.factory < 0x2001) || (uc.a.factory > 0x4001) || ((uc.a.factory & 0x8ffe) != 0)) {
                        Log.log("Invalid factory data!  FAIL!");
                        System.exit(1);
                    }

                    Log.log("Getting AS5040 programming register");
                    uc.getProg(uc.p);
                    Log.log("AS5040Prog M=" + uc.p.pmode + " D=" + uc.p.div + " I=" + uc.p.index + " Z=" + uc.p.z + " CCW=" + uc.p.ccw);

                    Log.log("Constants:");
                    for (int i = 0; i < 11; i++)
                        Log.log("Addr " + i + " = " + uc.doGetConstant((byte)i));

                    Log.log("---------------------------");

                    boolean aligment_mode = false;
                    int dist;
                    int old_dist = -100000;
                    int start_dist = uc.getDistance();

                    int min_alignment_filter = 0;
                    int ave_alignment_filter = 0;
                    int max_alignment_filter = 0;
                    int ave_alignment_sum = 0;
                    int alignment_sample = 0;
                    int total_samples = 0;
                    boolean looking_for_min = false;
                    boolean looking_for_max = false;
                    boolean found_min = false;
                    boolean found_max = false;
                    boolean started = false;
                    int cycle_length_sum = 0;
                    int prev_min_alignment_sample = 0;
                    int min_alignment_sample = 0;
                    int prev_max_alignment_sample = 0;
                    int max_alignment_sample = 0;
                    int cycle_length_count = 0;
                    int cycle_length;
                    int min_alignment = 1024;
                    int max_alignment = 0;
                    int ave_alignment = 0;
                    int recent_slope = 0;
                    int cycle = 0;
                    int recent_alignments[] = new int[4];
                    int recent_alignment_index = 0;
                    recent_alignments[0] = recent_alignments[1] = recent_alignments[2] = recent_alignments[3] = 0;

                    short old_angle = -1;
                    short angle;
                    short velocity;
                    short old_velocity = -10000;
                    short dir_change_count;
                    short old_dir_change_count = 0;
                    boolean stat;
                    int speed;
                    Servo s = IntelliBrain.getServo(1);
                    AnalogInput a = IntelliBrain.getThumbWheel();
                    ContinuousRotationServo crs = new ContinuousRotationServo(s, false);
                    //speed = ((a.sample() - a.getMaximum()/2) * 30) / (a.getMaximum()/2);
                    speed = 30;
                    crs.setPower(speed);
                    stat = uc.doDirChangeCount((short)0);

                    for (;;) {
                        status = uc.getStatus();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        velocity = uc.getVelocity();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        dir_change_count = uc.getDirChangeCount();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        dist = uc.getDistance();
                        dist -= start_dist;

                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        angle = uc.getAngle();
                        if (aligment_mode) {
                            total_samples++;
                            alignment_sample++;
                            ave_alignment_sum += angle;
                            if ((alignment_sample > 20) && !started) {
                                ave_alignment = ave_alignment_sum / alignment_sample;
                                started = true;
                                Log.log("Starting cycle analysis: average = " + ave_alignment);
                            }
                            if (angle < min_alignment) {
                                min_alignment = angle;
                                min_alignment_sample = total_samples;
                            }
                            if (angle > max_alignment) {
                                max_alignment = angle;
                                max_alignment_sample = total_samples;
                            }
                            recent_alignments[recent_alignment_index] = angle;
                            recent_alignment_index++;
                            if (recent_alignment_index > 3)
                                recent_alignment_index = 0;
                            recent_slope = (recent_alignments[3] - recent_alignments[0]) / 3;
                            if (started) { // we have a rough average value now
                                if (min_alignment < max_alignment) {
                                    if (angle < (ave_alignment - (ave_alignment - min_alignment)/2)) {
                                        looking_for_min = true;
                                    } else if (angle > (ave_alignment + (max_alignment - ave_alignment)/2)) {
                                        looking_for_max = true;
                                    }
                                }
                                if (looking_for_min && (recent_slope > 0)) {
                                    found_min = true;
                                    looking_for_min = false;
                                }
                                if (looking_for_max && (recent_slope < 0)) {
                                    found_max = true;
                                    looking_for_max = false;
                                }
                                if (found_min && found_max) {
                                    found_min = found_max = false;
                                    cycle++;
                                    cycle_length_count++;
                                    cycle_length_sum += max_alignment_sample - prev_max_alignment_sample;
                                    prev_max_alignment_sample = max_alignment_sample;
                                    cycle_length = cycle_length_sum / cycle_length_count;
                                    if (min_alignment_filter == 0)
                                        min_alignment_filter = min_alignment;
                                    else
                                        min_alignment_filter = (min_alignment + min_alignment_filter * 9) / 10;
                                    min_alignment = 1024;
                                    if (max_alignment_filter == 0)
                                        max_alignment_filter = max_alignment;
                                    else
                                        max_alignment_filter = (max_alignment + max_alignment_filter * 9) / 10;
                                    max_alignment = 0;
                                    ave_alignment = ave_alignment_sum / alignment_sample;
                                    if (ave_alignment_filter == 0)
                                        ave_alignment_filter = ave_alignment;
                                    else
                                        ave_alignment_filter = (ave_alignment + ave_alignment_filter * 9) / 10;
                                    ave_alignment_sum = ave_alignment_filter;
                                    alignment_sample = 0;
                                    Log.log("Cycle " + cycle + ", min " + min_alignment_filter + ", ave " + ave_alignment_filter + ", max " + max_alignment_filter + ", cycle len " + cycle_length + ", delta " + (max_alignment_filter - min_alignment_filter));
                                }
                            }
                        } else {
                            if ((velocity != old_velocity) || (dist != old_dist) || (angle != old_angle)) {
                                old_velocity = velocity;
                                old_dist = dist;
                                old_angle = angle;
                                Log.log("A" + angle + " V" + velocity + " D" + dist);
                                //Log.log("S" + speed + " V" + velocity + " D" + dist);
                            }

                            if (dir_change_count != old_dir_change_count) {
                                Log.log("Dir Change Count: " + dir_change_count);
                                //stat = uc.doDirChangeCount((short)0);
                                //dir_change_count = 0;
                                old_dir_change_count = dir_change_count;
                            }

                            if (dist > 10000) {
                                start_dist = uc.getDistance();
                                speed = -30;
                                crs.setPower(speed);
                            }
                            if (dist < -10000) {
                                Log.log("Dir Change Count: " + dir_change_count);
                                Log.log("PASS!");
                                uc.doCoast();
                                break;
                            }

                            if ((status & 0x0c) != 0) { // COF or LIN failure
                                Log.log("Status FAIL! 0x" + Integer.toHexString(status));
                                uc.doCoast();
                                break;
                            }

                            if (dir_change_count > 10) {
                                Log.log("Dir change FAIL!");
                                uc.doCoast();
                                break;
                            }

                            if ((dist > 200) && (velocity < 0)) {
                                Log.log("Forward FAIL!  Status: 0x" + Integer.toHexString(status));
                                uc.doCoast();
                                break;
                            } else if ((dist < -200) && (velocity > 0)) {
                                Log.log("Backward FAIL!  Status: 0x" + Integer.toHexString(status));
                                uc.doCoast();
                                break;
                            }
                        }
                        if (IntelliBrain.getStartButton().isPressed()) {
                            IntelliBrain.getStartButton().waitReleased();
                            angle = uc.getAngle();
                            uc.p.z += angle;
                            if (uc.p.z >= 1024)
                                uc.p.z -= 1024;
                            uc.doProg(uc.p); // when start pressed, reset index to this position
                            Log.log("- index set to " + uc.p.z + " -");
                            angle = uc.getAngle();
                            Log.log("-> A " + angle);
                            Log.log("Going to alignment mode; power cycle to leave this mode.");
                            if (uc.doAligmentMode())
                                aligment_mode = true;
                        }
                    }
                } else
                    Log.log("Echo failed.");
            } else
                Log.log("Sync failed.");
        } catch (Throwable t) {
            t.fillInStackTrace();
            t.printStackTrace();                                                        // Display a stack trace for any error
        }
    }
}


com/nubotics/ndi/Unicoder.java


/*
 * Unicoder
 *
 * Copyright 2009 by Noetic Design, Inc.
 * www.nubotics.com
 *
 * Noetic Design grants you the right to use, modify, make derivative works and
 * redistribute this source file provided you do not remove this copyright notice.
 */

package com.nubotics.ndi;
import com.nubotics.ndi.CommandInterface;
import com.nubotics.ndi.BinaryCommandInterface;
import com.nubotics.ndi.NdiCommander;
import com.nubotics.ndi.Log;

/**
 * This class provides an example of the Nubotics ME-210 
 * Unicoder intelligent encoder attached to the IntelliBrain. 
 */
public class Unicoder extends NdiCommander {

    public class AS5040 {
        public boolean parity;
        public boolean magnitude_decrease;
        public boolean magnitude_increase;
        public boolean linearity_alarm;
        public boolean cordic_overflow;
        public boolean offset_compensation_finished;
        public short angle;
        public short factory;
        public AS5040()
        {
            parity = false;
            magnitude_decrease = false;
            magnitude_increase = false;
            linearity_alarm = false;
            cordic_overflow = false;
            offset_compensation_finished = false;
            angle = 0;
            factory = 0;
        }
    }
    public AS5040 a;
    public class AS5040Prog {
        public byte pmode;
        public byte div;
        public boolean index;
        public short z;
        public boolean ccw;
        public AS5040Prog()
        {
            pmode = 0;
            div = 0;
            index = false;
            z = 0;
            ccw = false;
        }
    }
    public AS5040Prog p;

/*
 * Unicoder constructor
 */

    public Unicoder(CommandInterface com_int)
    {
        super(com_int);
        a = new AS5040();
        p = new AS5040Prog();
    }

    public Unicoder(BinaryCommandInterface com_int)
    {
        super(com_int);
        a = new AS5040();
        p = new AS5040Prog();
    }

    public boolean setShortMode(boolean sh_mode) throws Exception
    {
        byte mode = doGetConstant((byte)2);
        mode &= ~0x08;
        if (sh_mode)
            mode |= 0x08;
        return doSetConstant((byte)2, mode);
    }

    public boolean setI2CAddress(byte address)
    {
        return doSetConstant((byte)4, address);
    }

    public boolean doProg(AS5040Prog p) // byte pmode, byte div, boolean index, short z, boolean ccw)
    {
        int i, c;
        i = p.index ? 1 : 0;
        c = p.ccw ? 1 : 0;
        short prog = (short)((p.pmode & 0x03) | ((p.div & 0x03) << 2) | (i << 4) | ((p.z & 0x3ff) << 5) | (c << 15));
        if (doSetConstant((byte)5, (byte)(prog & 0x0ff)))
            return doSetConstant((byte)6, (byte)((prog >> 8) & 0x0ff));
        else
            return false;
    }

    public void getProg(AS5040Prog p) throws Exception
    {
        byte lo, hi;
        lo = doGetConstant((byte)5);
        hi = doGetConstant((byte)6);
        p.pmode = (byte)(lo & 0x03);
        p.div = (byte)((lo >> 2) & 0x03);
        p.index = (lo & 0x10) != 0 ? true : false;
        p.z = (short)(((lo >> 5) & 0x07) | ((hi & 0x7f) << 5));
        p.ccw = (hi & 0x80) != 0 ? true : false;
    }

    public void getAS5040(AS5040 a)
    {
        int ret = ci.doGetDWordCmd('Z');
        a.parity = (ret & 0x01) != 0 ? true : false;
        a.magnitude_decrease = (ret & 0x02) != 0 ? true : false;
        a.magnitude_increase = (ret & 0x04) != 0 ? true : false;
        a.linearity_alarm = (ret & 0x08) != 0 ? true : false;
        a.cordic_overflow = (ret & 0x10) != 0 ? true : false;
        a.offset_compensation_finished = (ret & 0x20) != 0 ? true : false;
        a.angle = (short)((ret >> 6) & 0x3ff); 
        a.factory = (short) ((ret >> 16) & 0xffff);
    }

    public int getStatus()
    {
        return ci.doGetWordCmd('S');
    }

    public int getTime()
    {
        return ci.doGetDWordCmd('T');
    }

    boolean doTime(int time)
    {
        return ci.doSetDWordCmd('T', time);
    }

    public boolean doSetConstant(byte address, byte value)
    {
        //Log.log("doSetConstant: both = " + both + ": 0x" + Integer.toHexString(both) + ", address = " + address + ", value = " + value);
        return ci.doSetByteByteCmd('F', address, value);
    }

    public byte doGetConstant(byte address) throws Exception
    {
        return ci.doGetByteByteCmd('F', address);
    }

    public boolean doStoreConstants()
    {
        return ci.doSetWordCmd('F', (short)0xffff);
    }

    public short getVelocity()
    {
        return ci.doGetWordCmd('V');
    }

    public int getPeriod()
    {
        return ci.doGetDWordCmd('P');
    }

    public short getDirChangeCount()
    {
        return ci.doGetWordCmd('Q');
    }

    public boolean doDirChangeCount(short count)
    {
        return ci.doSetWordCmd('Q', count);
    }

    public short getAngle()
    {
        ci.empty();
        return ci.doGetWordCmd('W');
    }

    public boolean doDistance(int distance)
    {
        return ci.doSetDWordCmd('X', distance);
    }

    public int getDistance()
    {
        ci.empty();
        return ci.doGetDWordCmd('D');
    }

    public boolean doCmd(char cmdChar, char expected)
    {
        for (int i = 1; i < 10; i++) {
            try {
                if (ci.doCmd(cmdChar, expected))
                    return true;
                Thread.sleep(1);
                doSync();
            } catch (Exception e) {
                //error = "doCmd Exception: " + e.getMessage();
                //Log.log(error);
            }
        }
        Log.log("bad doCmd return; cmd = " + cmdChar + "; expected " + expected);
        return false;
    }

    public boolean doCoast()
    {
        return doCmd('C', 'a');
    }

    public boolean doReset()
    {
        return doCmd('R', '\0');
    }

    public boolean doAligmentMode() // must power cycle to get out of this mode in the AS5040
    {
        return doCmd('A', 'a');
    }

}


 

 © 2004-2013 Noetic Design, Inc. All Rights Reserved. Nubotics, Unicoder, WheelCommander, and WheelWatcher are trademarks of Noetic Design, Inc.