Arm robot remote control using PHPoC Blue

Arm robot remote control using PHPoC Blue

Postby Khanh » Fri Apr 08, 2016 10:47 am

Outline
    - Demo
    - Systems Architecture & Data flow
    - User Interface
    - Wiring between the arm robot and PHPoC
    - Source Code
    - Workflow
    - Geometric Calculation
1. Demo
phpBB [video]


2. Systems Architecture & Data flow
Systems Architecture
system architecture.png
system architecture.png (123.27 KiB) Viewed 2650 times

Data flow
data flow.png
data flow.png (23.79 KiB) Viewed 2650 times


3. User Interface
Arm robot has 6 motors illustrated as follows:
robot.PNG
robot.PNG (137.11 KiB) Viewed 2650 times
ui.PNG
ui.PNG (71.4 KiB) Viewed 2650 times

- Zone A: Control motor 2, 3, 4.
- Zone B: Control motor 1.
- Zone C: Control motor 5.
- Zone D: Control motor 6 (gripper)

4. Wiring between the arm robot and PHPoC
In order to control six servo motors, six timers (four hardware timers and two software timers) are used to generate PWM signals. An extra power source is required to provide enough power for motors.
wiring.PNG
wiring.PNG (105.52 KiB) Viewed 2650 times


5. Source Code
Apart from library, there are two main source files in PHPoC:
- index.php: once receiving http request from a client, PHPoC interprets this file (only interpreting source inside <?php ?> tag) and then return a html file to client. So, client side is implemented in this file.
- task0.php: this file contains the source code of server side. So you can implement the function to receive the angles and control the robot here.
[Source code - index.php]
Code: Select all
<!DOCTYPE html>
<html>
<head>
<title>PHPoC - Arm Robot</title>
<meta name="viewport" content="width=device-width, initial-scale=0.7">
<style>
body { text-align: center; }
canvas { background-color: #f0f0f0; }
</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 canvas_width = 900, canvas_height = 600;
var sole_width = 160, sole_height = 75;
var pivot_x = canvas_width / 2, pivot_y = canvas_height - sole_height;

var arm_length_1 = 95,
    arm_length_2 = 88,
    arm_length_3 = 155;
var zone_A_radius = arm_length_1 + arm_length_2 + arm_length_3;

var zone_B_radius_in = zone_A_radius + 10;
var zone_B_radius_out = zone_B_radius_in + 60;
 
var zone_D_width = 200, zone_D_height = 30;
var zone_D_left =  pivot_x - zone_D_width - 40;
var zone_D_top =  pivot_y - zone_D_height - 50;

var zone_C_center_x = -pivot_x + 100;
var zone_C_center_y =  pivot_y - 100;
var zone_C_radius_in = 30, zone_C_radius_out = 60;

var grip_max_angle = 62; // 62 degree

var click_state = 0;
var mouse_xyra = {x:0, y:0, r:0.0, a:0.0};
var angles = [20, 120, 120, 120, 30, 50];

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);
    ctx.strokeStyle="#00a3cc";
    ctx.lineWidth = 4;
    
    // 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.fillStyle = "#00ccff";
    
    ctx.save();
    
    //draw zone A
    ctx.beginPath();
    ctx.arc(0,0,zone_A_radius, 0, 2*Math.PI);
    ctx.stroke();
    
    // draw zone B
    ctx.strokeStyle="#ff8080";
    var angle = angles[0] * Math.PI / 180;
    ctx.beginPath();
    ctx.arc(0, 0, zone_B_radius_out, 0, angle);
    ctx.arc(0, 0, zone_B_radius_in, angle, 0, true);
    ctx.fill();
    
    ctx.beginPath();
    ctx.arc(0, 0, zone_B_radius_out, 0, Math.PI);
    ctx.arc(0, 0, zone_B_radius_in, Math.PI, 0, true);
    ctx.closePath();
    ctx.stroke();
    
    // draw zone C
    angle = angles[MOTOR_5] * Math.PI / 180;
    ctx.beginPath();
    ctx.arc(zone_C_center_x, zone_C_center_y, zone_C_radius_out, 0, angle);
    ctx.arc(zone_C_center_x, zone_C_center_y, zone_C_radius_in, angle, 0, true);
    ctx.fill();
    
    ctx.beginPath();
    ctx.arc(zone_C_center_x, zone_C_center_y, zone_C_radius_out, 0, Math.PI);
    ctx.arc(zone_C_center_x, zone_C_center_y, zone_C_radius_in, Math.PI, 0, true);
    ctx.closePath();
    ctx.stroke();
    
    // draw zone D            
    var temp = Math.floor(angles[MOTOR_6] / grip_max_angle * 200);
    ctx.fillRect(zone_D_left, zone_D_top, temp, zone_D_height);
    ctx.rect(zone_D_left, zone_D_top, zone_D_width, zone_D_height);
    ctx.stroke();
    
    ctx.beginPath();
    ctx.moveTo(zone_D_left, zone_D_top + zone_D_height / 2);
    ctx.arc(zone_D_left, zone_D_top + zone_D_height / 2, zone_D_height, Math.PI / 2, Math.PI * 3 / 2);
    ctx.closePath();
    ctx.fill();
    
    ctx.beginPath();
    ctx.moveTo(zone_D_left + temp, zone_D_top + zone_D_height / 2);
    ctx.arc(zone_D_left + temp, zone_D_top + zone_D_height / 2, zone_D_height, Math.PI / 2, Math.PI * 3 / 2, true);
    ctx.closePath();
    ctx.fill();
    
    // draw arm segments
    ctx.lineWidth = 6;
    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();
    
    // draw sole
    ctx.fillStyle = "#818181";
    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 D or not
    var temp_x = x - zone_D_left;
    var temp_y = y - zone_D_top;
    
    //if(temp_x > 0 && temp_x < zone_D_width && temp_y > 0 && temp_y < zone_D_height)
    if(temp_x > -zone_D_height && temp_x < (zone_D_width + zone_D_height) && temp_y > 0 && temp_y < zone_D_height)    
    {
        if(temp_x < 0)
            temp_x = 0;
        else if(temp_x > zone_D_width)
            temp_x = zone_D_width;
        
        var angle = temp_x / zone_D_width * grip_max_angle;        
        angles[MOTOR_6] = Math.floor(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_in && distance < zone_C_radius_out && y > zone_C_center_y)
    {
        var angle = Math.atan2(temp_y, temp_x)* 180 / Math.PI;
        angles[MOTOR_5] = Math.floor(angle);
        return true;
    }        
    
    //check whether it's located in zone B or not
    r = Math.sqrt(x * x + y * y);
    
    if(r > zone_B_radius_out)        
        return false;
    
    if(r > zone_B_radius_in && y < 0)
        return false;
    
    if(r > zone_B_radius_in)
    {
        var angle = Math.atan2(y, x)* 180 / Math.PI;
        angles[MOTOR_1] = Math.floor(angle);
        return true;
    }
    
    //check whether it's located in zone A or not
    if(r > zone_A_radius)
        return false;
    
    if(y < -sole_height)
        return false;
    
    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;
    
    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, 5, 0, 2*Math.PI);
    ctx.stroke();
}
function ws_onmessage(e_msg)
{
    e_msg = e_msg || window.event; // MessageEvent
 
    //alert("msg : " + e_msg.data);
}
function ws_onopen()
{
    document.getElementById("ws_state").innerHTML = "OPEN";
    document.getElementById("wc_conn").innerHTML = "Disconnect";
    if(ws.readyState == 1)
        ws.send(angles.join(" ") + "\r\n");  
    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")?>/robot_arm", "csv.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(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()
{
    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)
        if(ws.readyState == 1)
        {
            ws.send(angles.join(" ") + "\r\n");  
            update_view();    
        }
}

window.onload = init;
//update_view();
setTimeout(function(){ update_view();}, 500);
</script>
</head>

<body>

<h2>
Arm Robot<br>
</h2>
<br>
<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>


[Source code - task0.php]
Code: Select all
<?php

if(_SERVER("REQUEST_METHOD"))
    exit; // avoid php execution via http request

include "/lib/sd_340.php";
include "/lib/sn_tcp_ws.php";

define("PWM_PERIOD", 20000); // 20000us (20ms)
define("WIDTH_MIN", 600);
define("WIDTH_MAX", 2450);

$angles = array(0, 20, 180, 90, 0, 75); // current angles of six motors (degree)
$angle_offset = array(180, 6, -60, -35, 0, 137); // due to assembly  
$new_angles = array(0, 140, 30, 30, 0, 75); // destined angles
$angle_max =  array(180, 180, 160, 160, 180, 137);
$angle_min =  array(  0,  0,   0,   0,   0,  75);
$direction = array(-1, 1, 1, 1, 1 ,-1); 
$steps 
= array(3, 2, 2, 2, 4 ,4); // moving steps of each motor (degree)
$pwms = array(0, 1, 2, 3, 4 ,5);

$n = 0;

for(
$i = 0; $i <6; $i++)
{
    $width = WIDTH_MIN + (int)round((WIDTH_MAX - WIDTH_MIN) * $angles[$i] / 180.0);
    if($i<=3)
        ht_pwm_setup($i, $width, PWM_PERIOD, "us");
    else
        st_pwm_setup
($i, $i + 8, $width, PWM_PERIOD, "us"); // pin 12 and pin 13 for software timers
}
ws_setup(0, "robot_arm", "csv.phpoc");

$rbuf = "";

function rotate_motor($id, $angle) 
{
    $width = WIDTH_MIN + (int)round((WIDTH_MAX - WIDTH_MIN) * $angle / 180.0);
    if($id<=3)
        ht_pwm_width($id, $width, PWM_PERIOD);
    else
        st_pwm_width
($id, $width, PWM_PERIOD);
}

while(
1)
{
    if(ws_state(0) == TCP_CONNECTED)
    {
        $rlen = ws_read_line(0, $rbuf);

        if($rlen)
        {
            $new_angles = explode(" ", $rbuf);    
            
            $n 
= 0;
            for($i = 0; $i <6; $i++)
            {
                $new_angles[$i] = (int)$new_angles[$i] * $direction[$i] + $angle_offset[$i];
                $max = abs($angles[$i] - $new_angles[$i]);
                $max = (int)floor($max / $steps[$i]);
                if($n < $max)    
                    $n 
= $max;
            }                    
        
}
        
        if
($n > 0)
        {
            for($i = 0; $i <6; $i++)
            {
                $step = ($angles[$i] - $new_angles[$i]) / $n;
                $angles[$i] -= $step;
                if($angles[$i] > $angle_max[$i])
                    $angles[$i] = $angle_max[$i];
                else if($angles[$i] < $angle_min[$i])
                    $angles[$i] = $angle_min[$i];
                rotate_motor($pwms[$i], $angles[$i]);    
            
}
            $n--;
            usleep(20000);                
        
}                            
        
//usleep(30000);    
    }
}
 
?>


6. Workflow

Client Side

When a user touches or sweeps finger (or clicks or moves mouse), we can get coordinate (x, y). The working flow is as follows:
client1.PNG
client1.PNG (35.64 KiB) Viewed 2650 times

In case of Zone A, to calculate angles of motor 2, 3, 4, we need to do some geometric calculation. You can refer to it at the end of this page.

Server side:

Once receiving a set of angles from clients, six motors moves from the current angles to the new angles gradually. Six motor should move and reach new angles at the same time.
Before going into detail how to control all motors, let’s look at how to control a single motor. Suppose that we want to move a motor from current angle ($angle) to new angle ($new_angle). Since speed of motor is high, we should slow down it. To do like that, two following steps are repeated until the motor reach new angle
- Move motor with a small step,
- Pause a small time and then move another step.
The following diagram illustrates the above scheme in case new angle is greater than current angle:
server1.PNG
server1.PNG (31.3 KiB) Viewed 2650 times

Where $n is number of steps the motor has to take. $step and $sleep_time is predefined values. Two later ones decide the speed and smoothness.
The above is just only for one robot.
To make robots start to move and reach destination at the same time, we can do as follows: Six motors take the same $n, but $step of each motor is different from each other.
So we have to choose $n. In this project, $n is maximum.
The flowing code show how to calculate the $n:
Code: Select all
$n = 0;
for($i = 0; $i <6; $i++)
{
   $max = abs($angles[$i] - $new_angles[$i]);
   $max = (int)floor($max / $steps[$i]);
   if($n < $max)   
      $n = $max;
}

After selecting the number of step, we calculate value of step for each motor as follow:
Code: Select all
if($n > 0)
{
   for($i = 0; $i <6; $i++)
   {
      $step = ($angles[$i] - $new_angles[$i]) / $n;
      $angles[$i] -= $step;
      rotate_motor($pwm[$i], $angles[$i]);   
   }
   $n--;
   usleep($sleep_time);            
}

Generally, working flow of sever side is as follow:
server2.PNG
server2.PNG (42.22 KiB) Viewed 2650 times


7. Geometric Calculation Explanation
Let’s make arm robot calculation into the following geometry problem:
geo1.PNG
geo1.PNG (12.49 KiB) Viewed 2650 times

We have:
- C is fixed: a known point
- D is the input from user: a known point
- CB, BA, AD (denoted by b, a, d respectively): lengths of each arm segments
Find: angles C, B, A
Solution:
- Make assumption that angle B and A are the same
- Add some additional points and segment
geo2.PNG
geo2.PNG (25.49 KiB) Viewed 2650 times

- We knew points C and D => we can calculate the length of DC (denoted by c)
- We can also calculate the δ.
- Look at triangle ABE, we can infer that AE = BE and ∠E = π - 2α.
So:
math1.PNG
math1.PNG (3.83 KiB) Viewed 2650 times

- The Law of Cosines in triangle CDE:
math2.PNG
math2.PNG (2.28 KiB) Viewed 2650 times

- Change (1) and (2) into (3), we have:
math3.PNG
math3.PNG (3.47 KiB) Viewed 2650 times

- Simplify the above:
math4.PNG
math4.PNG (2.34 KiB) Viewed 2650 times

- Since we know a, b, c and d, solve the above quadratic equation, we can calculate the value of α.
- And β = π – α
- Until now we found β, let’s find γ
- The Law of Cosines in triangles BDC and BDA:
math5.PNG
math5.PNG (2.4 KiB) Viewed 2650 times

- Solve this set of equations, we can calculate γ
- So, there required angles is : (δ+γ), β and β. These are angles of motors 2, 3 and 4 respectively.
Have fun!
Khanh
 
Posts: 72
Joined: Fri Mar 11, 2016 10:57 am

Return to Project

Who is online

Users browsing this forum: No registered users and 1 guest

cron