Tweet
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. 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
Source Code
[Source code - task0.php]
PHP Code:
<?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 https://forum.phpoc.com/blogs/khanh-...-robot-via-web.
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
PHP Code:
<?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
PHP Code:
<?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
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!