Integration of Subsystems on the Software level

This post we will be talking about integrating the all subsystems on software level. As a whole, we are using 3 arduino boards.

                                Board 1 - Robot Arm and Hand

                                Board 2 - IR Sensors on the Robot Hand

                                Board 3 - Glove (Vibrational Motors)

Shown is the overview of how the user control the robot remotely. Setting up the server is discussed in detail here.

Figure 1 - Overview

Thanks to Johnny-Five library integrating these subsystems was really easy. Using multiple boards in one Javascript is elaborated in this post. All three boards are synchronised so that they execute commands every 250ms.

Controlling the Arm and Hand together was unreliable as the leap motion data does not provide accurate results when the hand is near the limits. Only the results of finger tip positions were distorted. Distortion of the fingers doesn't effect on the Arm as we are only using the information on the palm to calculate the joint angles using Inverse Kinematics. As mentioned with the new leap motion skeletal tracking we can see how the leap motion sees our hand is. Low Confidence value means hand is more distorted. We clearly noticed this when our hand is in the limits of the leap motion. Confidence can be found using this code. 

Figure 2 - Distorted values. Low Confidence Value

	var Leap = require('leapjs');
	Leap.loop({enableGestures:true}), function(frame){
	console.log(frame.hands[0].confidence);
	});

view raw confidence.js hosted with ❤ by GitHub

When the hand is straight up above leap motion it gave a perfect confidence value of 1.

Figure 3- No distortion. Confidence = 1

With these results, we decided to control Arm and the hand in two modes. First the user controls the arm, with one hand. User moves the palm to the decided position so that the arm mimics the movement including the wrist gestures. Once that's done the user inputs the other hand (both hands enabled) and the robot hand is activated and the arm is disabled. This enables the user control the hand precisely even the arm the moved to its limits (even though we moved the arm to the limits of the leap motion). 

Overview of the software implementation is shown from this flowchart. 

Figure 4 - FlowChart

	board = new five.Board();
	board.on('ready', function() {
	enable = new five.Led(33);
	Motor_1a = new five.Led(45);
	Motor_1b = new five.Led(43);
	Motor_2a = new five.Led(41);
	Motor_2b = new five.Led(47);
	Motor_3a = new five.Led(39);
	Motor_3b = new five.Led(49);
	Motor_4Ra = new five.Led(51);
	Motor_4Rb = new five.Led(37);
	Motor_4La = new five.Led(35);
	Motor_4Lb = new five.Led(53);
	var servoThumb = new five.Servo({
	pin: 4,
	range: [ 20, 120 ],
	board: board
	});
	var servoIndex = new five.Servo({
	pin: 6,
	range: [ 20, 115 ],
	board: board
	});
	var servoMiddle = new five.Servo({
	pin: 5,
	range: [ 10, 125 ],
	board: board
	});
	var servoRing = new five.Servo({
	pin: 7,
	range: [ 10, 140 ],
	board: board
	});
	var servoPinky = new five.Servo({
	pin: 3,
	range: [ 25, 115 ],
	board: board
	});
	var basePot = new five.Sensor({
	pin: "A13",
	freq: 200,
	board: board
	});
	var shoulderPot = new five.Sensor({
	pin: "A12",
	freq: 200,
	board: board
	});
	var elbowPot = new five.Sensor({
	pin: "A11",
	freq: 200,
	board: board
	});
	var leftPot = new five.Sensor({
	pin: "A14",
	freq: 200,
	board: board
	});
	var rightPot = new five.Sensor({
	pin: "A15",
	freq: 200,
	board: board
	});
	leftPot.on("data",function(err, leftReading){
	leftValue = leftReading;
	});
	rightPot.on("data",function(err, rightReading){
	rightValue = rightReading;
	});
	basePot.on("read",function(err, value){
	baseValue = value;
	});
	shoulderPot.on("read",function(err, value){
	shoulderValue = value;
	});
	elbowPot.on("read",function(err, value){
	elbowValue = value;
	});
	//Initialize
	servoThumb.to(20);
	servoIndex.to(20);
	servoMiddle.to(10);
	servoRing.to(10);
	servoPinky.to(25);
	this.loop(200, function() {
	// moveBase(88,baseValue);
	if (handslength>0) {
	if (handslength>1){
	finalThumb = Math.round(map(finalThumb,0,40,20,120));
	finalIndex = Math.round(map(finalIndex,0,170,20,115));
	finalMiddle = Math.round(map(finalMiddle,0,170,10,125));
	finalRing = Math.round(map(finalRing,0,170,10,140));
	finalPinky = Math.round(map(finalPinky,0,170,25,115));
	servoThumb.to(finalThumb);
	servoIndex.to(finalIndex);
	servoMiddle.to(finalMiddle);
	servoRing.to(finalRing);
	servoPinky.to(finalPinky);
	motorStop();
	}
	else{
	if(theta2<=170 && theta2>=10){
	moveBase(theta2,baseValue);
	}
	else stopBase();
	if(!isNaN(angle2) && !isNaN(angle3)){
	if(angle2>=10 && angle2<=150){
	moveShoulder(angle2,shoulderValue);
	}
	else stopShoulder();
	if(angle3>=55 && angle3<=160){
	moverElbow(angle3,elbowValue);
	}
	else stopElbow();
	}
	else{
	motorStop();
	console.log("joint angles NAN value detected");
	}
	}
	}
	else{
	motorStop();
	}
	});
	});

view raw board1.js hosted with ❤ by GitHub

	board2 = new five.Board();
	board2.on('ready', function() {
	var Thumb = new five.Sensor({
	pin: "A0",
	freq: 250,
	board: board2
	});
	var Index = new five.Sensor({
	pin: "A1",
	freq: 250,
	board: board2
	});
	var Middle = new five.Sensor({
	pin: "A2",
	freq: 250,
	board: board2
	});
	var Ring = new five.Sensor({
	pin: "A3",
	freq: 250,
	board: board2
	});
	var Pinky = new five.Sensor({
	pin: "A4",
	freq: 250,
	board: board2
	});
	Thumb.on("data", function() {
	thumbvalue = this.value;
	// if (thumbinitial == 0) {
	// thumbinitial = thumbvalue;
	// }
	levelarray[0] = checklevel("T", thumbvalue);
	});
	Index.on("data", function() {
	indexvalue = this.value;
	// if (indexinitial == 0) {
	// indexinitial = indexvalue;
	// }
	levelarray[1] = checklevel("I", indexvalue);
	});
	Middle.on("data", function() {
	middlevalue = this.value;
	// if (middleinitial == 0) {
	// middleinitial = middlevalue;
	// }
	levelarray[2] = checklevel("M", middlevalue);
	});
	Ring.on("data", function() {
	ringvalue = this.value;
	// if (ringinitial == 0) {
	// ringinitial = ringvalue;
	// }
	levelarray[3] = checklevel("R", ringvalue);
	});
	Pinky.on("data", function() {
	pinkyvalue = this.value;
	// if (pinkyinitial == 0) {
	// pinkyinitial = pinkyvalue;
	// }
	levelarray[4] = checklevel("P", pinkyvalue);
	});
	this.loop(250, function() {
	// console.log("Initial values: " + thumbinitial + " " + indexinitial + " " + middleinitial + " " + ringinitial + " " + pinkyinitial);
	// console.log("DATA ARRAY: " + levelarray[0] + levelarray[1] + levelarray[2] + levelarray[3] + levelarray[4]);
	// console.log("Thumb: " + thumbvalue + " Index: " + indexvalue + " Middle: " + middlevalue + " Ring: " + ringvalue + " Pinky: " + pinkyvalue);
	});
	});

view raw board2.js hosted with ❤ by GitHub

	board3 = new five.Board();
	board3.on('ready', function() {
	var TMotor = new five.Motor(5);
	var IMotor = new five.Motor(6);
	var MMotor = new five.Motor(10);
	var RMotor = new five.Motor(9);
	var PMotor = new five.Motor(11);
	this.loop(250, function() {
	Thumb = levelarray[0];
	Index = levelarray[1];
	Middle = levelarray[2];
	Ring = levelarray[3];
	Pinky = levelarray[4];
	buzz(TMotor, Thumb);
	buzz(IMotor, Index);
	buzz(MMotor, Middle);
	buzz(RMotor, Ring);
	buzz(PMotor, Pinky);
	i++;
	});
	});

view raw board3.js hosted with ❤ by GitHub

	var Leap = require('leapjs');
	var five = require('johnny-five');
	var board, board2, board3;
	var frames = [];
	var p =0;
	var thumbState = false;
	var handslength = 0;
	var angles;
	var ThumbangleArray = new Array(0,0,0,0,0);
	var zaxisFingerArray = new Array(0,0,0,0,0);
	var finalThumb,finalIndex,finalMiddle,finalRing,finalPinky =0;
	var baseValue, shoulderValue, elbowValue;
	var theta2,angle2,angle3=0;
	var potPos;
	var Pitch,Roll;
	var pitchState;
	var rollState;
	var leftValue , rightValue;
	var basePotArray = new Array(0,0,0,0,0);
	var thumbvalue, indexvalue, middlevalue, ringvalue, pinkyvalue;
	var levelarray = [0,0,0,0,0];
	var Thumb, Index, Middle, Ring, Pinky;

view raw defineglobal.js hosted with ❤ by GitHub

	var controller = new Leap.Controller();
	controller.on('frame', function(frame) {
	handslength = frame.hands.length;
	if (handslength>0) {
	var hand = frame.hands[0];
	Pitch = Math.round(180 * (hand.pitch())/Math.PI);
	Roll = Math.round(180 * (hand.roll())/Math.PI);
	var handPosition = frame.hands[0].palmPosition;
	var v1 = hand.palmNormal;
	var v2 = hand.pointables[0].direction;
	var v3 = hand.pointables[1].direction;
	var v4 = hand.pointables[2].direction;
	var v5 = hand.pointables[3].direction;
	var v6 = hand.pointables[4].direction;
	// Calculating the bending angle
	angle_thumb = bendingAngle(v1,v2);
	angle_index = bendingAngle(v1,v3);
	angle_middle = bendingAngle(v1,v4);
	angle_ring = bendingAngle(v1,v5);
	angle_pinky = bendingAngle(v1,v6);
	// ///////////////////////////////////////////////////////////////////////////////////////////////
	for (var i = ThumbangleArray.length-1 ; i > 0; i--) {
	ThumbangleArray[i] = ThumbangleArray[i-1];
	};
	ThumbangleArray[0] = angle_thumb;
	if ( (ThumbangleArray[2]>30)&&((ThumbangleArray[2]-ThumbangleArray[1]) >20 )&&( ThumbangleArray [2] >= ThumbangleArray[1]) ) thumbState = true;
	else if (( ThumbangleArray[2]<20) && ((ThumbangleArray[1] - ThumbangleArray[2])>20) && (ThumbangleArray[1] >= ThumbangleArray[2] )) thumbState = false;
	if (thumbState == false) finalThumb = ThumbangleArray[1];
	else finalThumb = 40;
	if (v3[2] > 0) finalIndex = 180-angle_index;
	else finalIndex = angle_index;
	if (v4[2] > 0) finalMiddle = 180-angle_middle;
	else finalMiddle = angle_middle;
	if (v5[2] > 0) finalRing = 180-angle_ring;
	else finalRing = angle_ring;
	if (v6[2] > 0) finalPinky = 180-angle_pinky;
	else finalPinky = angle_pinky;
	angles = jointAngles(handPosition);
	theta2 = angles.a1;
	angle2 = angles.a2;
	angle3 = angles.a3;
	}
	frames.push(frame);
	});
	// Leap Motion connected
	controller.on('connect', function(frame) {
	console.log("Leap Connected.");
	setTimeout(function() {
	var time = frames.length/2;
	}, 200);
	});
	controller.connect();

view raw leapcontroller.js hosted with ❤ by GitHub

	function map( x, in_min, in_max, out_min, out_max){
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
	}
	function filter(values){
	var smoothing = 1;
	var value = values[0]; // start with the first input
	for (var i=1, len=values.length; i<len; ++i){
	var currentValue = values[i];
	value += (currentValue - value) / smoothing;
	values[i] = Math.round(value);
	}
	var sum = 0;
	for(var i = 0; i < values.length; i++){
	sum += parseInt(values[i], 10);
	}
	var avg = sum/values.length;
	return avg;
	}
	function bendingAngle(v1,v2){
	var dotProduct = v1[0]*v2[0] +v1[1]*v2[1] +v1[2]*v2[2];
	var v1normal = Math.sqrt(v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2]);
	var v2normal = Math.sqrt(v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2]);
	var angle = 90-((Math.acos(dotProduct/(v1normal*v2normal)))*180/Math.PI);
	return Math.round(angle);
	}
	function jointAngles(v){
	var a = 165;
	var b = 150;
	var x,y,c,B,C=0;
	var handX = -v[2];
	var handY = v[1];
	var handZ = v[0];
	var theta2_1 = Math.atan2(handX,handZ);
	var theta2_2= Math.abs(theta2_1*180/(Math.PI));
	var theta2 = Math.round(theta2_2);
	if(handX<0) x = 0;
	else x = Math.round(handX);
	if (handY<0) y=-150;
	else if(handY>450) y=300;
	else y = Math.round(handY)-150;
	c = Math.sqrt( x*x + y*y );
	B = (Math.acos( (b*b - a*a - c*c)/(-2*a*c) )) * (180/Math.PI);
	C = (Math.acos( (c*c - b*b - a*a)/(-2*a*b) )) * (180/Math.PI);
	var theta = (Math.asin( y / c )) * (180/Math.PI);
	var POT_2_Angle = B + theta;
	var POT_3_Angle = 180 - C;
	var subangle3 = -POT_3_Angle;
	var angle3 = -(Math.round(-55 + (1+0.005)*subangle3));
	var subangle2 = POT_2_Angle;
	var angle2 = -(Math.round(-155 + (1+0.005)*subangle2));
	return{
	a1: theta2,
	a2: angle2,
	a3: angle3
	}
	}
	function moveBase(theta2,baseValue){
	var angle = map(baseValue, 47,590,180,0);
	if (angle < theta2 - 5){
	enable.on();
	Motor_1a.on();
	Motor_1b.off();
	}
	else if (angle > theta2 + 5){
	enable.on();
	Motor_1a.off();
	Motor_1b.on();
	}
	else {
	Motor_1a.off();
	Motor_1b.off();
	}
	console.log("BASE >>>>> POT: " + angle + " LEAP: " + theta2 + " Value: " + baseValue);
	}
	function moveShoulder(angle2,shoulderValue){
	var angle = map(shoulderValue,550,940,10,150);
	if (angle > angle2 + 10){
	enable.on();
	Motor_2a.on();
	Motor_2b.off();
	}
	else if (angle< angle2 - 10){
	enable.on();
	Motor_2a.off();
	Motor_2b.on();
	}
	else {
	Motor_2a.off();
	Motor_2b.off();
	}
	console.log("SHOULDER>>>>>POT: " + angle + " LEAP: " + angle2 + " Value: " + shoulderValue);
	}
	function moverElbow(angle3,elbowValue){
	var angle = map(elbowValue,600,1023,55,160);
	if (angle < angle3 - 10){
	enable.on();
	Motor_3a.on();
	Motor_3b.off();
	}
	else if (angle >angle3 + 10){
	enable.on();
	Motor_3a.off();
	Motor_3b.on();
	}
	else {
	Motor_3a.off();
	Motor_3b.off();
	}
	console.log("ELBOW >>>>> LeapAngle: "+angle3+"\t"+"joint: "+angle+"\t"+"value: "+elbowValue);
	}
	function motorStop(){
	enable.off();
	Motor_1a.off();
	Motor_1b.off();
	Motor_2a.off();
	Motor_2b.off();
	Motor_3a.off();
	Motor_3b.off();
	}
	function stopBase(){
	Motor_1a.off();
	Motor_1b.off();
	}
	function stopShoulder(){
	Motor_2a.off();
	Motor_2b.off();
	}
	function stopElbow(){
	Motor_3a.off();
	Motor_3b.off();
	}
	function buzz(motor, level) {
	if (level == 3) {motor.start(255);}
	else if (level == 2) {motor.start(128);}
	else if (level == 1) {motor.start(64);}
	else {motor.stop();}
	}
	function checklevel(finger, value) {
	if (finger == "T") {
	if (value > 35) { return 3; }
	if (value < 35 && value > 17) {return 2; }
	if (value < 17 && value > 11) {return 1; }
	if (value < 11 && value > 9) {return 0; }
	else {return 0;}
	}
	else if (finger == "I") {
	if (value > 35) { return 3; }
	if (value < 35 && value > 15) {return 2; }
	if (value < 15 && value > 9) {return 1; }
	if (value < 9 && value > 6) {return 0; }
	else {return 0;}
	}
	else if (finger == "M") {
	if (value > 40) { return 3; }
	if (value < 40 && value > 20) {return 2; }
	if (value < 20 && value > 13) {return 1; }
	if (value < 13 && value > 8) {return 0; }
	else {return 0;}
	}
	else if (finger == "R") {
	if (value > 55) { return 3; }
	if (value < 55 && value > 25) {return 2; }
	if (value < 25 && value > 18) {return 1; }
	if (value < 18 && value > 12) {return 0; }
	else {return 0;}
	}
	else if (finger == "P") {
	if (value > 28) { return 3; }
	if (value < 28 && value > 11) {return 2; }
	if (value < 11 && value > 8) {return 1; }
	if (value < 8 && value > 5) {return 0; }
	else {return 0;}
	}
	else {
	return 0;
	}
	}

view raw otherfunctions.js hosted with ❤ by GitHub