/*
 * Exponential position change in user units script
 *
 * The script performs discrete control of movement according to a certain law of motion.
 * The amplitude and the law of displacement are given. 
 * A correction speed is used to maintain the positioning accuracy.
 */

// Main characteristics
var time_discre = 10; // Discreteness of movement control(ms)
var end_err = 0.01; // The accuracy of reaching the final coordinate of the movement.

var full_move = 6; // Total movement in mm

// If you change the equation of motion, you will need to change the equation for the correction velocity, since this change is not linear.
var K = 0.4;
var Glob_err = 0;

// Advanced setting.
var mm_per_step = 0.00125; // Distance in gr for 1 completed step.
var calb = new_calibration(mm_per_step, get_engine_settings().MicrostepMode); // create calibration structure

// Setting the starting position.
command_stop(); // send STOP command (does immediate stop)
command_wait_for_stop(10); // wait until controller stops moving
command_zero(); // send ZERO command (sets current position and encoder value to zero)


log("Start:");
// Setting 0 speeds and accelerations.
zero_movesettings();

// 
go_position(full_move, time_discre)


// Function for calculating the set speed and acceleration
function set_movesettings(time, corr_speed)
{
  // Speed, Accel, Decel setting.
  var m = get_move_settings_calb(calb); // read movement settings from the controller

  // The equation of speed is equal to the derivative of the equation of motion.
  m.Speed =  K*Math.exp(K*time/1000 )+ corr_speed;
  
  set_move_settings_calb(m, calb); // write movement settings into the controller

 log("Speed = " +m.Speed);
log("corr_speed = " +corr_speed);
}

// Set the initial parameters of motion
function zero_movesettings()
{
  var m = get_move_settings_calb(calb); // read movement settings from the controller
  m.Speed = 0.1; // set movement speed
  
  set_move_settings_calb(m, calb); // write movement settings into the controller
}

// A function of calculating target coordinates from time
function current_target_coordinate(time)
{
  // The equation of motion. The time is set in milliseconds. The position at the initial time is 0.
  return (Math.exp(K*time/1000) - 1);
}

// The calculation of the correction speed
function speed_corr(err_pos)
{
  Glob_err = Glob_err + err_pos*0.35;
  return  Glob_err;
}

// The main moving
function go_position(full_time, time_discre)
{
  Glob_err = 0;
  var end_position = full_move;
  
  // Setting the movement to the desired coordinate.
  command_move_calb(end_position, calb);
  
  // Pause before starting to move, to turn on the power button.
  msleep(300);
  var mas = 1;
  var basetime = new Date();
  var curr_time = new Date();
  var err_pos = 0;
  var pos = 0;
  var pos1 = 0;
  var i = time_discre;
  do {
    // If you do not need position feedback, you can instead speed_corr(err_pos) write 0
    set_movesettings(i+1, speed_corr(err_pos));

    // Waiting for the end of a discrete time interval
    do { 
      curr_time = new Date - basetime;
      msleep(1);
    }
    while ((curr_time) < i); //
    
    // Reading the actual and calculating the planned coordinate if used.
    pos =  get_position_calb(calb).Position;
    pos1 =  current_target_coordinate(i);

    // Calculation of the position error.
    err_pos =  (pos1 -  pos);

    log("time = " + i);
    log("err_pos = " + err_pos);
    
    i = i + time_discre
  }
  while (Math.abs(pos - end_position) > end_err); // Completion when the end of the movement is reached with the specified accuracy.
}