主页 > 编程资料 > Arduino >
发布时间:2018-08-26 作者:网络 阅读:233次


需要的组件和用品

Arduino UNO和Genuino UNO  、WiFi Shield 、机械手臂

clipboard.png

项目介绍示范:

clipboard (1).png

https://youtu.be/_MF0dTScoLE


界面功能介绍:

clipboard.png

机器人手臂有6个电机。

A区:控制电机2,3,4(控制三个手关节)

B区:控制电机1(控制基座)

区域C:控制电机5(控制夹具的旋转)

D区:控制电机6(控制夹具)


系统架构:

clipboard.png

工作流程:

客户端(在网页中 - 用JavaScript编写)

当用户触摸或扫过手指(或点击或移动鼠标)时,我们可以得到坐标(x,y)。工作流程如下:

_3LhVrTQ0a3.jpeg

在区域A的情况下,为了计算电机2,3,4的角度,我们需要进行一些几何计算。您可以在本页末尾参考。

服务器端(Arduino代码):

一旦从客户接收到一组角度,六个电机逐渐从当前角度移动到新角度。六个电机应同时移动并达到新的角度。在详细介绍如何控制所有电机之前,让我们来看看如何控制单个电机。假设我们想要将电机从当前角度(角度)移动到新角度(new_angle)。由于电机速度很高,我们应该放慢速度。为此,重复以下两个步骤,直到电机达到新的角度:

  • 小步移动电机。

  • 暂停一小段时间,然后再移动一步。

下图说明了新角度大于当前角度时的上述方案:

_3LhVrTQ0a3.jpeg

Wherestep_num是电机必须采取的步数。是预定义的值。两个后来决定速度和平滑度。以上仅适用于一个机器人。为了让机器人同时开始移动并到达目的地,我们可以这样做:六个电机采用相同的step_num ,但每个电机彼此不同。所以我们必须在这个项目中选择step_num 是最大的。step and timestep

一般来说,Arduino的工作流程如下:

_3LhVrTQ0a3.jpeg

5.几何计算

让我们将机器人手臂计算成以下几何问题:

_3LhVrTQ0a3.jpeg

已知

  • C是固定的

  • 已知点 - D是来自用户的输入

  • 已知点 - CB,BA,AD(分别用b,a,d表示)

  • 每个臂段的长度查找:角度C,B,A 解决方案:

  • 假设角度B和A是相同的

  • 添加一些额外的点和细分

_3LhVrTQ0a3.jpeg

计算

  • 我们知道点C和D =>我们可以计算出DC的长度(用c表示)

  • 我们也可以计算δ

  • 看三角形ABE,我们可以推断出AE = BE和∠E=π - 2α。

  • 所以:

_3LhVrTQ0a3.jpeg

  • 三角形CDE中的余弦定律:

_3LhVrTQ0a3.jpeg

  • 将(1)和(2)改为(3),我们有:

_3LhVrTQ0a3.jpeg

简化

  • 简化以上内容:

_3LhVrTQ0a3.jpeg

  • 由于我们知道a,b,c和d,求解上述二次方程,我们可以计算出α的值。 - 并且β=π - α - 到现在为止我们找到β,让我们找到γ - 三角形BDC和BDA中的余弦定律:

_3LhVrTQ0a3.jpeg

  • 求解这组方程,我们就可以计算出γ。

  • 因此,它们所需的角度是:(δ+γ),β和β。这些是电动机2,3和4的角度。

6.源代码

源代码包括两个文件:

  • RobotArmWeb.ino:Arduino代码

  • Remote_arm.php:Web应用程序代码,上传到PHPoC WiFi Shield或PHPoC Shield。

    您还需要将图像文件flywheel.png上传到PHPoC Shield。


    原理图:

    clipboard.png


    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);
    	}
    }


    PHP代码:

    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>



    原文地址:https://create.arduino.cc/projecthub/phpoc_man/arduino-control-arm-robot-via-web-379ef3?ref=platform&ref_id=424_trending___&offset=3


    关键字词:arduino  机械手臂  

    必填

    ◎已有0人评论

    加载更多