Control two-legged robot using PHPoC Blue

Control two-legged robot using PHPoC Blue

Postby Khanh » Mon Apr 18, 2016 5:11 pm

Demo
phpBB [video]

Introduction
Two-legger robot has six servo moters. Three ones are in right leg and three ones are in left leg. With just below 120 PHPoC lines of code, you can make the two-legged robot walk naturally.

Basic Idea

I am not a specialist in robot, I am just an IT man. So I take a very simple method to make robot move.
- Just imitate the human step,
- choose some states (six values of angles of six motors)
- get angle of each state (write down these values)
- move robot state by state in a loop
In my case, I choose 9 states. You can change this value to make robot walk more naturally.

Source Code

[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(69, 116, 150, 91, 68, 64); 
$offsets 
= array(69, 116, 150, 91, 68, 64); 
$state1 
= array(-28, 0, 0, -17, 0, 0);
$state2 = array(2, -24, -47, -18, 3, 1);

$state3 = array(-15, -14, -26, -11, -40, -16);
$state4 = array(4, -18, -9, 13, -33, -37);
$state5 = array(4, -5, -9, 42, -37, -37);
$state6 = array(20, -5, -9, 64, -33, -37);
$state7 = array(22, 11, -9, 8, 4, -27);
$state8 = array(22, 11, -9, 8, 91, -5);
$state9 = array(22, 11, -9, 8, 113, 55);
$state10 = array(22, 11, 13, 8, 113, 58);
$state12 = array(22, 6, 13, 8, 52, 58);

// arry for loop;
$state = array($state3, $state4, $state5, $state6, $state7, $state8, $state9, $state10, $state12);

$steps = array(1, 1, 1, 1, 1 ,1); // moving steps of each motor (degree)
$pwm = array(0, 1, 2, 3, 4 ,5);

$sign = 1;
$stt1 = array();
$stt2 = array(0, 0, 0, 0, 0, 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
}

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);
}
function move($new_angles, $period = 300000) 
{
    global $angles;
    global $steps;
    global $pwm;
    global $offsets;
    $n = 0;
    for($i = 0; $i <6; $i++)
    {
        $new_angles[$i] += $offsets[$i];
        $temp = abs($angles[$i] - $new_angles[$i]);
        $temp = (int)floor($temp / $steps[$i]);
        if($n < $temp)    
            $n 
= $temp;
    }
    if($n != 0)
        $sleep_time = (int)floor($period / $n);
    else
        return
;
    for($j = $n; $j >0; $j--)
    {
        for($i = 0; $i <6; $i++)
        {
            $temp = ($angles[$i] - $new_angles[$i]) /$j;
            $angles[$i] -= $temp;
            rotate_motor($pwm[$i], $angles[$i]);    
        
}
        usleep($sleep_time);
    }
    // adjust error due to floor function
    for($i = 0; $i <6; $i++)
    {
        $angles[$i] = $new_angles[$i];
        rotate_motor($pwm[$i], $angles[$i]);    
    
}
}

sleep(5);

move($state1);
move($state2);

while(
1)
{
    for($i = 0; $i < 9; $i++)
    {
        echo $i;
        $stt1 = $state[$i];
        if($sign == -1)
        {
            $stt2[0] = -$stt1[3];
            $stt2[1] = -$stt1[4];
            $stt2[2] = -$stt1[5];
            $stt2[3] = -$stt1[0];
            $stt2[4] = -$stt1[1];
            $stt2[5] = -$stt1[2];
            move($stt2);
        }
        else
            move
($stt1);
    }
    $sign *=-1;
}
 
?>


Wiring between the two-legged robot and PHPoC is similar to Arm robot. You can refer here:
viewtopic.php?f=42&t=151
Someone may ask how to get angles of each state?
You can use the following code to adjust angle of each motor and then get angle of each states.
Index.php
Code: Select all

<?php
    define
("CMD_STOP", 0);
    define("CMD_LEFT_1_INC", 1);
    define("CMD_LEFT_1_DEC", 2);
    define("CMD_LEFT_2_INC", 4);
    define("CMD_LEFT_2_DEC", 8);
    define("CMD_LEFT_3_INC", 16);
    define("CMD_LEFT_3_DEC", 32);
    define("CMD_RIGHT_1_INC", 64);
    define("CMD_RIGHT_1_DEC", 128);
    define("CMD_RIGHT_2_INC", 256);
    define("CMD_RIGHT_2_DEC", 512);
    define("CMD_RIGHT_3_INC", 1024);
    define("CMD_RIGHT_3_DEC", 2048);
?>
<!DOCTYPE html>
<html>
<head>
<title>PHPoC Robot ARM</title>
<meta name="viewport" content="width=device-width, initial-scale=0.7">
<style type="text/css">
body { text-align: center; }
#area {
    margin-right: auto;
    margin-left: auto;
    height: 650px; 
    width: 501px; 
    position: relative;
    margin-bottom: 10px;
    border: 1px solid #000;
}
div[class^="arrow"] {
    position: absolute; 
    width:63px; 
    height:70px;
}


div[class^="arrow_right"] {
    background: url(right.png) no-repeat;
    background-size: contain;     
    left:375px;
}

div[class^="arrow_left"]  {
    background: url(left.png) no-repeat; 
    background-size: contain;
    left:65px;
}

div[class*="1"] {top:50px;}
div[class*="2"] {top:150px;}
div[class*="3"] {top:250px;}
div[class*="4"] {top:350px;}
div[class*="5"] {top:450px;}
div[class*="6"] {top:550px;}
</style>
<script type="text/javascript">
var ws;
var cmd = 0;

function init() 
{       
    var action = document.querySelector("#area");
    action.addEventListener("touchstart", mouse_down);
    action.addEventListener("touchend", mouse_up);
    action.addEventListener("touchcancel", mouse_up);
    action.addEventListener("mousedown", mouse_down);
    action.addEventListener("mouseup", mouse_up);
    action.addEventListener("mouseout", mouse_up);    
}
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";
}
function ws_onclose()
{
    document.getElementById("ws_state").innerHTML = "CLOSED";
    document.getElementById("wc_conn").innerHTML = "Connect";

    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_leg", "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(event) 
{
    if (event.target !== event.currentTarget) 
    {       
        set_command(event.target.id);
    }
    event.stopPropagation();    
    event.preventDefault();    
}
function mouse_up(event) 
{
    if (event.target !== event.currentTarget) 
    {
        clear_command(event.target.id);
    }
    event.stopPropagation();   
    event.preventDefault();    
}
function set_command(id) 
{    
    cmd = cmd | id ;
    if(ws.readyState == 1)
        ws.send(cmd + "\r\n");    
}
function clear_command(id)
{    
    cmd = cmd & (~id) ;
    if(ws.readyState == 1)
        ws.send(cmd + "\r\n");   
}

window.onload = init;
</script>
</head>
<body>

<div id="area">
    <div id="<?echo CMD_LEFT_1_INC?>" class="arrow_left_1"></div>
    <div id="<?echo CMD_LEFT_1_DEC?>" class="arrow_right_1"></div>
    <div id="<?echo CMD_LEFT_2_INC?>" class="arrow_left_2"></div>
    <div id="<?echo CMD_LEFT_2_DEC?>" class="arrow_right_2"></div>
    <div id="<?echo CMD_LEFT_3_INC?>" class="arrow_left_3"></div>
    <div id="<?echo CMD_LEFT_3_DEC?>" class="arrow_right_3"></div>
    <div id="<?echo CMD_RIGHT_1_INC?>" class="arrow_left_4"></div>
    <div id="<?echo CMD_RIGHT_1_DEC?>" class="arrow_right_4"></div>
    <div id="<?echo CMD_RIGHT_2_INC?>" class="arrow_left_5"></div>
    <div id="<?echo CMD_RIGHT_2_DEC?>" class="arrow_right_5"></div>
    <div id="<?echo CMD_RIGHT_3_INC?>" class="arrow_left_6"></div>
    <div id="<?echo CMD_RIGHT_3_DEC?>" class="arrow_right_6"></div>
    
</div>
<p>
WebSocket : <span id="ws_state">null</span><br>
</p>
<button id="wc_conn" type="button" onclick="wc_onclick();">Connect</button>
</body>
</html>    


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

define("CMD_STOP", 0);
define("CMD_LEFT_1_INC", 1);
define("CMD_LEFT_1_DEC", 2);
define("CMD_LEFT_2_INC", 4);
define("CMD_LEFT_2_DEC", 8);
define("CMD_LEFT_3_INC", 16);
define("CMD_LEFT_3_DEC", 32);
define("CMD_RIGHT_1_INC", 64);
define("CMD_RIGHT_1_DEC", 128);
define("CMD_RIGHT_2_INC", 256);
define("CMD_RIGHT_2_DEC", 512);
define("CMD_RIGHT_3_INC", 1024);
define("CMD_RIGHT_3_DEC", 2048);

define("LEFT_1", 0);
define("LEFT_2", 1);
define("LEFT_3", 2);
define("RIGHT_1", 3);
define("RIGHT_2", 4);
define("RIGHT_3", 5);
 
$angles 
= array(69, 116, 150, 91, 68, 64); 
$offsets 
= array(69, 116, 150, 91, 68, 64); 
$state1 
= array(-28, 0, 0, -17, 0, 0);
$state2 = array(2, -24, -47, -18, 3, 1);

$state3 = array(-15, -14, -26, -11, -40, -16);
$state4 = array(4, -18, -9, 13, -33, -37);
$state5 = array(4, -5, -9, 42, -37, -37);
$state6 = array(20, -5, -9, 64, -33, -37);
$state7 = array(22, 11, -9, 8, 4, -27);
$state8 = array(22, 11, -9, 8, 91, -5);
$state9 = array(22, 11, -9, 8, 113, 55);
$state10 = array(22, 11, 13, 8, 113, 58);
$state12 = array(22, 6, 13, 8, 52, 58);

$state = array($state3, $state4, $state5, $state6, $state7, $state8, $state9, $state10, $state12);

$state_last = array(0, 140, 30, 30, 0, 75);
$angle_max =  array(180, 180, 180, 180, 180, 180);
$angle_min =  array(  0,  0,   0,   0,   0,  0);
$move_states = array(0, 0, 0, 0, 0 ,0); // 1: move forward, 0: stop, -1: move backward
$steps = array(1, 1, 1, 1, 1 ,1); // moving steps of each motor (degree)
$pwm = array(0, 1, 2, 3, 4 ,5);

$sign = 1;
$stt1 = array();
$stt2 = array(0, 0, 0, 0, 0, 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_leg", "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)
        {
            $cmd = (int)$rbuf;    
            
            $move_states
[LEFT_1] = ($cmd&CMD_LEFT_1_INC) - (($cmd&CMD_LEFT_1_DEC)>>1);
            $move_states[LEFT_2] = (($cmd&CMD_LEFT_2_INC)>>2) - (($cmd&CMD_LEFT_2_DEC)>>3);
            $move_states[LEFT_3] = (($cmd&CMD_LEFT_3_INC)>>4) - (($cmd&CMD_LEFT_3_DEC)>>5);
            $move_states[RIGHT_1] = (($cmd&CMD_RIGHT_1_INC)>>6) - (($cmd&CMD_RIGHT_1_DEC)>>7);
            $move_states[RIGHT_2] = (($cmd&CMD_RIGHT_2_INC)>>8) - (($cmd&CMD_RIGHT_2_DEC)>>9);
            $move_states[RIGHT_3] = (($cmd&CMD_RIGHT_3_INC)>>10) - (($cmd&CMD_RIGHT_3_DEC)>>11);    
            
// Debug
            if($cmd == 0)
            {
                echo "\r\n";
                for($i = 0; $i <6; $i++)
                {
                    echo $angles[$i] - $offsets[$i];
                    if($i != 5)
                        echo ", ";
                }
            }
            // End Debug
        }
        
        for
($i = 0; $i <6; $i++)
        {
            $angles[$i] += $move_states[$i]*$steps[$i];
            if($angles[$i] > $angle_max[$i])
                $angles[$i] = $angle_max[$i];
            else if($angles[$i] < $angle_min[$i])
                $angles[$i] = $angle_min[$i];
            // set angle 
            rotate_motor($pwm[$i], $angles[$i]);
        }
        usleep(30000);    
    
}
}
 
?>
    

Open the web browser on your computer or smart phone, and connect to PHPoC
1.PNG
1.PNG (50.58 KiB) Viewed 2055 times

By using this program, you can adjust angles of six motors to states you want to.

Thanks, and if you have any question, please don't hesitate to contact me or add comment!
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