Controle Remoto de Braço Robótico Arduino 6-DOF via Interface Web

Componentes e Materiais

Arduino UNO		1
PHPoC WiFi Shield para Arduino		1
Braço Robótico 6DOF		1

Sobre o Projeto

Se você é iniciante, recomendo ler os seguintes tutoriais:

• Arduino - Motor
• Arduino - Motor Servo
• Arduino - WiFi

1. Demonstração

2. Interface do Usuário

O braço robótico tem 6 motores.

• Zona A: Controla motores 2, 3, 4 (controla três juntas da mão)
• Zona B: Controla motor 1 (controla base)
• Zona C: Controla motor 5 (controla rotação da garra)
• Zona D: Controla motor 6 (controla garra)

3. Arquitetura do Sistema

4. Fluxo de Trabalho

Lado do Cliente (interface web do usuário - escrita em JavaScript + HTML + CSS)

Quando um usuário toca ou arrasta o dedo (ou clica ou move o mouse), podemos obter a coordenada (x, y). O fluxo de trabalho é o seguinte:

No caso da Zona A, para calcular ângulos dos motores 2, 3, 4, precisamos fazer alguns cálculos geométricos. Você pode consultá-los no final desta página.

Lado do Servidor (código Arduino):

Uma vez recebido um conjunto de ângulos dos clientes, seis motores movem-se dos ângulos atuais para os novos ângulos gradualmente. Os seis motores devem mover-se e atingir os novos ângulos ao mesmo tempo. Antes de detalhar como controlar todos os motores, vejamos como controlar um único motor. Suponha que queremos mover um motor do ângulo atual (angle) para novo ângulo (new_angle). Como a velocidade do motor é alta, devemos desacelerá-lo. Para fazer isso, duas etapas a seguir são repetidas até o motor atingir o novo ângulo:

• Mover motor com um pequeno passo.
• Pausar um pequeno tempo e então mover outro passo.

O diagrama a seguir ilustra o esquema acima no caso do novo ângulo ser maior que o atual:

Onde step_num é o número de passos que o motor deve dar. step e time são valores predefinidos. Estes dois últimos decidem a velocidade e suavidade. O acima é apenas para um robô. Para fazer robôs começarem a mover e atingirem o destino ao mesmo tempo, podemos fazer o seguinte: Seis motores tomam o mesmo step_num, mas step de cada motor é diferente. Então temos que escolher step_num neste projeto como máximo.

Geralmente, o fluxo de trabalho do Arduino é o seguinte:

5. Cálculo Geométrico

Vamos transformar o cálculo do braço robótico no seguinte problema de geometria:

Conhecido

• C é fixo
• Ponto conhecido - D é a entrada do usuário
• Ponto conhecido - CB, BA, AD (denotados por b, a, d respectivamente)
• Comprimentos de cada segmento do braço

Encontrar: ângulos C, B, A

Solução:

• Assumir que ângulos B e A são iguais
• Adicionar alguns pontos e segmentos adicionais

Cálculo

• Sabemos pontos C e D => podemos calcular o comprimento de DC (denotado por c)
• Também podemos calcular δ
• Olhe para o triângulo ABE, podemos inferir que AE = BE e ∠E = π - 2α.
• Então:

• Lei dos Cossenos no triângulo CDE:

• Mudar (1) e (2) em (3), temos:

Simplificar

• Simplificar o acima:

• Como sabemos a, b, c e d, resolva a equação quadrática acima, podemos calcular o valor de α. - E β = π – α - Até agora encontramos β, vamos encontrar γ - Lei dos Cossenos nos triângulos BDC e BDA:

• Resolva este conjunto de equações, podemos calcular γ.
• Então, seus ângulos necessários são: (δ+γ), β e β. Estes são ângulos dos motores 2, 3 e 4, respectivamente.

6. Código Fonte

Código fonte inclui dois arquivos:

• RobotArmWeb.ino: Código Arduino
• Remote_arm.php: Código da aplicação web, que é carregado no PHPoC WiFi Shield ou PHPoC Shield. (Veja instruções neste artigo.)

Você também precisa carregar o arquivo de imagem flywheel.png no PHPoC Shield.

O Melhor Kit Arduino Iniciante para Iniciantes

Se você está procurando um kit Arduino, veja O Melhor Kit Arduino para Iniciantes

Referências de Funções

• Arduino - Biblioteca Servo
• Servo.attach()
• Servo.write()
• Servo.writeMicroseconds()
• Servo.read()
• Servo.attached()
• Servo.detach()
• Serial.begin()
• Serial.println()

Código

RobotArmWeb Arduino

Este é o Código Arduino

#include <Phpoc.h>
#include <Servo.h>
int angle_init[] = {90, 101, 165, 153, 90, 120}; // quando motor está reto. Na web, o ângulo quando motor está reto é {0, 90, 130, 180, 0, 0};
int angle_offset[] = {0, 11, -15, -27, 0, 137}; // offset entre servo real e ângulo na web
int cur_angles[] = {90, 101, 165, 153, 90, 120}; // ângulos atuais dos seis motores (graus)
int dest_angles[] = {0, 0, 0, 0, 0, 0}; // ângulos destinados
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}; // passos de movimento de cada motor (graus)
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);
 servo2.attach(3);
 servo3.attach(4);
 servo4.attach(5);
 servo5.attach(6);
 servo6.attach(7);
 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;
 // mover motores em muitos pequenos passos para movimento suave, evitando movimento súbito
 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;
 }
 }
 }
 // mover motores passo a passo
 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 PHP

Este código é a aplicação web.

<!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 graus
 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);
 // parâmetros equação quadrática
 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 zone A
 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 zone B
 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 zone C
 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 zone D
 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; }
 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>

Esquemáticos

Precisa fornecer fonte de alimentação externa para 6 motores