by Vitalii Savchuk, Fullstack Senior Developer at ElifTech

This article was originally published in the Software Developer’s Journal on February 10, 2017

Nowadays we face a very interesting period of modern technologies development with already existing web sites and services for different kinds of activities. Each year there are less technologies, that can crucially change the work of developers comparing to 10-20 years ago. I can see that software development industry is growing in terms of human resources but the number of state-of-the-art technologies decreases. Of course an average developer doesn’t care much about this but most of the people who have chosen this profession are dreamers, inventors, creative creatures, – people who hate monotonous and boring work. I also belong to them, that is why one of my life priorities is continuous discovery of something new. Observing the tendencies of big and famous companies development becomes obvious that priority goes to IoT technologies, artificial intelligence, robotechnics etc. These are technologies that allow us not only to visualize an information but also to feel and look at the real world. Turn around! Can you see a robot somewhere? Does your T-shirt talk to you? No? Then this is the right time to dive into this technology and become one of the dreamers, who changes the world.

 

The idea that was pushing me forward was quite simple: “To create an Assistant that would make a cup of coffee for me.” Of course I will not highlight all the aspects of fully functional coffee making assistant development, but you will find out how to create a robot arm out of a toy and make it autonomous, programmable and with an access to internet.

In this article you will find out how to create a Robot Arm from a toy and how to program it with JavaScript. Most of the microcontrollers are oriented on servos and I will describe how connect them to DC-motors with the help of a board built on H-bridges. It is not necessary to have some deep knowledge in JavaScript in order to develop an arm, but if you would like to establish a set-up adaptor by yourself due to the scheme I have described, you should have some expertise in electronics and know how to use a solder. For sure I will describe where to order all the parts if you will find it hard to design them by yourself. I will also show how to control a Robot Arm remotely via WiFi and how to define an Arm condition for calibration over webcam.

Material resources selection

At the baseline of everything is a toy, that can be purchased on Amazon – OWI Robotic Arm Edge1 (pic. 1). It goes in a set with a control joystick and modification of a possible launching it to the computer via USB. But I have chosen the first alternative as far as I have planned to launch it myself to one of the existing microcontroller kits.

The lifting weight of this toy is 100 gr. Of course it will not lift a cup of coffee but it will manage to turn on the coffee machine and drop a couple of sugar cubes.

Pic. 1 OWI Robotic Arm Edge

There is a big amount of microcomputers: Raspberry Pi, Arduino, ODROID etc. But my attention was turned to a new microcomputer – Tessel 22 (pic.2), which allows programming on JavaScript and Rust. I have considered JavaScript as far as I use this technology both for frontend and backend at my full-time work. Due to its simplicity and cross platforming it is gaining popularity among developers which means that this will only increase an amount of people interested in this project.

Pic. 2 Tessel 2

With a set of board Tessel 2 there should be also ordered a module for motor operation Servo Module PCA96853 (pic. 3). It will control all the motors.

Pic. 3 Servo Module PCA9685

This module is designed for servo motors operations and our toy has DC-motors. To launch them you should additionally order DC-motor controllers. Unfortunately, I couldn’t find 1 controller that will fulfill all the requirements: starting current power 3.5A, voltage 5-9V that will control 5 motors at the same time. That is why I have decided to design this circuit myself. But if you lack expertise in scheme construction you can buy a ready one at internet shop, for e.g. Single R/C DC Motor Driver5

Method to connect Tessel 2 and Robot Arm

As a result of studying of an already existing solutions for launching DC-motors to circuit Tessel 2, I have not found an appropriate solution, that is why I will describe how to create it by yourself.

At first, we need to figure out what we are dealing with. Servo-module, that can be ordered together with a circuit Tessel 2, is made on the basis of a microchip PCA96854. The PCA9685 is an I²C-bus controlled 16-channel LED controller optimized for Red/Green/Blue/Amber (RGBA) color backlighting applications. Each LED output has its own 12-bit resolution (4096 steps) fixed frequency individual PWM controller that operates at a programmable frequency from a typical of 24 Hz to 1526 Hz with a duty cycle that is adjustable from 0% to 100% to allow the LED to be set to a specific brightness value. All outputs are set to the same PWM frequency. Consequently, each of the 16-channel tabs presents half H-bridge, and for controlling of motor rotations in both directions we need a full H-bridge (pic. 4).

Pic. 4 H-bridge

The basic operating mode of an H-bridge is fairly simple: if Q1 and Q4 are turned on (pic. 5), the left lead of the motor will be connected to the power supply, while the right lead is connected to ground. Current starts flowing through the motor which energizes the motor in (let’s say) the forward direction and the motor shaft starts spinning.

Pic. 5 Move forward

If Q2 and Q3 are turned on, the reverse will happen, the motor gets energized in the reverse direction, and the shaft will start spinning backwards.

Pic. 6 Move backward

In a bridge, you should never ever close both Q1 and Q2 (or Q3 and Q4) at the same time (pic. 7). If you did that, you just have created a really low-resistance path between power and GND, effectively short-circuiting your power supply. This condition is called ‘shoot-through’ and is an almost guaranteed way to quickly destroy your bridge, or something else in your circuit.

Pic. 7 Shoot-through state

So in order to implement full control of a motor we will use just two rows of pins. Final circuit of controlling a motor looks like this:

Pic. 8 Wiring a motor to the PCA9685

The finished board looks like this:

Pic. 9 Done scheme

Description of work with PCA9685

 

Technical writing of PCA96854 says this microcircuit supports I²C and uses 0x06 – 0x45 registers to set signals at the outputs. The function that returns registers addresses and their values to set motor start will look like this:

/**
* Return address of registers and value for set motor state
*
* @param pinIndex Pin index 1-10 (we have only 5 motors)
* @param value Value to set
* @return array
*/
function getChainValues(pinIndex, value) {
// pca9685 registers
var LED0_ON_L = 0x06;
var LED0_ON_H = 0x07;
var LED0_OFF_L = 0x08;
var LED0_OFF_H = 0x09;

// values
var convertOn = 0;
var convertOff = value;

var index = (pinIndex - 1) * 4;
var chain = [
LED0_ON_L + index, LED0_ON_H + index,
LED0_OFF_L + index, LED0_OFF_H + index
];
var values = [
convertOn, convertOn >> 8,
convertOff, convertOff >> 8
];
return [chain, values];
}

As I wrote before, we have to prevent the situation when motor gets immediately two signals “move forward” and “move backward”, so we will add this logic to the function.

var DIRECTION_NONE = -1;
var DIRECTION_FORWARD = 0;
var DIRECTION_BACKWARD = 1;

/**
* Return signals for setting motor state
* @param motorIndex Motor index
* @param direction Direction of rotate (DIRECTION_NONE, DIRECTION_FORWARD, DIRECTION_BACKWARD)
* @returns {*[]}
*/function getMotorState(motorIndex, direction) {
var MAX = 4096;
var chain = [], values = [];

var forwardIndex = motorIndex;
var backwardIndex = motorIndex + 1;
var chain1, chain2;

if (direction == DIRECTION_NONE) {
// stop all motors
chain1 = getChainValues(forwardIndex, 0);
chain2 = getChainValues(backwardIndex, 0);
} else if (direction == DIRECTION_FORWARD) {
// stop backward and start forward
chain1 = getChainValues(backwardIndex, 0);
chain2 = getChainValues(forwardIndex, MAX / 2);
} else if (direction == DIRECTION_BACKWARD) {
// stop forward and start backward
chain1 = getChainValues(forwardIndex, 0);
chain2 = getChainValues(backwardIndex, MAX / 2);
}

chain = chain.concat(chain1[0]);
values = values.concat(chain1[1]);

chain = chain.concat(chain2[0]);
values = values.concat(chain2[1]);

return [chain, values];
}

Also I decided to save state of all motors at all time and send it at every state change of one of motors, so the function that changes state of a motor will look like this:

var MOTOR1 = 1;
var MOTOR2 = 3;
var MOTOR3 = 5;
var MOTOR4 = 7;
var MOTOR5 = 9;

var motorStates = {};
motorStates[MOTOR1] = DIRECTION_NONE;
motorStates[MOTOR2] = DIRECTION_NONE;
motorStates[MOTOR3] = DIRECTION_NONE;
motorStates[MOTOR4] = DIRECTION_NONE;
motorStates[MOTOR5] = DIRECTION_NONE;

/**
* Write states of all motors into controller
* @param states
*/function writeValues(states) {
var chain = [], values = [];
for (var motorIndex in states) {
if (!states.hasOwnProperty(motorIndex)) {
return;
}

var chains = getMotorState(parseInt(motorIndex), states[motorIndex]);

chain = chain.concat(chains[0]);
values = values.concat(chains[1]);
}
servo._chainWrite(chain, values);
}

/**
* Change motor state
*
* @param motorIndex Motor index (1-5)
* @param direction Direction of rotate (DIRECTION_NONE, DIRECTION_FORWARD, DIRECTION_BACKWARD)
*/function setMotorState(motorIndex, direction) {
motorStates[motorIndex] = direction;
writeValues(motorStates);
}

function stopMotor(index) {
motorStates[index] = DIRECTION_NONE;
writeValues(motorStates);
}

Unlike servo motors DC-motors can not return for a certain angle, the only way to control them is time, so for convenience I made a function that sets the state of the engine and waits some time before the next action.

/**
* Change motor state and wait some time
*
* @param motorIndex Motor index (1-5)
* @param direction Direction of rotate (DIRECTION_NONE, DIRECTION_FORWARD, DIRECTION_BACKWARD)
* @param time Time to wait
* @returns {Promise}
*/function setMotorStateForTime(motorIndex, direction, time) {
return new Promise(function (resolve) {
setMotorState(motorIndex, direction);

setTimeout(resolve, time);
});
}

That’s all, with help of this simple function now we can build a complex algorithm. For example, algorithm that moves the hand forward and then backward during 1 second looks like:

servo.on('ready', function () {
var time = 1000;

setMotorStateForTime(MOTOR2, DIRECTION_FORWARD, time).then(function () {
console.info('Move M2 forward');
setMotorStateForTime(MOTOR2, DIRECTION_BACKWARD, time).then(function () {
console.info('Move M2 backward');

stopMotor(MOTOR2);
});
});
});

With the next step I have decided to make it possible to operate the Arm with frontend. For this I have launched Socket.IO6:

var fs = require('fs');
var socketIo = require('socket.io');

var server = http.createServer(function (request, response) {
response.writeHead(200, {"Content-Type": "text/html"});

// Use fs to read in index.html
fs.readFile(__dirname + '/index.html', function (err, content) {
// If there was an error, throw to stop code execution
if (err) { throw err; }

// Serve the content of index.html read in by fs.readFile
response.end(content);
});
});

var io = socketIo(server);

io.on('connection', function(socket){
socket.on('setMotorStateForTime', setMotorStateForTime);
socket.on('setMotorState', setMotorState);
socket.on('stopMotor', stopMotor)
});
server.listen(80);

For convenience I have decided to use AngularJS7. Index.html file looks as follows:

<!doctype html>
<html lang="en">
<head>
    <meta charset="UTF-8">

    < script src="angular.min.js">< /script>
    <script src="socket.io.js">< /script>

    <link rel="stylesheet" href="https://maxcdn.bootstrapcdn.com/bootstrap/3.3.7/css/bootstrap.min.css">

            < script>
    var MOTOR1 = 1;
    var MOTOR2 = 3;
    var MOTOR3 = 5;
    var MOTOR4 = 7;
    var MOTOR5 = 9;

    var DIRECTION_NONE = -1;
    var DIRECTION_FORWARD = 0;
    var DIRECTION_BACKWARD = 1;


    var app = angular.module('app', [])
    app.controller('MainCtrl', function ($scope) {
        var socket = io('http://192.168.1.101'); // Tessel wifi address

        var time = 100;

        $scope.DIRECTION_FORWARD = DIRECTION_FORWARD;
        $scope.DIRECTION_BACKWARD = DIRECTION_BACKWARD;
        $scope.DIRECTION_NONE = DIRECTION_NONE;

        $scope.motorStates = {};
        $scope.motorStates[MOTOR1] = DIRECTION_NONE;
        $scope.motorStates[MOTOR2] = DIRECTION_NONE;
        $scope.motorStates[MOTOR3] = DIRECTION_NONE;
        $scope.motorStates[MOTOR4] = DIRECTION_NONE;
        $scope.motorStates[MOTOR5] = DIRECTION_NONE;

        $scope.motors = [
            MOTOR1,
            MOTOR2,
            MOTOR3,
            MOTOR4,
            MOTOR5
        ];

        socket.on('motorStops', function (index) {
            $scope.motorStates[index] = DIRECTION_NONE;
        });

        $scope.forward = function (index) {
            $scope.motorStates[index] = DIRECTION_FORWARD;
            socket.emit('setMotorStateForTime', index, DIRECTION_FORWARD, time);
        };

        $scope.backward = function (index) {
            $scope.motorStates[index] = DIRECTION_BACKWARD;
            socket.emit('setMotorStateForTime', index, DIRECTION_BACKWARD, time);
        };

        $scope.stop = function (index) {
            $scope.motorStates[index] = DIRECTION_NONE;
            socket.emit('stopMotor', index);
        };
    })
    < /script>

    </head>
    <body ng-app="app" ng-controller="MainCtrl" style="padding: 40px;">


            
<div ng-repeat="(n, motor) in motors">
            Motor #{{n+1}}


    
<div class="btn-group" role="group">
            <button class="btn btn-default" ng-class="{'active': motorStates[motor] == DIRECTION_FORWARD}" ng-click="forward(motor)">forward
            </button>
            <button class="btn btn-default" ng-class="{'active': motorStates[motor] == DIRECTION_BACKWARD}" ng-click="backward(motor)">backward
            </button>
            <button class="btn btn-default" ng-class="{'active': motorStates[motor] == DIRECTION_NONE}" ng-click="stop(motor)">Stop
            </button>
            </div>


            <hr/>
            </div>



            </body>
            </html>

            

As you can see it from the code I am connecting to Tessel via WiFi, indicating its address 192.168.1.101. As a result, we have a web page where we can control all the engines of the Arm.

Pic. 10: Webpage for controlling the arm

With the next step I would like to show how to define a current position of an Arm with the help of a webcam, because this toy has usual DC-motors, that don’t show its current position.

For this I will use an opensource library ArUco8. ArUco is a minimal library for Augmented Reality applications based on OpenCv9. This library is ported on JavaScript – js-aruco10. Firstly, you need to print out markers, that will be placed on the Arm elements.

Pic. 11 Markers

These markers are similar to QR codes, but they are more simplified. Each of them contains its own individual code, that will help us differentiate dots from one another. Lets modificate our   index.html:

    <!-- put this in head -->
    < script src="cv.js"></ script>
            < script src="aruco.js"></ script>
            < script src="lodash.js"></ script>

            <!-- this in body -->
            <video id="video" autoplay="true" style="display:none;"></video>
            <canvas id="canvas" style="width:640px; height:480px;"></canvas>
            

 

Now we can write a script, that will identify markers in the video stream and draw a current position of a Robot Arm.


    var video, canvas, context, imageData, detector;

    function onLoad(){
        video = document.getElementById("video");
        canvas = document.getElementById("canvas");
        context = canvas.getContext("2d");

        canvas.width = parseInt(canvas.style.width);
        canvas.height = parseInt(canvas.style.height);

        navigator.getUserMedia = navigator.getUserMedia || navigator.webkitGetUserMedia || navigator.mozGetUserMedia;
        if (navigator.getUserMedia){
            function successCallback(stream){
                if (window.webkitURL) {
                    video.src = window.webkitURL.createObjectURL(stream);
                } else if (video.mozSrcObject !== undefined) {
                    video.mozSrcObject = stream;
                } else {
                    video.src = stream;
                }
            }
            function errorCallback(error){
            }

            navigator.getUserMedia({video: true}, successCallback, errorCallback);
            detector = new AR.Detector();
            requestAnimationFrame(tick);
        }
    }

    function tick(){
        requestAnimationFrame(tick);

        if (video.readyState === video.HAVE_ENOUGH_DATA){
            snapshot();

            // detect markers on video snapshot
            var markers = detector.detect(imageData);

            // find all point of arms
            var marker1 = _.find(markers, { id: 963 });
            var marker2 = _.find(markers, { id: 45 });
            var marker3 = _.find(markers, { id: 3 });
            var marker4 = _.find(markers, { id: 1001 });

            if (marker1 && marker2 && marker3 && marker4) {
                markers = [marker1, marker2, marker3, marker4];

                // draw all markers on canvas
                context.lineWidth = 10;
                context.strokeStyle = "lightgreen";
                context.beginPath();

                for (i = 0; i !== markers.length; ++ i){
                    var corner = markers[i].corners[0];
                    var nextMarker = markers[i + 1];

                    context.moveTo(corner.x, corner.y);
                    if (nextMarker) {
                        context.lineTo(nextMarker.corners[0].x, nextMarker.corners[0].y);
                    }
                }

                context.stroke();
                context.closePath();
            }
        }
    }
    // make snapshot from camera
    function snapshot(){
        context.drawImage(video, 0, 0, canvas.width, canvas.height);
        imageData = context.getImageData(0, 0, canvas.width, canvas.height);
    }
    window.onload = onLoad;
    

As a result, we will receive a process of a Robot Arm identification in the real time:

Pic. 12: Snapshot from recognizing script

With the help of these data we can define a slope angle of every part of the hand:

    function getAngle(p1, p2, p3) {
        var ap2 = { x: p2.x - p1.x, y: p2.y - p1.y };
        var cp2 = { x: p2.x - p3.x, y: p2.y - p3.y };
        // dot product
        var dot = (ap2.x * cp2.x + ap2.y * cp2.y);
        // length square of both vectors
        var abSqr = ap2.x * ap2.x + ap2.y * ap2.y;
        var cbSqr = cp2.x * cp2.x + cp2.y * cp2.y;
        // square of cosine of the needed angle
        var cosSqr = dot * dot / abSqr / cbSqr;
        // this is a known trigonometric equality:
        // cos(alpha * 2) = [ cos(alpha) ]^2 * 2 - 1
        var cos2 = 2 * cosSqr - 1;

        // Here's the only invocation of the heavy function.
        // It's a good idea to check explicitly if cos2 is within [-1 .. 1] range
        var alpha2 = (cos2 <= -1) ? Math.PI : (cos2 >= 1) ? 0 : Math.acos(cos2);
        var rslt = alpha2 / 2;
        var rs = rslt * 180 / Math.PI;
        return Math.round(rs * 100) / 100;
    }

    var angle1 = getAngle(marker1.corners[0], marker2.corners[0], marker3.corners[0]);
    var angle2 = getAngle(marker2.corners[0], marker3.corners[0], marker4.corners[0]);
    

That’s it. By using these data we can create different control scripts, calibration or define a starting position of an Arm.

Conclusion

In this article I have described ways to connect a toy Robot Arm to the microcontroller Tessel 2 and programme it. We have also examined how to launch a Socket.IO library in order to transfer all main logic to the frontend side, that will help us control an Arm remotely by connecting it to wifi and eliminate the need to download an upgraded programme to the microcontroller. Taking into consideration a simple construction of a toy, especially lack of possibiity to get a current position of each part, I have shown how to do this using simple webcam. Of course, an algorithm is quite simple, but this is only the beginning. The next steps are: movements kinematics, motion of an Arm from position A to position B and I was even thinking how to make movements calibration through genetic algorithm. As far as it is hard to include all these into one article, you can keep track of next development process on my repository: https://github.com/elifTech/robotkit-tessel

Interesting to read:

  1. ROBOT ARM TUTORIAL
  2. Complete Motor guide for Robotics
  3. Tessel SERVO
  4. Motion Planning for a Robot Arm by Using Genetic Algorithm
  5. How to Calculate a Robot’s Forward Kinematics in 5 Easy Steps
  6. Solving Kinematics Problems of a 6-DOF Robot Manipulator

References

  1. Robot Arm – Build Your Own Robotic Arm!
  2. Tessel2
  3. Tessel Servo Module
  4. Datasheet PCA9685
  5. Wasp 10A, 6.5V to 28V Single R/C DC Motor Driver
  6. SocketIO
  7. AngularJS
  8. ArUco: a minimal library for Augmented Reality applications based on OpenCV
  9. OpenCV
  10. js-aruco