모터 제어기

//task0.php
<?php
include_once "/lib/sd_spc.php";

spc_reset(); // reset all smart slaves stacked on P4S-34X
spc_sync_baud(115200); // synchronize master to slave baud-rate

spc_request_dev(1, "set vref stop 4");   // set stop current to 4/15
spc_request_dev(1, "set vref drive 15"); // set drive current to 15/15
spc_request_dev(1, "set mode 32");       // set micro-step to 1/32

spc_request_dev(2, "set vref stop 4");   // set stop current to 4/15
spc_request_dev(2, "set vref drive 15"); // set drive current to 15/15
spc_request_dev(2, "set mode 32");       // set micro-step to 1/32

while(1)
{
  for($i = 0; $i < 4; $i++)
  {
    spc_request_dev(1, "move +16000 128000 1280000");
    spc_request_dev(2, "move -16000 128000 1280000");
	
    while((int)spc_request_dev(1, "get state"))
      usleep(1);
    usleep(200000);
  }

  $pos1 = -(int)spc_request_dev(1, "get pos");
  $pos2 = -(int)spc_request_dev(2, "get pos");

  spc_request_dev(1, "move $pos1"); // return to initial position
  spc_request_dev(2, "move $pos2"); // return to initial position
  
  while((int)spc_request_dev(1, "get state"))
    usleep(1);
  usleep(200000);
}
?>

//task0.php
<?php
include_once "/lib/sd_340.php";
include_once "/lib/sd_spc.php";
include_once "/lib/sn_tcp_ws.php";

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

define("SID_X",      13);
define("SID_Y",      14);

define("MAX_X",      1710);
define("MAX_Y",      2140);
define("TOUCH_OFFSET",  100);

define("PEN_UP",     0);
define("PEN_DOWN",   1);

define("CMD_PEN_UP",   0);
define("CMD_PEN_DOWN", 1);
define("CMD_MOVE",     2);

define("SPEED_X_COEF",    20);
define("SPEED_Y_COEF",    20);
define("SPEED_X_OFFSET",  50);
define("SPEED_Y_OFFSET",  50);
define("SPEED_X_MAX",     5000);
define("SPEED_Y_MAX",     5000);

define("ACCEL_X_COEF",    1500);
define("ACCEL_Y_COEF",    1000);
define("ACCEL_X_OFFSET",  0);
define("ACCEL_Y_OFFSET",  0);
define("ACCEL_X_MAX",     20000);
define("ACCEL_Y_MAX",     20000);

define("STATE_X_LOCK",  1);
define("STATE_X_RUN",   2);
define("STATE_Y_LOCK",  4);
define("STATE_Y_RUN",   8);


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";
      return false;
    }

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

function spc_check_did($sid, $did)
{
  $resp = spc_request_csv($sid, 0, "get did");

  if($resp === false)
  {
    echo "spc_check_did: sid$sid - device not found\r\n";
    return false;
  }

  if($resp[1] != "40002405")
  {
    echo "spc_check_did: unknown device ", $resp[2], "\r\n";
    return false;
  }

  return true;
}

function pen_up()
{
  $angle = 110;

  $width = WIDTH_MIN + (int)round((WIDTH_MAX - WIDTH_MIN) * $angle / 180.0);

  if(($width >= WIDTH_MIN) && ($width <= WIDTH_MAX))
    ht_pwm_width(2, $width, PWM_PERIOD);
}

function pen_down()
{
  $angle = 180;

  $width = WIDTH_MIN + (int)round((WIDTH_MAX - WIDTH_MIN) * $angle / 180.0);

  if(($width >= WIDTH_MIN) && ($width <= WIDTH_MAX))
    ht_pwm_width(2, $width, PWM_PERIOD);
}

function xy_goto($x, $y)
{
  global $mode;
  
  if($x < TOUCH_OFFSET)
    $x = TOUCH_OFFSET;
  if($x > (MAX_X - TOUCH_OFFSET))
    $x = MAX_X - TOUCH_OFFSET;
  
  if($y < TOUCH_OFFSET)
    $y = TOUCH_OFFSET;
  if($y > (MAX_Y - TOUCH_OFFSET))
    $y = MAX_Y - TOUCH_OFFSET;
  
  $x0 = (int)step_cmd(SID_X, "get pos") / $mode;
  $y0 = (int)step_cmd(SID_Y, "get pos") / $mode;
  
  $delta_x = $x - $x0; 
  $delta_y = $y - $y0;
  
  if($delta_x == 0 && $delta_y == 0)
    return;
  
  $speed_x = SPEED_X_COEF * abs($delta_x) + SPEED_X_OFFSET;
  $speed_y = SPEED_Y_COEF * abs($delta_y) + SPEED_Y_OFFSET;
  
  $x *= $mode; 
  $y *= $mode;
  $speed_x *= $mode;
  $speed_y *= $mode;
  
  if($speed_x > SPEED_X_MAX * $mode)
    $speed_x = SPEED_X_MAX * $mode;
  
  if($speed_y > SPEED_Y_MAX * $mode)
    $speed_y = SPEED_Y_MAX * $mode;
  
  $accel_x = ACCEL_X_COEF * $speed_x + ACCEL_X_OFFSET;
  $accel_y = ACCEL_Y_COEF * $speed_y + ACCEL_Y_OFFSET;
  
  if($accel_x > ACCEL_X_MAX * $mode)
    $accel_x = ACCEL_X_MAX * $mode;
  
  if($accel_y > ACCEL_Y_MAX * $mode)
    $accel_y = ACCEL_Y_MAX * $mode;
  
  if($delta_x == 0)
  {
    step_cmd(SID_Y, "goto $y $speed_y $accel_y");
  }
  else if($delta_y == 0)
  {
    step_cmd(SID_X, "goto $x $speed_x $accel_x");
  }
  else
  {
    step_cmd(SID_X, "goto $x $speed_x $accel_x");
    step_cmd(SID_Y, "goto $y $speed_y $accel_y");
  }
  //echo "state: ", step_cmd(SID_Y, "get state"), "\r\n";
  //echo SID_X, ", goto $x $speed_x $accel_x\r\n";
  //echo SID_Y, ", goto $y $speed_y $accel_y\r\n";
}

function xy_state()
{
  $ret_val = 0;
  
  $x_state = (int)step_cmd(SID_X, "get state");
  $y_state = (int)step_cmd(SID_Y, "get state");
  
  if($x_state == 1) // motor is locked
    $ret_val |= STATE_X_LOCK;
  else if($x_state > 1) // motor is running
    $ret_val |= STATE_X_RUN;
    
  if($y_state == 1) // motor is locked
    $ret_val |= STATE_Y_LOCK;
  else if($y_state > 1) // motor is running
    $ret_val |= STATE_Y_RUN;
    
  return $ret_val;
}

function xy_wait()
{
  while(xy_state() & (STATE_X_RUN | STATE_Y_RUN))
    usleep(1);
}

function xy_init()
{
  global $mode;
  
  $width = WIDTH_MIN + (int)round((WIDTH_MAX - WIDTH_MIN) * 110 / 180.0);
  ht_pwm_setup(2, $width, PWM_PERIOD);
  pen_up();
  
  spc_reset();
  spc_sync_baud(460800);

  if(!spc_check_did(SID_X, "40002405"))
    return;
  if(!spc_check_did(SID_Y, "40002405"))
    return;

  step_cmd(SID_X, "set vref stop 4");
  step_cmd(SID_X, "set vref drive 15");
  step_cmd(SID_X, "set mode $mode");
  step_cmd(SID_X, "set rsnc 120 250");
  
  step_cmd(SID_Y, "set vref stop 4");
  step_cmd(SID_Y, "set vref drive 15");
  step_cmd(SID_Y, "set mode $mode");
  step_cmd(SID_Y, "set rsnc 120 250");
  
  // move pen to (0, 0)
  $v = 800 * $mode;
  $a = 10000 * $mode;
  step_cmd(SID_X, "goto -sw1 $v $a");
  step_cmd(SID_Y, "goto -sw1 $v $a");
  xy_wait();
  step_cmd(SID_X, "reset");
  step_cmd(SID_Y, "reset");
  /*
  $time_stemp = st_free_get_count(0);
  step_cmd(SID_X, "goto 100 $v $a");
  step_cmd(SID_Y, "goto 100 $v $a");
  xy_wait();
  $time_stemp = st_free_get_count(0) - $time_stemp;
  
  echo $time_stemp;
  
  step_cmd(SID_X, "reset");
  step_cmd(SID_Y, "reset");
  */
  xy_goto(TOUCH_OFFSET, TOUCH_OFFSET);
  xy_wait();
  
  // uncomment this block for the first run and change the value in line 45 of index.php
  /*
  // check max steps
  step_cmd(SID_X, "goto +sw0 1600 20000");
  step_cmd(SID_Y, "goto +sw0 1600 20000");
  
  xy_wait();
  // change this value in line 45 of index.php: var MAX_X * $mode = 3421, MAX_Y * $mode = 4296;
  echo "Max X:", step_cmd(SID_X, "get pos"), "\r\n";
  echo "Max Y:", step_cmd(SID_Y, "get pos"), "\r\n";
  */
}

function send_position($pen_state)
{
  global $mode;
  
  $x = (int)step_cmd(SID_X, "get pos") / $mode;
  $y = (int)step_cmd(SID_Y, "get pos") / $mode;
  $wbuf = "[$x, $y, $pen_state]";
  ws_write(0, $wbuf);
}

$mode = 32;

ws_setup(0, "xy_plotter", "csv.phpoc");
xy_init();

$pre_x = 0;
$pre_y = 0;
$cur_x = 0;
$cur_y = 0;
$rbuf = "";
$pen_state = CMD_PEN_UP;
$x_is_unlock = false;
$y_is_unlock = false;

while(1)
{
  if(ws_state(0) == TCP_CONNECTED)
  {
    $rlen = ws_read_line(0, $rbuf);
    
    if($rlen)
    {
      $data = explode(" ", $rbuf);
      $cmd = (int)$data[0];
      
      switch($cmd)
      {
        case CMD_PEN_DOWN:
          $x = (int)$data[1];
          $y = (int)$data[2];
          xy_goto($x, $y);
          xy_wait();
          
          pen_down();
          $pen_state = PEN_DOWN;
          
          send_position($pen_state);
          
          break;
        case CMD_PEN_UP:
          pen_up();
          $pen_state = PEN_UP;
          
          send_position($pen_state);
          
          break;
        case CMD_MOVE:
          $x = (int)$data[1];
          $y = (int)$data[2];
          xy_goto($x, $y);
          //xy_wait();
          
          break;
      }
    }
  }
  
  $cur_x = (int)step_cmd(SID_X, "get pos") / $mode;
  $cur_y = (int)step_cmd(SID_Y, "get pos") / $mode;
  
  if(abs($pre_x - $cur_x) > 10 || abs($pre_y - $cur_y) > 10)
  {
    $pre_x = $cur_x;
    $pre_y = $cur_y;
    send_position($pen_state);
  }
  
  $xy_state = xy_state();
  
  if(($xy_state & STATE_X_LOCK) && ($x_is_unlock == false))
  {
    step_cmd(SID_X, "unlock");
    $x_is_unlock = true;
  }
  else if(($xy_state & STATE_X_RUN))
  {
    step_cmd(SID_X, "eio set 0 mode lock");
    step_cmd(SID_X, "eio set 1 mode lock");
    $x_is_unlock = false;
  }
  
  if(($xy_state & STATE_Y_LOCK) && ($y_is_unlock == false))
  {
    step_cmd(SID_Y, "unlock");
    $y_is_unlock = true;
  }
  else if(($xy_state & STATE_Y_RUN))
  {
    step_cmd(SID_Y, "eio set 0 mode lock");
    step_cmd(SID_Y, "eio set 1 mode lock");
    $y_is_unlock = false;
  }
}
?>