/*
 * Maze script
 *
 * Description of the script: 
 *  Script for going through maze.
 * 
 * Note:
 *  This is a rather difficult script to learn, since it uses a large number of commands and structures.
 * 	The script was created specifically for joystick control.
 *
 * Important: 
 *  To run the script, upload it to the mDrive Direct Control software
 */

// Enter desired speed for going through the maze
var speed_mm = 30;

// Enter the serial numbers of your axes.
serial_number_x = 235;
serial_number_y = 243;

// one maze cell size in steps
var cell_size_steps = 1600;

var MAX_JOY = 4000;

var axis_x = new_axis(serial_number_x);
var axis_y = new_axis(serial_number_y);

// set axes speed
set_axes_speed(axis_x, axis_y, speed_mm);

// main
joystick_off(axis_x, axis_y);

var stop_flag = 0;

var chart_t = axis_x.get_chart_data();
var joystick_zero_x = chart_t.Joy;

var chart_t = axis_y.get_chart_data();
var joystick_zero_y = chart_t.Joy;

// perform homing
axis_x.command_home();
axis_x.command_wait_for_stop(100); // wait until controller stops moving
axis_y.command_home();
axis_y.command_wait_for_stop(100); // wait until controller stops moving

// zero position
axis_x.command_zero();
axis_y.command_zero();

axis_x.command_movr(11200); // send MOVR command (does a relative shift)
axis_x.command_wait_for_stop(100); // wait until controller stops moving

axis_y.command_movr(-1200); // send MOVR command (does a relative shift)
axis_y.command_wait_for_stop(100); // wait until controller stops moving

while( !(stop_flag))
{
   go_maze(axis_x, axis_y);
   go_maze_start(axis_x, axis_y);
}

joystick_on(axis_x, axis_y);

// special methods for going through the maze
function go_maze(x_axis, y_axis) {

  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 2, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 1, 1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 3, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 1, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 2, 1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 1, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 2, 1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 3, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 2, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 2, 1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 3, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 2, -1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 1, 1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 1, 1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(y_axis, 1, 1);
  if (stop_flag) {
    return;
  }

  go_maze_squares(x_axis, 2, -1);
  if (stop_flag) {
    return;
  }

  
}

function go_maze_start(x_axis, y_axis) {

   if (stop_flag) {
    return;
  }

   go_maze_squares(y_axis, 4, 1);
  if (stop_flag) {
    return;
  }

   go_maze_squares(x_axis, 5, 1);
  if (stop_flag) {
    return;
  }

}


function go_maze_squares(axis, squares_n, direction) {

  axis.command_movr(squares_n * cell_size_steps * direction); // send MOVR command (does a relative shift)
  // axis.command_wait_for_stop(100); // wait until controller stops moving
  local_wait_for_stop(axis, 100);
}

function joystick_on(x_axis, y_axis) {

   axis_x_control = x_axis.get_control_settings();
   axis_x_control.Flags = 1;
   x_axis.set_control_settings(axis_x_control);

   axis_y_control = y_axis.get_control_settings();
   axis_y_control.Flags = 1;
   y_axis.set_control_settings(axis_y_control);
}

// service methods for axes settings
function joystick_off(x_axis, y_axis) {

   axis_x_control = x_axis.get_control_settings();
   axis_x_control.Flags = 2;
   x_axis.set_control_settings(axis_x_control);

   axis_y_control = y_axis.get_control_settings();
   axis_y_control.Flags = 2;
   y_axis.set_control_settings(axis_y_control);
}

function set_axes_speed(x_axis, y_axis, speed_in_mm) {
  // set desired speed for two axis
  var m = x_axis.get_move_settings(); // read movement settings from the controller
  m.Speed = speed_mm * 400; 
  x_axis.set_move_settings(m); // write movement settings into the controller

  var m = y_axis.get_move_settings(); // read movement settings from the controller
  m.Speed = speed_mm * 400; 
  y_axis.set_move_settings(m); // write movement settings into the controller
}

function local_wait_for_stop(axis, refresh_period) {

  var move_stop_flag = 0; 

  while (!(move_stop_flag))
  {
    // check status and joysticks
    var status_x = axis.get_status();
    if (!(status_x.MvCmdSts & MVCMD_RUNNING))
    {
      move_stop_flag = 1; 
     }

     // check joystick
     var joystick_move = 0;
     // check
     var chart_t = axis_x.get_chart_data();
      if ((chart_t.Joy - joystick_zero_x)  *  (chart_t.Joy - joystick_zero_x) > MAX_JOY * MAX_JOY)
     {
        stop_flag = 1;
      }

      var chart_t = axis_y.get_chart_data();
      if ((chart_t.Joy - joystick_zero_y)  *  (chart_t.Joy - joystick_zero_y) > MAX_JOY * MAX_JOY)
     {
        stop_flag = 1;
      }

     msleep(refresh_period);
   }

}