Управление роботом-рукой через Web-интерфейс

Демонстрация

Интерфейс пользователя

Робот-рука содержит в себе 6 моторов-сервоприводов:

Зона A: Управление моторами 2, 3, 4 (3 шарнирных соединения руки)
Зона B: Управление мотором 1 (поворотная основа)
Зона C: Управление мотором 5 (управление поворотом захвата)
Зона D: Управление мотором 6 (Управление захватом)

Архитектурное решение

Структура программы

Сторона клиента (Web-интерфейс, написанный на JavaScript)

Когда пользователь воздействует на экран: прикасается пальцами или двигает мышью, мы можем получить координату точки прикосновения (x, y). При этом структура программы выглядит следующим образом:

В случае расположения курсора или пальца на экране в зоне А, необходимо рассчитать углы поворота для двигателей 2, 3, 4. При этом нужно выполнить геометрический расчет. Вернемся к этому чуть позже.

Сторона сервера (код Arduino):

Получив набор углов от клиента, шесть двигателей поворачиваются на заданный угол поворота. Шесть двигателей могут поворачиваться на заданный угол одновременно. Прежде чем подробно рассказать о том, как управлять всеми двигателями, давайте посмотрим, как управлять одним двигателем. Предположим, что мы хотим повернуть двигатель с текущего угла (angle) на новый угол (new_angle). Поскольку скорость двигателя высока, мы должны замедлить ее. Чтобы сделать это, два следующих шага повторяются до тех пор, пока двигатель не достигнет нового угла:

Мотор поворачивается на небольшой угол.
Делается пауза и снова поворот на небольшой угол.

Следующая иллюстрация показывает приведенную выше схему, если новый угол больше, чем угол текущий:

Здесь step_num — число шагов двигателя, которые необходимо сделать до поворота на заданный угол. step и time — предопределенные значения, которые определяют плавность и скорость перемещения.

Вышеуказанный алгоритм позволяет управлять только одним двигателем. Чтобы двигатели начали поворачиваться на указанные углы одновременно, можно сделать следующее: все шесть двигателей используют одно и тоже значение step_num, но шаг каждого двигателя отличается друг от друга. Поэтому мы должны выбрать максимальное значение step_num для всего проекта.

Структура программы при этом может выглядеть следующим образом:

Геометрические вычисления

Ранее мы указывали о необходимости производить геометрические вычисления. Рассмотрим следующую задачу:

Известно следующее:

Точка С фиксирована;
Известно положение точки D — вводит пользователь;
Известно расстояние CB, BA, AD (обозначим соответственно b, a, d);
Обозначим углы между сегментами руки-робота C, B, A;
Предположим что углы B и A равны;
Добавим дополнительные точки с сегмент.

Вычисление:

Мы знаем точки C и D => мы можем вычислить длину отрезка DC (обозначим ее буквой c);
Мы также можем вычислить угол δ;
Рассмотрим треугольник ABE, можно сказать, что он равнобедренный и AE = BE, ∠E = π — 2α;
Таким образом, получаем:

По теореме косинусов для треугольника имеем:

Подставив выражения (1) и (2) в (3), мы получаем:

Упростив выражение, получаем:

Поскольку мы знаем значения a, b, c и d, решаем квадратичное уравнение относительно α и β = π — α. Таким образом мы нашли β. Теперь найдем γ. По закону косинусов в треугольниках BDC и BDA:

Решив систему уравнений, можем вычислить γ;
Таким образом, вычисленные углы (δ + γ), β и β и являются нашими искомыми углами. Это углы для двигателей 2, 3 и 4 соответственно.

Код прошивки

Код прошивки включает два файла:

RobotArmWeb.ino: код Arduino
remote_arm.php: код веб-приложения, который загружается в плату PHPoC WiFi Shield или PHPoC Shield.

Вам также необходимо загрузить файл изображения flywheel.png в PHPoC Shield

RobotArmWeb.ino

#include "SPI.h"
#include "Phpoc.h"
#include <Servo.h>

int angle_init[]	= {90, 101, 165, 153, 90, 120}; // when motor stands straight. In web, the angle when motor stands straight is {0, 90, 130, 180, 0, 0};
int angle_offset[]	= {0, 11, -15, -27, 0, 137}; // offset between real servo motor and angle on web
int cur_angles[]	= {90, 101, 165, 153, 90, 120}; // current angles of six motors (degree) 
int dest_angles[]	= {0, 0, 0, 0, 0, 0}; // destined angles
int angle_max[]		= {180, 180, 160, 120, 180, 137};
int angle_min[]		= { 0, 0, 0, 20, 0, 75};
int direction[]		= {1, 1, 1, 1, 1 ,-1};
int angleSteps[]	= {3, 2, 2, 2, 4 ,4}; // moving steps of each motor (degree)

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

Servo servo[6] = {servo1, servo2, servo3, servo4, servo5, servo6};

PhpocServer server(80);
PhpocClient client;

int stepNum = 0;

void setup(){
	Serial.begin(9600);

	Phpoc.begin(PF_LOG_SPI | PF_LOG_NET);
	server.beginWebSocket("remote_arm");

	servo1.attach(2);  // attaches the servo on pin 2 to the servo object
	servo2.attach(3);  // attaches the servo on pin 3 to the servo object
	servo3.attach(4);  // attaches the servo on pin 4 to the servo object
	servo4.attach(5);  // attaches the servo on pin 5 to the servo object
	servo5.attach(6);  // attaches the servo on pin 6 to the servo object
	servo6.attach(7);  // attaches the servo on pin 7 to the servo object

	for(int i = 0; i < 6; i++)
		servo[i].write(angle_init[i]);
}

void loop() {
	PhpocClient client = server.available();

	if(client) {
		String angleStr = client.readLine();

		if(angleStr) {
			Serial.println(angleStr);
			int commaPos1 = -1;
			int commaPos2;

			for(int i = 0; i < 5; i++) {
				commaPos2 = angleStr.indexOf(',', commaPos1 + 1);
				int angle = angleStr.substring(commaPos1 + 1, commaPos2).toInt();
				dest_angles[i] = angle * direction[i] + angle_offset[i];
				commaPos1 = commaPos2;
			}

			int angle5 = angleStr.substring(commaPos1 + 1).toInt();
			dest_angles[5] = angle5 * direction[5] + angle_offset[5];

			stepNum = 0;

			// move motors in many small steps to make motor move smooth, avoiding move motor suddently. The below is step calculation
			for(int i = 0; i < 6; i++) {
				int dif = abs(cur_angles[i] - dest_angles[i]);
				int step = dif / angleSteps[i];

				if(stepNum < step)
					stepNum = step;
			}
		}
	}

	// move motors step by step
	if(stepNum > 0) {
		for(int i = 0; i < 6; i++) {
			int angleStepMove = (dest_angles[i] - cur_angles[i]) / stepNum;
			cur_angles[i] += angleStepMove;

			if(cur_angles[i] > angle_max[i])
				cur_angles[i] = angle_max[i];
			else if(cur_angles[i] < angle_min[i])
				cur_angles[i] = angle_min[i];

			servo[i].write(cur_angles[i]);
		}

		stepNum--;
		delay(20);
	}
}

#include "SPI.h"

#include "Phpoc.h"

#include <Servo.h>

int angle_init[] = {90, 101, 165, 153, 90, 120}; // when motor stands straight. In web, the angle when motor stands straight is {0, 90, 130, 180, 0, 0};

int angle_offset[] = {0, 11, -15, -27, 0, 137}; // offset between real servo motor and angle on web

int cur_angles[] = {90, 101, 165, 153, 90, 120}; // current angles of six motors (degree)

int dest_angles[] = {0, 0, 0, 0, 0, 0}; // destined angles

int angle_max[] = {180, 180, 160, 120, 180, 137};

int angle_min[] = { 0, 0, 0, 20, 0, 75};

int direction[] = {1, 1, 1, 1, 1 ,-1};

int angleSteps[] = {3, 2, 2, 2, 4 ,4}; // moving steps of each motor (degree)

Servo servo1;

Servo servo2;

Servo servo3;

Servo servo4;

Servo servo5;

Servo servo6;

Servo servo[6] = {servo1, servo2, servo3, servo4, servo5, servo6};

PhpocServer server(80);

PhpocClient client;

int stepNum = 0;

void setup(){

Serial.begin(9600);

Phpoc.begin(PF_LOG_SPI | PF_LOG_NET);

server.beginWebSocket("remote_arm");

servo1.attach(2); // attaches the servo on pin 2 to the servo object

servo2.attach(3); // attaches the servo on pin 3 to the servo object

servo3.attach(4); // attaches the servo on pin 4 to the servo object

servo4.attach(5); // attaches the servo on pin 5 to the servo object

servo5.attach(6); // attaches the servo on pin 6 to the servo object

servo6.attach(7); // attaches the servo on pin 7 to the servo object

for(int i = 0; i < 6; i++)

servo[i].write(angle_init[i]);

}

void loop() {

PhpocClient client = server.available();

if(client) {

String angleStr = client.readLine();

if(angleStr) {

Serial.println(angleStr);

int commaPos1 = -1;

int commaPos2;

for(int i = 0; i < 5; i++) {

commaPos2 = angleStr.indexOf(',', commaPos1 + 1);

int angle = angleStr.substring(commaPos1 + 1, commaPos2).toInt();

dest_angles[i] = angle * direction[i] + angle_offset[i];

commaPos1 = commaPos2;

}

int angle5 = angleStr.substring(commaPos1 + 1).toInt();

dest_angles[5] = angle5 * direction[5] + angle_offset[5];

stepNum = 0;

// move motors in many small steps to make motor move smooth, avoiding move motor suddently. The below is step calculation

for(int i = 0; i < 6; i++) {

int dif = abs(cur_angles[i] - dest_angles[i]);

int step = dif / angleSteps[i];

if(stepNum < step)

stepNum = step;

}

// move motors step by step

if(stepNum > 0) {

for(int i = 0; i < 6; i++) {

int angleStepMove = (dest_angles[i] - cur_angles[i]) / stepNum;

cur_angles[i] += angleStepMove;

if(cur_angles[i] > angle_max[i])

cur_angles[i] = angle_max[i];

else if(cur_angles[i] < angle_min[i])

cur_angles[i] = angle_min[i];

servo[i].write(cur_angles[i]);

}

stepNum--;

delay(20);

}

remote_arm.php

<!DOCTYPE html>
<html>
<head>
<title>Arduino - Arm Robot - Web</title>
<meta name="viewport" content="width=device-width, initial-scale=0.9">
<style>
body { text-align: center; }
canvas { background-color: #FFFFFF; }
</style>
<script>
var MOTOR_1 = 0;
var MOTOR_2 = 1;
var MOTOR_3 = 2;
var MOTOR_4 = 3;
var MOTOR_5 = 4;
var MOTOR_6 = 5;

var ARM_LENGTH_1 = 95;
var ARM_LENGTH_2 = 88;
var ARM_LENGTH_3 = 155;

var SOLE_WIDTH = 160;
var SOLE_HEIGHT = 73;

var flywheel_img = new Image();
flywheel_img.src = "flywheel.png";

var zone_A_radius = ARM_LENGTH_1 + ARM_LENGTH_2 + ARM_LENGTH_3;

var pivot_x = zone_A_radius + 5;
var pivot_y = zone_A_radius + 5;

var zone_B_radius = 100;
var zone_B_center_x = pivot_x / 2;
var zone_B_center_y = -(zone_B_radius + SOLE_HEIGHT + 20);
var zone_B_last_angle = 0;

var zone_C_radius = 70;
var zone_C_center_x = -pivot_x / 2;
var zone_C_center_y = -(zone_B_radius + SOLE_HEIGHT + 20);
var zone_C_last_angle = 0;

var zone_D_width = 30;
var zone_D_height = 200.0;
var zone_D_center_x = 0;
var zone_D_center_y = -(zone_B_radius + SOLE_HEIGHT + 110);
var grip_height = 70;
var grip_width = 100;

var canvas_width = zone_A_radius * 2 + 10;
var canvas_height = zone_A_radius  + SOLE_HEIGHT + zone_B_radius * 2 + 180;

var grip_max_angle = 62.0; // 62 degree

var click_state = 0;
var mouse_xyra = {x:0, y:0, r:0.0, a:0.0};
var angles = [90, 90, 180, 180, 90, 17];

var ws = null;
var servo = null, ctx = null;

var last_time;

var a = 0, b = 0, c = 0;

function init()
{
	servo = document.getElementById("servo");
	servo.width = canvas_width;
	servo.height = canvas_height;

	servo.addEventListener("touchstart", mouse_down);
	servo.addEventListener("touchend", mouse_up);
	servo.addEventListener("touchmove", mouse_move);
	servo.addEventListener("mousedown", mouse_down);
	servo.addEventListener("mouseup", mouse_up);
	servo.addEventListener("mousemove", mouse_move);

	ctx = servo.getContext("2d");

	ctx.translate(pivot_x, pivot_y);
	ctx.rotate(Math.PI);

	// quadratic equation parameters
	a = 4*ARM_LENGTH_1*ARM_LENGTH_3;
	b = 2*(ARM_LENGTH_2*ARM_LENGTH_1 + ARM_LENGTH_2*ARM_LENGTH_3);
	c = Math.pow(ARM_LENGTH_1, 2) + Math.pow(ARM_LENGTH_2, 2)  + Math.pow(ARM_LENGTH_3, 2) - 2*ARM_LENGTH_1*ARM_LENGTH_3;
}
function update_view()
{
	ctx.clearRect(-canvas_width/2, -SOLE_HEIGHT, canvas_width, canvas_height);
	ctx.save();

	//draw zone A
	ctx.fillStyle = "#D9D9D9";
	ctx.beginPath();
	ctx.arc(0,0,zone_A_radius, 0, 2*Math.PI);
	ctx.fill();
	
	ctx.fillStyle = "#FFFFFF";
	ctx.fillRect(-pivot_x , -canvas_height + pivot_y, canvas_width, canvas_height - pivot_y - SOLE_HEIGHT);

	// draw arm segments
	ctx.strokeStyle="#00979D";
	ctx.fillStyle = "#FF4500";
	ctx.lineWidth = 20;
	ctx.rotate(angles[MOTOR_2] / 180 * Math.PI);
	ctx.beginPath();
	ctx.moveTo(0,0);
	ctx.lineTo(ARM_LENGTH_1,0);
	ctx.stroke();
	draw_pivot(0, 0);

	ctx.translate(ARM_LENGTH_1,0);
	ctx.rotate(-Math.PI + angles[MOTOR_3] / 180 * Math.PI);
	ctx.beginPath();
	ctx.moveTo(0,0);
	ctx.lineTo(ARM_LENGTH_2, 0);
	ctx.stroke();
	draw_pivot(0, 0);

	ctx.translate(ARM_LENGTH_2,0);
	ctx.rotate(-Math.PI + angles[MOTOR_4] / 180 * Math.PI);
	ctx.beginPath();
	ctx.moveTo(0,0);
	ctx.lineTo(ARM_LENGTH_3, 0);
	ctx.stroke();
	draw_pivot(0, 0);

	ctx.restore();

	ctx.strokeStyle = "#00979D";
	ctx.lineWidth = 6;

	// draw zone B
	angle = (angles[MOTOR_1] + 45) * Math.PI / 180;
	ctx.save();
	ctx.translate(zone_B_center_x, zone_B_center_y);
	ctx.rotate(angle);
	ctx.drawImage(flywheel_img, - zone_B_radius, - zone_B_radius, zone_B_radius * 2, zone_B_radius * 2);
	ctx.beginPath();
	ctx.arc(0, 0, zone_B_radius, 0, 2 * Math.PI);
	ctx.stroke();
	ctx.restore();

	// draw zone C
	angle = (angles[MOTOR_5] + 45) * Math.PI / 180;
	ctx.save();
	ctx.translate(zone_C_center_x, zone_C_center_y);
	ctx.rotate(angle);
	ctx.drawImage(flywheel_img, - zone_C_radius, - zone_C_radius, zone_C_radius * 2, zone_C_radius * 2);
	ctx.beginPath();
	ctx.arc(0, 0, zone_C_radius, 0, 2 * Math.PI);
	ctx.stroke();
	ctx.restore();

	// draw zone D
	ctx.fillStyle = "#00DEE6";
	ctx.lineWidth = 15;
	var grip_dist = Math.floor(angles[MOTOR_6] / grip_max_angle * zone_D_height);
	ctx.fillRect(zone_D_center_x - zone_D_width / 2, zone_D_center_y - zone_D_height / 2, zone_D_width, zone_D_height);

	ctx.lineWidth = 1;
	ctx.strokeStyle = "#FFFFFF";

	var center_x = zone_D_center_x;
	var center_y = zone_D_center_y + grip_dist / 2;

	var grd = ctx.createLinearGradient(center_x, center_y, center_x, center_y  + grip_height);
	grd.addColorStop(0,"#004A4D");
	grd.addColorStop(1,"#b3fcff");
	ctx.fillStyle = grd;
	ctx.beginPath();
	ctx.moveTo(center_x + grip_width / 2, center_y);
	ctx.bezierCurveTo(center_x + grip_width / 4, center_y + grip_height, center_x - grip_width / 4, center_y + grip_height,center_x - grip_width / 2, center_y);
	ctx.closePath();
	ctx.fill();
	ctx.stroke();

	center_x = zone_D_center_x;
	center_y = zone_D_center_y - grip_dist / 2;

	grd = ctx.createLinearGradient(center_x, center_y, center_x, center_y  - grip_height);
	grd.addColorStop(0,"#004A4D");
	grd.addColorStop(1,"#b3fcff");
	ctx.fillStyle = grd;
	ctx.beginPath();
	ctx.moveTo(center_x + grip_width / 2, center_y);
	ctx.bezierCurveTo(center_x + grip_width / 4, center_y - grip_height, center_x - grip_width / 4, center_y - grip_height,center_x - grip_width / 2, center_y);
	ctx.closePath();
	ctx.fill();
	ctx.stroke();

	ctx.restore();

	// draw sole
	ctx.fillStyle = "#006468";
	ctx.fillRect(-SOLE_WIDTH/2, -SOLE_HEIGHT, SOLE_WIDTH, SOLE_HEIGHT);
}
function event_handler(event)
{
	var x, y, r, alpha;
	// convert coordinate
	if(event.touches)
	{
		var touches = event.touches;

		x = (touches[0].pageX - touches[0].target.offsetLeft) - pivot_x;
		y = (touches[0].pageY - touches[0].target.offsetTop) - pivot_y;

	}
	else
	{
		x = event.offsetX - pivot_x;
		y = event.offsetY - pivot_y;
	}
	x = -x;
	y = -y;

	//check whether it's located in zone A or not
	r = Math.sqrt(x * x + y * y);

	if(r < zone_A_radius && y > -SOLE_HEIGHT)
	{
		if((x < SOLE_WIDTH/2) && (x > -SOLE_WIDTH/2) && (y < 0))
			return false;

		alpha = Math.atan2(y, x);

		if(alpha < 0)
			alpha += 2*Math.PI;

		mouse_xyra.x = x;
		mouse_xyra.y = y;
		mouse_xyra.r = r;
		mouse_xyra.a = alpha;

		if(geometric_calculation())
			return true;
	}

	//check whether it's located in zone B or not
	temp_x = x - zone_B_center_x;
	temp_y = y - zone_B_center_y;
	var distance = Math.sqrt(temp_x * temp_x + temp_y * temp_y);

	if(distance < zone_B_radius)
	{
		var angle = Math.atan2(temp_y, temp_x)* 180 / Math.PI;

		if(click_state == 0)
			zone_B_last_angle = angle;
		else
		{
			if((Math.abs(angle) > 90) && (angle * zone_B_last_angle < 0))
			{
				if(zone_B_last_angle > 0)
					zone_B_last_angle = -180;
				else
					zone_B_last_angle = 180;
			}

			angles[MOTOR_1] += Math.floor(angle - zone_B_last_angle);

			angles[MOTOR_1] = Math.max(0, angles[MOTOR_1]);
			angles[MOTOR_1] = Math.min(180, angles[MOTOR_1]);

			zone_B_last_angle = angle;
		}
		return true;
	}

	//check whether it's located in zone C or not
	temp_x = x - zone_C_center_x;
	temp_y = y - zone_C_center_y;
	var distance = Math.sqrt(temp_x * temp_x + temp_y * temp_y);

	if(distance < zone_C_radius)
	{
		var angle = Math.atan2(temp_y, temp_x)* 180 / Math.PI;

		if(click_state == 0)
			zone_C_last_angle = angle;
		else
		{
			if((Math.abs(angle) > 90) && (angle * zone_C_last_angle < 0))
			{
				if(zone_C_last_angle > 0)
					zone_C_last_angle = -180;
				else
					zone_C_last_angle = 180;
			}

			angles[MOTOR_5] += Math.floor(angle - zone_C_last_angle);

			angles[MOTOR_5] = Math.max(0, angles[MOTOR_5]);
			angles[MOTOR_5] = Math.min(180, angles[MOTOR_5]);

			zone_C_last_angle = angle;
		}
		return true;
	}

	//check whether it's located in zone D or not
	var temp_x = Math.abs(x - zone_D_center_x);
	var temp_y = Math.abs(y - zone_D_center_y);

	if(temp_x < (zone_D_width / 2) && temp_y < (zone_D_height / 2))
	{
		var angle = temp_y / (zone_D_height / 2) * grip_max_angle;
		angles[MOTOR_6] = Math.floor(angle);
		console.log(angles[MOTOR_6]);
		return true;
	}

	return false;
}
function geometric_calculation()
{
	var c_ = c - Math.pow(mouse_xyra.r, 2);
	var delta = b*b - 4*a*c_;
	if(delta < 0)
		return false; // no root

	var x1 = 0, x2 = 0;
	var x = 0;
	var cos_C = 0;
	var alpha = 0, beta = 0, gamma = 0;

	x1 = (-b + Math.sqrt(delta)) / (2*a);
	x2 = (-b - Math.sqrt(delta)) / (2*a);
	x = x1;

	if(x > 1)
		return false;

	alpha = Math.acos(x);
	alpha = alpha * 180 / Math.PI;
	beta = 180 - alpha;
	cos_C = Math.pow(mouse_xyra.r, 2) + Math.pow(ARM_LENGTH_1, 2) - ( Math.pow(ARM_LENGTH_2, 2) + Math.pow(ARM_LENGTH_3, 2) + 2*ARM_LENGTH_2*ARM_LENGTH_3*x );

	cos_C = cos_C /(2*mouse_xyra.r*ARM_LENGTH_1);
	angle_C = Math.acos(cos_C);
	gamma = (angle_C + mouse_xyra.a) % (2*Math.PI);
	gamma = gamma * 180 / Math.PI;

	if(gamma < 0)
		gamma += 360;

	if(gamma > 180)
	{
		var temp = gamma -  mouse_xyra.a * 180 / Math.PI;
		gamma = gamma - 2* temp;
		beta = 360 - beta;
	}

	if(gamma < 0 || gamma > 180)
		return false;

	angles[MOTOR_3] = Math.floor(beta);
	angles[MOTOR_4] = Math.floor(beta);
	angles[MOTOR_2] = Math.floor(gamma);

	return true;
}
function draw_pivot(x, y)
{
	ctx.beginPath();
	ctx.arc(x,y, 10, 0, 2*Math.PI);
	ctx.stroke();
	ctx.fill();
}
function ws_onmessage(e_msg)
{
	e_msg = e_msg || window.event; // MessageEvent
}
function ws_onopen()
{
	document.getElementById("ws_state").innerHTML = "OPEN";
	document.getElementById("wc_conn").innerHTML = "Disconnect";
	angles_change_notice();

	update_view();
}
function ws_onclose()
{
	document.getElementById("ws_state").innerHTML = "CLOSED";
	document.getElementById("wc_conn").innerHTML = "Connect";
	console.log("socket was closed");
	ws.onopen = null;
	ws.onclose = null;
	ws.onmessage = null;
	ws = null;
}
function wc_onclick()
{
	if(ws == null)
	{
		ws = new WebSocket("ws://<?echo _SERVER("HTTP_HOST")?>/remote_arm", "text.phpoc");
		document.getElementById("ws_state").innerHTML = "CONNECTING";

		ws.onopen = ws_onopen;
		ws.onclose = ws_onclose;
		ws.onmessage = ws_onmessage;  
	}
	else
		ws.close();
}
function mouse_down()
{
	if(ws == null)
		return;

	if(event.touches && (event.touches.length > 1))
		click_state = event.touches.length;

	if(click_state > 1)
		return;

	var state = event_handler(event);
	if(state)
	{
		click_state = 1;
		angles_change_notice();
	}

	event.preventDefault();
}
function mouse_up()
{
	click_state = 0;
}
function mouse_move()
{
	if(ws == null)
		return;

	var d = new Date();
	var time = d.getTime();
	if((time - last_time) < 50)
		return;

	last_time = time;

	if(event.touches && (event.touches.length > 1))
		click_state = event.touches.length;

	if(click_state > 1)
		return;

	if(!click_state)
		return;

	var state = event_handler(event);
	if(state)
	{
		click_state = 1;
		angles_change_notice();
	}

	event.preventDefault();
}
function angles_change_notice()
{
	if(ws != null && ws.readyState == 1)
	{
		ws.send(angles.join(",") + "\r\n");
		update_view(); 
	}
}
window.onload = init;
setTimeout(function(){ update_view();}, 500);
</script>
</head>
<body>
<h2>Arduino - Control Arm Robot via Web</h2>
<canvas id="servo"></canvas>
<p>WebSocket : <span id="ws_state">null</span><br></p>
<button id="wc_conn" type="button" onclick="wc_onclick();">Connect</button>
</body>
</html>

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

<!DOCTYPE html>

<html>

<head>

<title>Arduino - Arm Robot - Web</title>

<style>

body { text-align: center; }

canvas { background-color: #FFFFFF; }

</style>

var MOTOR_1 = 0;

var MOTOR_2 = 1;

var MOTOR_3 = 2;

var MOTOR_4 = 3;

var MOTOR_5 = 4;

var MOTOR_6 = 5;

var ARM_LENGTH_1 = 95;

var ARM_LENGTH_2 = 88;

var ARM_LENGTH_3 = 155;

var SOLE_WIDTH = 160;

var SOLE_HEIGHT = 73;

var flywheel_img = new Image();

flywheel_img.src = "flywheel.png";

var zone_A_radius = ARM_LENGTH_1 + ARM_LENGTH_2 + ARM_LENGTH_3;

var pivot_x = zone_A_radius + 5;

var pivot_y = zone_A_radius + 5;

var zone_B_radius = 100;

var zone_B_center_x = pivot_x / 2;

var zone_B_center_y = -(zone_B_radius + SOLE_HEIGHT + 20);

var zone_B_last_angle = 0;

var zone_C_radius = 70;

var zone_C_center_x = -pivot_x / 2;

var zone_C_center_y = -(zone_B_radius + SOLE_HEIGHT + 20);

var zone_C_last_angle = 0;

var zone_D_width = 30;

var zone_D_height = 200.0;

var zone_D_center_x = 0;

var zone_D_center_y = -(zone_B_radius + SOLE_HEIGHT + 110);

var grip_height = 70;

var grip_width = 100;

var canvas_width = zone_A_radius * 2 + 10;

var canvas_height = zone_A_radius + SOLE_HEIGHT + zone_B_radius * 2 + 180;

var grip_max_angle = 62.0; // 62 degree

var click_state = 0;

var mouse_xyra = {x:0, y:0, r:0.0, a:0.0};

var angles = [90, 90, 180, 180, 90, 17];

var ws = null;

var servo = null, ctx = null;

var last_time;

var a = 0, b = 0, c = 0;

function init()

{

servo = document.getElementById("servo");

servo.width = canvas_width;

servo.height = canvas_height;

servo.addEventListener("touchstart", mouse_down);

servo.addEventListener("touchend", mouse_up);

servo.addEventListener("touchmove", mouse_move);

servo.addEventListener("mousedown", mouse_down);

servo.addEventListener("mouseup", mouse_up);

servo.addEventListener("mousemove", mouse_move);

ctx = servo.getContext("2d");

ctx.translate(pivot_x, pivot_y);

ctx.rotate(Math.PI);

// quadratic equation parameters

a = 4*ARM_LENGTH_1*ARM_LENGTH_3;

b = 2*(ARM_LENGTH_2*ARM_LENGTH_1 + ARM_LENGTH_2*ARM_LENGTH_3);

c = Math.pow(ARM_LENGTH_1, 2) + Math.pow(ARM_LENGTH_2, 2) + Math.pow(ARM_LENGTH_3, 2) - 2*ARM_LENGTH_1*ARM_LENGTH_3;

}

function update_view()

{

ctx.clearRect(-canvas_width/2, -SOLE_HEIGHT, canvas_width, canvas_height);

ctx.save();

//draw zone A

ctx.fillStyle = "#D9D9D9";

ctx.beginPath();

ctx.arc(0,0,zone_A_radius, 0, 2*Math.PI);

ctx.fill();

ctx.fillStyle = "#FFFFFF";

ctx.fillRect(-pivot_x , -canvas_height + pivot_y, canvas_width, canvas_height - pivot_y - SOLE_HEIGHT);

// draw arm segments

ctx.strokeStyle="#00979D";

ctx.fillStyle = "#FF4500";

ctx.lineWidth = 20;

ctx.rotate(angles[MOTOR_2] / 180 * Math.PI);

ctx.beginPath();

ctx.moveTo(0,0);

ctx.lineTo(ARM_LENGTH_1,0);

ctx.stroke();

draw_pivot(0, 0);

ctx.translate(ARM_LENGTH_1,0);

ctx.rotate(-Math.PI + angles[MOTOR_3] / 180 * Math.PI);

ctx.beginPath();

ctx.moveTo(0,0);

ctx.lineTo(ARM_LENGTH_2, 0);

ctx.stroke();

draw_pivot(0, 0);

ctx.translate(ARM_LENGTH_2,0);

ctx.rotate(-Math.PI + angles[MOTOR_4] / 180 * Math.PI);

ctx.beginPath();

ctx.moveTo(0,0);

ctx.lineTo(ARM_LENGTH_3, 0);

ctx.stroke();

draw_pivot(0, 0);

ctx.restore();

ctx.strokeStyle = "#00979D";

ctx.lineWidth = 6;

// draw zone B

angle = (angles[MOTOR_1] + 45) * Math.PI / 180;

ctx.save();

ctx.translate(zone_B_center_x, zone_B_center_y);

ctx.rotate(angle);

ctx.drawImage(flywheel_img, - zone_B_radius, - zone_B_radius, zone_B_radius * 2, zone_B_radius * 2);

ctx.beginPath();

ctx.arc(0, 0, zone_B_radius, 0, 2 * Math.PI);

ctx.stroke();

ctx.restore();

// draw zone C

angle = (angles[MOTOR_5] + 45) * Math.PI / 180;

ctx.save();

ctx.translate(zone_C_center_x, zone_C_center_y);

ctx.rotate(angle);

ctx.drawImage(flywheel_img, - zone_C_radius, - zone_C_radius, zone_C_radius * 2, zone_C_radius * 2);

ctx.beginPath();

ctx.arc(0, 0, zone_C_radius, 0, 2 * Math.PI);

ctx.stroke();

ctx.restore();

// draw zone D

ctx.fillStyle = "#00DEE6";

ctx.lineWidth = 15;

var grip_dist = Math.floor(angles[MOTOR_6] / grip_max_angle * zone_D_height);

ctx.fillRect(zone_D_center_x - zone_D_width / 2, zone_D_center_y - zone_D_height / 2, zone_D_width, zone_D_height);

ctx.lineWidth = 1;

ctx.strokeStyle = "#FFFFFF";

var center_x = zone_D_center_x;

var center_y = zone_D_center_y + grip_dist / 2;

var grd = ctx.createLinearGradient(center_x, center_y, center_x, center_y + grip_height);

grd.addColorStop(0,"#004A4D");

grd.addColorStop(1,"#b3fcff");

ctx.fillStyle = grd;

ctx.beginPath();

ctx.moveTo(center_x + grip_width / 2, center_y);

ctx.bezierCurveTo(center_x + grip_width / 4, center_y + grip_height, center_x - grip_width / 4, center_y + grip_height,center_x - grip_width / 2, center_y);

ctx.closePath();

ctx.fill();

ctx.stroke();

center_x = zone_D_center_x;

center_y = zone_D_center_y - grip_dist / 2;

grd = ctx.createLinearGradient(center_x, center_y, center_x, center_y - grip_height);

grd.addColorStop(0,"#004A4D");

grd.addColorStop(1,"#b3fcff");

ctx.fillStyle = grd;

ctx.beginPath();

ctx.moveTo(center_x + grip_width / 2, center_y);

ctx.bezierCurveTo(center_x + grip_width / 4, center_y - grip_height, center_x - grip_width / 4, center_y - grip_height,center_x - grip_width / 2, center_y);

ctx.closePath();

ctx.fill();

ctx.stroke();

ctx.restore();

// draw sole

ctx.fillStyle = "#006468";

ctx.fillRect(-SOLE_WIDTH/2, -SOLE_HEIGHT, SOLE_WIDTH, SOLE_HEIGHT);

}

function event_handler(event)

{

var x, y, r, alpha;

// convert coordinate

if(event.touches)

{

var touches = event.touches;

x = (touches[0].pageX - touches[0].target.offsetLeft) - pivot_x;

y = (touches[0].pageY - touches[0].target.offsetTop) - pivot_y;

}

else

{

x = event.offsetX - pivot_x;

y = event.offsetY - pivot_y;

}

x = -x;

y = -y;

//check whether it's located in zone A or not

r = Math.sqrt(x * x + y * y);

if(r < zone_A_radius && y > -SOLE_HEIGHT)

{

if((x < SOLE_WIDTH/2) && (x > -SOLE_WIDTH/2) && (y < 0))

return false;

alpha = Math.atan2(y, x);

if(alpha < 0)

alpha += 2*Math.PI;

mouse_xyra.x = x;

mouse_xyra.y = y;

mouse_xyra.r = r;

mouse_xyra.a = alpha;

if(geometric_calculation())

return true;

}

//check whether it's located in zone B or not

temp_x = x - zone_B_center_x;

temp_y = y - zone_B_center_y;

var distance = Math.sqrt(temp_x * temp_x + temp_y * temp_y);

if(distance < zone_B_radius)

{

var angle = Math.atan2(temp_y, temp_x)* 180 / Math.PI;

if(click_state == 0)

zone_B_last_angle = angle;

else

{

if((Math.abs(angle) > 90) && (angle * zone_B_last_angle < 0))

{

if(zone_B_last_angle > 0)

zone_B_last_angle = -180;

else

zone_B_last_angle = 180;

}

angles[MOTOR_1] += Math.floor(angle - zone_B_last_angle);

angles[MOTOR_1] = Math.max(0, angles[MOTOR_1]);

angles[MOTOR_1] = Math.min(180, angles[MOTOR_1]);

zone_B_last_angle = angle;

}

return true;

}

//check whether it's located in zone C or not

temp_x = x - zone_C_center_x;

temp_y = y - zone_C_center_y;

var distance = Math.sqrt(temp_x * temp_x + temp_y * temp_y);

if(distance < zone_C_radius)

{

var angle = Math.atan2(temp_y, temp_x)* 180 / Math.PI;

if(click_state == 0)

zone_C_last_angle = angle;

else

{

if((Math.abs(angle) > 90) && (angle * zone_C_last_angle < 0))

{

if(zone_C_last_angle > 0)

zone_C_last_angle = -180;

else

zone_C_last_angle = 180;

}

angles[MOTOR_5] += Math.floor(angle - zone_C_last_angle);

angles[MOTOR_5] = Math.max(0, angles[MOTOR_5]);

angles[MOTOR_5] = Math.min(180, angles[MOTOR_5]);

zone_C_last_angle = angle;

}

return true;

}

//check whether it's located in zone D or not

var temp_x = Math.abs(x - zone_D_center_x);

var temp_y = Math.abs(y - zone_D_center_y);

if(temp_x < (zone_D_width / 2) && temp_y < (zone_D_height / 2))

{

var angle = temp_y / (zone_D_height / 2) * grip_max_angle;

angles[MOTOR_6] = Math.floor(angle);

console.log(angles[MOTOR_6]);

return true;

}

return false;

}

function geometric_calculation()

{

var c_ = c - Math.pow(mouse_xyra.r, 2);

var delta = b*b - 4*a*c_;

if(delta < 0)

return false; // no root

var x1 = 0, x2 = 0;

var x = 0;

var cos_C = 0;

var alpha = 0, beta = 0, gamma = 0;

x1 = (-b + Math.sqrt(delta)) / (2*a);

x2 = (-b - Math.sqrt(delta)) / (2*a);

x = x1;

if(x > 1)

return false;

alpha = Math.acos(x);

alpha = alpha * 180 / Math.PI;

beta = 180 - alpha;

cos_C = Math.pow(mouse_xyra.r, 2) + Math.pow(ARM_LENGTH_1, 2) - ( Math.pow(ARM_LENGTH_2, 2) + Math.pow(ARM_LENGTH_3, 2) + 2*ARM_LENGTH_2*ARM_LENGTH_3*x );

cos_C = cos_C /(2*mouse_xyra.r*ARM_LENGTH_1);

angle_C = Math.acos(cos_C);

gamma = (angle_C + mouse_xyra.a) % (2*Math.PI);

gamma = gamma * 180 / Math.PI;

if(gamma < 0)

gamma += 360;

if(gamma > 180)

{

var temp = gamma - mouse_xyra.a * 180 / Math.PI;

gamma = gamma - 2* temp;

beta = 360 - beta;

}

if(gamma < 0 || gamma > 180)

return false;

angles[MOTOR_3] = Math.floor(beta);

angles[MOTOR_4] = Math.floor(beta);

angles[MOTOR_2] = Math.floor(gamma);

return true;

}

function draw_pivot(x, y)

{

ctx.beginPath();

ctx.arc(x,y, 10, 0, 2*Math.PI);

ctx.stroke();

ctx.fill();

}

function ws_onmessage(e_msg)

{

e_msg = e_msg || window.event; // MessageEvent

}

function ws_onopen()

{

document.getElementById("ws_state").innerHTML = "OPEN";

document.getElementById("wc_conn").innerHTML = "Disconnect";

angles_change_notice();

update_view();

}

function ws_onclose()

{

document.getElementById("ws_state").innerHTML = "CLOSED";

document.getElementById("wc_conn").innerHTML = "Connect";

console.log("socket was closed");

ws.onopen = null;

ws.onclose = null;

ws.onmessage = null;

ws = null;

}

function wc_onclick()

{

if(ws == null)

{

ws = new WebSocket("ws://<?echo _SERVER("HTTP_HOST")?>/remote_arm", "text.phpoc");

document.getElementById("ws_state").innerHTML = "CONNECTING";

ws.onopen = ws_onopen;

ws.onclose = ws_onclose;

ws.onmessage = ws_onmessage;

}

else

ws.close();

}

function mouse_down()

{

if(ws == null)

return;

if(event.touches && (event.touches.length > 1))

click_state = event.touches.length;

if(click_state > 1)

return;

var state = event_handler(event);

if(state)

{

click_state = 1;

angles_change_notice();

}

event.preventDefault();

}

function mouse_up()

{

click_state = 0;

}

function mouse_move()

{

if(ws == null)

return;

var d = new Date();

var time = d.getTime();

if((time - last_time) < 50)

return;

last_time = time;

if(event.touches && (event.touches.length > 1))

click_state = event.touches.length;

if(click_state > 1)

return;

if(!click_state)

return;

var state = event_handler(event);

if(state)

{

click_state = 1;

angles_change_notice();

}

event.preventDefault();

}

function angles_change_notice()

{

if(ws != null && ws.readyState == 1)

{

ws.send(angles.join(",") + "\r\n");

update_view();

}

window.onload = init;

setTimeout(function(){ update_view();}, 500);

</script>

</head>

<body>

<h2>Arduino - Control Arm Robot via Web</h2>

<p>WebSocket : <span id="ws_state">null</span><br></p>

<button id="wc_conn" type="button" onclick="wc_onclick();">Connect</button>

</body>

</html>

Схемное решение

Для работы устройства необходимо обеспечить все 6 двигателей внешним питанием:

Робототехника

Управление роботом-рукой через Web-интерфейс

Демонстрация

Интерфейс пользователя

Архитектурное решение

Структура программы

Геометрические вычисления

Код прошивки

Схемное решение

Статьи на тему:

информация