Things used in this project

Hardware components:
Phpoc blue per 100x1000 euk22wtv9i
PHPoC Blue
×1
Micro stepper motor controller t type 4hz2ygod0g
PHPoC Stepper Motor Controller Ⅱ (T-type)
×2
step moter (4s42q-l03434s3)
×2
11026 02
Jumper wires (generic)
×1
battery (used for phpoc and moter)
×2
screw (or anything to attach your moters or boards to your car body. Like tape.)
×1
Software apps and online services:
  rv17q3stcl
PHPoC Debugger
Hand tools and fabrication machines:
Lasercutter
Laser cutter (generic)

Custom parts and enclosures

robotic vacuum body
body
default_QtXTpBg7FE.rld

Schematics

20171029_213218_kNjCYJLl2X.jpg
20171029 213218 knjcyjll2x
20171029_213224_8W8votdH4Z.jpg
20171029 213224 8w8votdh4z
20171029_213233_qIyP0BdmCl.jpg
20171029 213233 qiyp0bdmcl

Code

task0.phpPHP
<?php
if(_SERVER("REQUEST_METHOD"))
	exit; // avoid php execution via http request

include_once "/lib/sd_340.php";
include_once "/lib/sd_spc.php";
include_once "/lib/sn_tcp_ws.php";

define("TRACK",				300); //in mm
define("WHEEL_RADIUS",		65); // in mm
define("WHEEL_PERIMETER",	2*M_PI*WHEEL_RADIUS); // in mm

define("FULL_STEP_ANGLE",	1.8); //degree0
define("HALF_STEP_ANGLE",	0.9); //degree
define("FULL_STEP_NUM",		200); //step per round
define("HALF_STEP_NUM",		400); //step per round

define("MAP_IMG_WIDTH",		1140); //pixel
define("MAP_REAL_WIDTH",	25*450.0); //millimeter

define("STATE_FREE",		0);
define("STATE_PAUSE",		1);
define("STATE_MOVE",		2);

define("INF_DISTANCE",		1000); // 1000cm

function step_cmd($sid, $cmd)
{
	$resp = spc_request($sid, 4, $cmd);

	if($resp)
	{
		$resp = explode(",", $resp);

		if((int)$resp[0] != 200)
			echo "step_cmd : '$cmd' request error ", $resp[0], "\r\n";

		if(count($resp) > 1)
			return $resp[1];
		else
			return "";
	}
	else
		return "";
}

function car_goto($target_x, $target_y)
{
	global $pre_x, $pre_y;
	global $pre_vector_x, $pre_vector_y;
	global $target_step_1, $target_step_2;
	global $car_state;
	
	// calculate the rotate angle
	$vector_x  = $target_x - $pre_x;
	$vector_y  = $target_y - $pre_y;
	
	$length_1 = sqrt($pre_vector_x * $pre_vector_x + $pre_vector_y * $pre_vector_y);
	$length_2 = sqrt($vector_x * $vector_x + $vector_y * $vector_y);
	$cosin_alpha = ($pre_vector_x * $vector_x + $pre_vector_y * $vector_y) / ( $length_1 * $length_2);
	$angle = acos($cosin_alpha) * 180 / M_PI;
	
	$dir = $pre_vector_x * $vector_y - $pre_vector_y * $vector_x;
	
	//save new values
	$pre_x = $target_x;
	$pre_y = $target_y;
	$pre_vector_x = $vector_x;
	$pre_vector_y = $vector_y;
	
	$step_num = ($angle * TRACK) / (HALF_STEP_ANGLE * WHEEL_RADIUS) /2;
	
	if($dir < 0)
	{
		$target_step_1 += $step_num;
		$target_step_2 += $step_num;
	}
	else
	{
		$target_step_1 -= $step_num;
		$target_step_2 -= $step_num;
	}
	
	$pos_1 = round($target_step_1);
	$pos_2 = round($target_step_2);
	
	step_cmd(13, "goto $pos_1 400 1600");
	step_cmd(14, "goto $pos_2 400 1600");
	
	while((int)step_cmd(13, "get state"))
		usleep(1);
	while((int)step_cmd(14, "get state"))
		usleep(1);
	
	$dist = sqrt($vector_x*$vector_x + $vector_y*$vector_y);
	
	//millimeter to step
	$step_num = $dist / WHEEL_PERIMETER * HALF_STEP_NUM;
	
	$target_step_1 += $step_num;
	$target_step_2 -= $step_num;
	
	$pos_1 = round($target_step_1);
	$pos_2 = round($target_step_2);
	
	step_cmd(13, "goto $pos_1 400 1600");
	step_cmd(14, "goto $pos_2 400 1600");
	
	$car_state = STATE_MOVE;
}

function sensor_setup()
{
	// setup trigger pulse timer
	ht_ioctl(0, "set mode output pulse");
	ht_ioctl(0, "set div us");
	ht_ioctl(0, "set repc 1");
	ht_ioctl(0, "set count 5 10"); // 10us pulse width
	 
	// setup echo capture timer
	ht_ioctl(1, "reset");
	ht_ioctl(1, "set div us");
	ht_ioctl(1, "set mode capture toggle");
	ht_ioctl(1, "set trigger from pin rise");
	ht_ioctl(1, "set repc 4");
}

function sensor_read()
{
	ht_ioctl(1, "start"); // we should start capture timer first
	ht_ioctl(0, "start"); // start trigger pulse

	usleep(100000); // sleep 100ms
	ht_ioctl(1, "stop");

	// 1st capture value ("get count 0") is always zero.
	// we should get 2nd capture value;
	$us = ht_ioctl(1, "get count 1");
	
	if($us == 0)
		$dist = INF_DISTANCE;
	else
	{
		$dist = $us * 340.0 / 2; // us to meter conversion
		$dist = $dist / 10000; // meter to centimeter conversion
	}
	
	return $dist;
}

function car_loop()
{
	global $target_step_1, $target_step_2;
	global $car_state;
	
	if($car_state == STATE_FREE)
		return;
	
	$pos_1 = round($target_step_1);
	$pos_2 = round($target_step_2);
	
	if(sensor_read() < 5)
	{
		if($car_state == STATE_MOVE)
		{
			$car_state = STATE_PAUSE;
			
			step_cmd(13, "stop");
			step_cmd(14, "stop");
		}
		
		return;
	}
	else if($car_state == STATE_PAUSE)
	{
		$car_state = STATE_MOVE;
		
		step_cmd(13, "goto $pos_1 400 1600");
		step_cmd(14, "goto $pos_2 400 1600");
	}
	
	$cur_step_1 = (float)step_cmd(13, "get pos");
	$cur_step_2 = (float)step_cmd(14, "get pos");
	
	if($cur_step_1 == $pos_1 && $cur_step_2 == $pos_2)
		$car_state = STATE_FREE;
}

function network_loop()
{
	if(ws_state(0) == TCP_CONNECTED)
	{
		$rbuf = "";
		$rlen = ws_read_line(0, $rbuf);
		
		if($rlen)
		{
			$data = explode(" ", $rbuf);
			$target_x = (int)$data[1];
			$target_y = (int)$data[2];
			
			//pixel to millimeter 
			$ratio = MAP_IMG_WIDTH / MAP_REAL_WIDTH; //pixel / millimeter 
			$target_x /= $ratio;
			$target_y /= $ratio;
			
			car_goto($target_x, $target_y);
		}
	}
}

sensor_setup();
ws_setup(0, "step_motor_car", "csv.phpoc");
spc_reset();
spc_sync_baud(460800);

step_cmd(13, "set vref stop 2");
step_cmd(13, "set vref drive 14");
step_cmd(13, "set mode half");
step_cmd(13, "set rsnc 120 250");

step_cmd(14, "set vref stop 2");
step_cmd(14, "set vref drive 14");
step_cmd(14, "set mode half");
step_cmd(14, "set rsnc 120 250");

$pre_x = 16*450.0;
$pre_y = 22*450.0;
$pre_vector_x = 0;
$pre_vector_y = 1;

$target_step_1 = 0.0;
$target_step_2 = 0.0;

$car_state = STATE_FREE;

while(1)
{
	car_loop();
	
	if($car_state == STATE_FREE)
		network_loop();
}

?>
index.phpHTML
<!DOCTYPE html>
<html>
<head>
<title>PHPoC - Step Motor</title>
<meta name="viewport" content="width=device-width, initial-scale=0.7">
<style>
body {
	text-align: center;
	background-color: #33C7F2;
}
#cvs_frame {
	margin-right: auto;
	margin-left: auto; 
	position: relative;
	background-color: #FFFFFF;
}
.canvas {
	position: absolute; 
	left: 0px;
	top: 0px;
	overflow-y: auto;
	overflow-x: hidden;
	-webkit-overflow-scrolling: touch; /* nice webkit native scroll */
}

#layer_1 {
	z-index: 2;
}
#layer_2 {
	z-index: 1;
	background: url(map.png) no-repeat; 
	background-size: contain; 
	border: 1px solid #000;
}

</style>
<script>
var CMD_MOVE = 2;
var RESOLUTION = 20;

var IMAGE_WIDTH = 1140, IMAGE_HEIGHT = 1562;

var ws = null;
var layer_1 = null, layer_2 = null;
var cvs_frame = null;
var ctx1 = null, ctx2 = null;

var canvas_width = 0, canvas_height = 0;

var x = 0, y = 0; //position in image coordinate (pixel)
var touch_x = 0; touch_y = 0; //position in canvas coordinate (in pixel). 

var cvs_pos_x = 0, cvs_pos_y = 0;
var pre_cvs_pos_x = 0, pre_cvs_pos_y = 0;

function init()
{
	//initial position
	x = 16 * IMAGE_WIDTH / 25.0;
	y = 22 * IMAGE_HEIGHT / 34.0;
	
	cvs_frame = document.getElementById("cvs_frame");
	
	layer_1 = document.getElementById("layer_1");
	layer_2 = document.getElementById("layer_2");
	
	layer_1.addEventListener("touchstart", mouse_down);
	layer_1.addEventListener("touchend", mouse_up);
	layer_1.addEventListener("touchmove", mouse_move);
	layer_1.addEventListener("mousedown", mouse_down);
	layer_1.addEventListener("mouseup", mouse_up);
	layer_1.addEventListener("mousemove", mouse_move);
	
	ctx1 = layer_1.getContext("2d");
	ctx2 = layer_2.getContext("2d");
	
	canvas_resize();
}
function canvas_clear()
{
	ctx1.clearRect(0, 0, canvas_width, -canvas_height);
	ctx2.clearRect(0, 0, canvas_width, -canvas_height);
}
function event_handler(event, type)
{
	var pre_x = x, pre_y = y;
	// convert coordinate
	if(event.targetTouches)
	{
		var targetTouches = event.targetTouches;
		
		touch_x = targetTouches[0].pageX - cvs_frame.offsetLeft;
		touch_y = targetTouches[0].pageY - cvs_frame.offsetTop - canvas_height;
	}
	else
	{
		touch_x = event.offsetX;
		touch_y = event.offsetY - canvas_height;
	}
	
	var temp_x = Math.round(touch_x / canvas_width * IMAGE_WIDTH);
	var temp_y = Math.round((-touch_y) / canvas_height * IMAGE_HEIGHT); 
	
	if(type == "MOVE")
	{
		var delta_x = temp_x - pre_x;
		var delta_y = temp_y - pre_y;
		var dist = Math.sqrt(Math.pow(delta_x, 2) + Math.pow(delta_y, 2));
		
		if(dist < RESOLUTION)
			return false;
	}
	
	x = temp_x;
	y = temp_y;
	
	return true;
}
function ws_onmessage(e_msg)
{
	var arr = JSON.parse(e_msg.data);
	var pos_x			= arr[0];
	var pos_y			= arr[1];
	
	pre_cvs_pos_x = cvs_pos_x;
	pre_cvs_pos_y = cvs_pos_y;
	
	cvs_pos_x = Math.round(pos_x * canvas_width / MAX_X);
	cvs_pos_y = -Math.round(pos_y * canvas_height / MAX_Y);
	
	ctx1.clearRect(0, 0, canvas_width, -canvas_height);
	ctx1.beginPath();
	ctx1.arc(cvs_pos_x, cvs_pos_y, 7, 0, 2*Math.PI);
	ctx1.fill();
	
	//ctx2.lineTo(cvs_pos_x, cvs_pos_y);
	//ctx2.stroke();
}
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")?>/step_motor_car", "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.targetTouches)
	{
		event.preventDefault();
		if(event.targetTouches.length > 1)
			return;
	}
	
	event_handler(event, "DOWN");
	
	if(ws == null || ws.readyState != 1)
		return;

	ws.send(CMD_MOVE + " " + x + " " + y + "\r\n"); 
	
	//console.log(x + ", " + y);
	console.log(touch_x + ", " + touch_y);
	
	ctx2.lineTo(touch_x, touch_y);
	ctx2.stroke();
}
function mouse_up()
{
	if(event.targetTouches)
	{
		event.preventDefault();
	}
	
	if(ws == null || ws.readyState != 1)
		return;
}
function mouse_move()
{
	if(event.targetTouches)
	{
		event.preventDefault();
		if(event.targetTouches.length > 1)
			return;
	}
	
	if(!event_handler(event, "MOVE"))
		return;
	
	if(ws == null || ws.readyState != 1)
		return;
	
	ws.send(CMD_MOVE + " " + x + " " + y + "\r\n"); 
	
	ctx2.lineTo(touch_x, touch_y);
	ctx2.stroke();
}
function canvas_resize()
{
	var width = window.innerWidth;
	var height = window.innerHeight;
	canvas_height = height - 100;
	canvas_width = Math.round(canvas_height / IMAGE_HEIGHT * IMAGE_WIDTH);
	
	cvs_frame.style.width = canvas_width + "px";
	cvs_frame.style.height = canvas_height + "px";
	
	layer_1.width = canvas_width;
	layer_1.height = canvas_height;
	ctx1.translate(0, canvas_height);
	ctx1.lineWidth = 5;
	ctx1.fillStyle = "green";
	
	layer_2.width = canvas_width;
	layer_2.height = canvas_height;
	ctx2.translate(0, canvas_height);
	ctx2.lineWidth = 5;
	ctx2.strokeStyle = "blue";
	
	var touch_x = Math.round(x * canvas_width / IMAGE_WIDTH);
	var touch_y = Math.round((-y) * canvas_height / IMAGE_HEIGHT);
	
	ctx1.beginPath();
	ctx1.arc(touch_x, touch_y, 3, 0, 2*Math.PI);
	ctx1.fill();
	ctx2.beginPath();
	ctx2.lineTo(touch_x, touch_y);
}

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

<body onresize="canvas_resize()">
<div id="cvs_frame">
	<canvas id="layer_1" class="canvas"></canvas>
	<canvas id="layer_2" class="canvas"></canvas>
</div>
<p>WebSocket : <span id="ws_state">null</span></p>
<button id="wc_conn" type="button" onclick="wc_onclick();">Connect</button>
<button type="button" onclick="canvas_clear();">Clear</button>
</body>
</html>
init.phpPHP
<?php
system ("php task0.php");
?>

Credits

Photo
loper zand
1 project • 2 followers
Contact
E06a418d5e1b9a059d133637eb19b069
김용진
1 project • 1 follower
Contact
 gxbnoqxssw
gledel
2 projects • 20 followers
Looking back on my childhood, I was happy when I was making something and I was proud of myself. "Making is instinct!"
Contact
10354686 10150004552801856 220367501106153455 n
김형준
1 project • 1 follower
Contact
Thanks to phpoc_man.

Replications

Did you replicate this project? Share it!

I made one

Love this project? Think it could be improved? Tell us what you think!

Give feedback

Comments

Sign up / LoginProjectsPlatformsTopicsContestsLiveAppsBetaBlog