Linear Stage in Python

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

[source code]

import pilib
import time
import random

MOTOR_CONF = {
            "elec_cycle_div":       50000,
            "kp":                   1250.0,
            "ki":                   10.0,
            "kd":                   340.0,
            "ilim":                 1000,
            "max_pid_output":       30000,
            "enc_counts_per_rot":   2048,
            "poles":                4,
            "phases":               3,
            "reset_current":        28000,
            "reset_wait_time_ms":   3000,
            "max_pos_error":        1500,
            "use_aux_enc_for_servo":0,
            "output_polarity":      0,
            "encoder_polarity":     0
        }
        
class Stage(pilib.Pidev):
    def __init__(self, addr=None, password = None, port = 0):
        pilib.Pidev.__init__(self, addr, password, port)
        
        # configure limit switches
        self.gpio_set_direction(0xff, 0b11111110) # gpio 0 is output, all others input
        self.gpio_set_pullup(0xff, 0b00000110)    # pullup on gpio 1 & 2
        self.gpio_set_value(0xff, 0x00)           # set low   
    
        # get off limit switches to reset motor
        if self.gpio_get_value(0xff) != 0:
            self.mtr_set_pwr_enable(0)
            print ('Move stage to the center...')
            while self.gpio_get_value(0xff) != 0:
                time.sleep(0.1)
            time.sleep(2)
            print('thanks!')
            
        # configure the motor
        self.reset_motor(MOTOR_CONF)
    
        self.last_vel = 0
        
        start_pos = self.mtr_get_actual_pos()
        left = self.find_endstop(-500, 0b00000010)
        self.move_to(start_pos)
        right = self.find_endstop(500, 0b00000100)
        self.positions = {'left': left, 'right': right}
        print self.positions
        
        
    def configure_motor(self, conf):
        
        self.mtr_set_mtr_type(0)
        
        # set the pid and trajectory generator update rate
        self.mtr_set_elec_cycle_div(conf["elec_cycle_div"])
        
        # set the pid gains (proportional, integral, derivitive)
        self.mtr_set_kp(conf["kp"])
        self.mtr_set_ki(conf["ki"])
        self.mtr_set_kd(conf["kd"])
        self.mtr_set_ilim(conf["ilim"])
        
        # limit the output of the amplifiers
        self.mtr_set_max_pid_output(conf["max_pid_output"])
        
        # set the number of encoder counts for one rotation of the motor
        self.mtr_set_enc_counts_per_rot(conf["enc_counts_per_rot"])
        self.mtr_set_enc_polarity(conf["encoder_polarity"])
        
        # configure the number of poles the motor has
        self.mtr_set_poles(conf["poles"])
        self.mtr_set_reset_current(conf["reset_current"])
        self.mtr_set_reset_wait_time_ms(conf["reset_wait_time_ms"])
        self.mtr_set_max_pos_error(conf["max_pos_error"])
        self.mtr_set_use_aux_enc_for_servo(conf["use_aux_enc_for_servo"])
        self.mtr_set_output_polarity(conf["output_polarity"])

    def reset_motor(self, conf):
        self.configure_motor(conf)
        self.mtr_set_pwr_enable(1)    
    
        # configure the motor phase waveforms
        self.mtr_write_phase_mem_default(conf["phases"], conf["poles"], conf["enc_counts_per_rot"])
        self.mtr_reset()
        while self.mtr_get_reset_state() != 0:
            print '.',
            time.sleep(0.1)
        print('Finished resetting.')
            
    def stop(self):
        pos = self.mtr_get_actual_pos()
        self.mtr_write_target_position(pos, 1)
        
    def start(self, segments):
        self.mtr_write_table(0,segments)
        self.mtr_start(0,pilib.TRIG_SRC_NONE) 

    def move_velocity(self, velocity, acceleration):
        profile = [{'velocity': velocity, 'acceleration': velocity}]
        segments = self.mtr_create_velocity_profile(self.last_vel, profile, servomode = 1, elec_cycle_div=MOTOR_CONF['elec_cycle_div'])
        self.start(segments)
        self.last_vel = velocity
    
    def find_endstop(self, velocity, gpio_mask):
        self.move_velocity(velocity, 1600)
        while (self.mtr_get_motion_error() == 0):
            pos = self.mtr_get_actual_pos()
            found = self.gpio_get_value(gpio_mask)
            if found != 0: 
                self.stop()
                self.mtr_set_motion_error(0)
                return pos

    def move_to(self, pos):
        actual_pos = self.mtr_get_actual_pos()
        print 'target =', pos, 'actual =', actual_pos, 'temp =', self.pidev_get_temperature()
        if pos != actual_pos:
            profile = [{'position': pos, 'velocity': 5000, 'acceleration': 8000}]
            segments = self.mtr_create_trapezoidal_profile(actual_pos, profile, elec_cycle_div=MOTOR_CONF['elec_cycle_div'])
            for i, s in enumerate(segments):
                print i, s
            self.start(segments)
            while self.mtr_get_profile_finished() == 0:
                time.sleep(0.01)
        
    def move_random(self):
        for x in range(1000000):
            pos = random.randint(self.positions['left'] + 50, self.positions['right'] - 50)
            self.move_to(pos)
        
            
s = Stage('192.168.1.200')
s.move_random()

print 'Done'