Linear Stage in Java

This example demonstrates the initialization of a linear stage and then randomly generated moves between the limit switches.

[MyPimotionTest.java]

public class MyPimotionTest 
{
  public static Stage s;

  public static void main(String[] args) 
  {
    s = new Stage("192.168.1.200");
    while (true) {
        s.move_to_random(true);
    }
  }

}

[Stage.java]

import com.vena.Pilib;
import com.vena.Pilib.MtrSegment;
import com.vena.Pilib.TrapezoidalEntry;
import com.vena.Pilib.VelocityEntry;
import com.vena.Pilib.Trigger;
import com.vena.Pilib.ServoMode;

import java.util.Random;

public class Stage extends Pilib {

 public class MtrConf {
    public int elec_cycle_div =       50000;
    public double kp =                 1250.0;
    public double ki =                 10.0;
    public double kd =                 340.0;
    public int ilim =                 1000;
    public int max_pid_output =       30000;
    public int enc_counts_per_rot =   2048;
    public int poles =                4;
    public int phases =               3;
    public int reset_current =        28000;
    public int reset_wait_time_ms =   3000;
    public int max_pos_error =        1500;
    public int use_aux_enc_for_servo =0;
    public int output_polarity =      0;
    public int encoder_polarity =     0;
  }  

 public Pilib p;
  MtrConf conf;
  long left;
  long right;
  TrapezoidalEntry[] entries;
  MtrSegment[] segments;
  int num_segments;
  double last_vel;
  Random rand;

  public Stage(String address)
  {
    super(address,"password",0);
    conf = new MtrConf();
    rand = new Random();
    
    entries = new TrapezoidalEntry[1];
    entries[0] = new TrapezoidalEntry(0., 0, 5000., 8000., 8000.);

    segments = new MtrSegment[3];
    segments[0] = new MtrSegment();
    segments[1] = new MtrSegment();
    segments[2] = new MtrSegment();

    // configure limit switches
    gpio_set_direction(0xff, 0b11111110); // gpio 0 is output, all others input
    gpio_set_pullup(0xff, 0b00000110);    // pullup on gpio 1 & 2
    gpio_set_value(0xff, 0x00);           // set low   

    // get off limit switches to reset motor
    if (gpio_get_value(0xff) != 0) {
      mtr_set_pwr_enable(0);
      print ("Move stage to the center...");
      while (gpio_get_value(0xff) != 0) {
        sleep(0.1);
      }
      sleep(2);
      print("thanks!");
    }

    reset_motor();

    last_vel = 0.0;
    long start_pos = mtr_get_actual_pos();

    find_endstop(-500, 0b00000010);   
    find_endstop(500, 0b00000000);      
    left = find_endstop(-50, 0b00000010);  
    
    move_to(start_pos, true);
    
    find_endstop(500, 0b00000100);
    find_endstop(-500, 0b00000000);
    right = find_endstop(50, 0b00000100);
    
    move_to(start_pos, true);

  }  

  public void configure_motor()
  {
    mtr_set_mtr_type(0);

    // set the pid and trajectory generator update rate
    mtr_set_elec_cycle_div(conf.elec_cycle_div);

    // set the pid gains (proportional, integral, derivitive)
    mtr_set_kp(conf.kp);
    mtr_set_ki(conf.ki);
    mtr_set_kd(conf.kd);
    mtr_set_ilim(conf.ilim);

    // limit the output of the amplifiers
    mtr_set_max_pid_output(conf.max_pid_output);

    // set the number of encoder counts for one rotation of the motor
    mtr_set_enc_counts_per_rot(conf.enc_counts_per_rot);
    mtr_set_enc_polarity(conf.encoder_polarity);

    // configure the number of poles the motor has
    mtr_set_poles(conf.poles);
    mtr_set_reset_current(conf.reset_current);
    mtr_set_reset_wait_time_ms(conf.reset_wait_time_ms);
    mtr_set_max_pos_error(conf.max_pos_error);
    mtr_set_use_aux_enc_for_servo(conf.use_aux_enc_for_servo);
    mtr_set_output_polarity(conf.output_polarity);
  }

  public void reset_motor()
  {
    configure_motor();
    mtr_set_pwr_enable(1);  

    // configure the motor phase waveforms
    mtr_write_phase_mem_default(conf.phases, conf.poles, conf.enc_counts_per_rot);
    mtr_reset();
    print ("Resetting...");
    while (mtr_get_reset_state() != 0) {
      sleep(0.1);
    }
    print("Finished resetting.");
  }

  public void stop()
  {
    long pos = mtr_get_actual_pos();
    mtr_write_target_position(pos, 1);     
  }

  public void start()
  {
    mtr_write_table(0, num_segments, segments);
    mtr_start(0,Trigger.TRIG_SRC_NONE);
  }

  public void move_velocity(double velocity, double acceleration)
  {
    VelocityEntry profile[] = new VelocityEntry[1];
    profile[0] = new VelocityEntry(0.0, velocity, acceleration);

    num_segments = mtr_create_velocity_profile(last_vel, profile, ServoMode.SERVOMODE_POSITION, conf.elec_cycle_div, segments);
    start();
    last_vel = velocity;
  }
  

  public long find_endstop(int velocity, int gpio_val) 
  {
    move_velocity(velocity, 1600);
    while (mtr_get_motion_error() == 0) 
    {
      long pos = mtr_get_actual_pos();
      int val = gpio_get_value(0b00000110);
      if (val == gpio_val) 
      {
        stop();
        mtr_set_motion_error(0);
        return pos;
      }
    }
    return 0;
  }
  
  public void move_to(long pos, boolean wait_for_complete)
  {
    long actual_pos = mtr_get_actual_pos();
    if (actual_pos != pos) {
      entries[0].position = pos;
      num_segments = mtr_create_trapezoidal_profile(actual_pos, entries, conf.elec_cycle_div, segments);
      start();
      while (wait_for_complete && (mtr_get_profile_finished() == 0)) {
        sleep(0.1);
      }
    }
  }
  
  public void move_to_random(boolean wait_for_complete) 
  {
      long pos = randlong(left + 50, right - 50);
      move_to(pos, wait_for_complete);
  }

  public long randlong(long min, long max)
  {
    return rand.nextInt((int)((max - min) + 1)) + min;
  }

  public void print(String data)
  {
    System.out.println(data);      
  }

  public void sleep(double sec)
  {
    int ms = (int)(sec * 1000.0);
    try {
        Thread.sleep(ms);
    } catch(InterruptedException ex) {
        Thread.currentThread().interrupt();
    }

  }

  public void print_temp()
  {
    double temperature = pidev_get_temperature();
    System.out.format("The temperature is %.2f C\n", temperature);
  }
}