Things used in this project

Custom parts and enclosures

P-CAD circuit
robotarm_ooGQ8UMLKN.sch

Schematics

Circuit of controlling a motor
Inbox  esvit666gmail com  2017 02 20 22 25 12 rj6h0fkqok

Code

Code snippet #1Plain text
/**
* 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];
}
Code snippet #2Plain text
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];
}
Code snippet #3Plain text
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);
}
Code snippet #4Plain text
/**
* 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);
});
}
Code snippet #5Plain text
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);
});
});
});
Code snippet #6Plain text
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);
Code snippet #7Plain text
<!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>

            
Code snippet #8Plain text
    <!-- 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>
            
Code snippet #9Plain text
    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;
    
Code snippet #10Plain text
    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]);
    
Github
https://github.com/elifTech/robotkit-tessel
Github
https://github.com/jcmellado/js-aruco

Credits

Thanks to esvit.

Replications

Did you replicate this project? Share it!

I made one

Love this project? Think it could be improved? Tell us what you think!

Give feedback

Comments

Similar projects you might like

MannyBot365
Intermediate
  • 2,271
  • 26

Home automation system.

MannyBot365

3 members

Access Control with Arduino
Intermediate
  • 10,686
  • 13

Work in progress

Project that integrates Arduino, RTC display with I2C, RFID reader and SD card module.

IOT Home Automation (Node MCU + BLYNK)
Intermediate
  • 4,613
  • 13

Control home appliances over internet. This IOT project done using Node MCU ESP8266 and BLYNK app. Control devices from anywhere.

Distinguish walking and running using Machine Learning
Intermediate
  • 2,650
  • 18

Full instructions

Gather data and develop algorithm to distinguish walking and running.

Water Pump Controller
Intermediate
  • 6,068
  • 12

Full instructions

Controlling water pump using on and off buttons.

Indoor Fire Pathmaker
Intermediate
  • 650
  • 8

Work in progress

Dynamically create safe paths within buildings that are on fire using the information collected from indoor moving sensors.

Add projectSign up / Login