source: ETALON/BPM/initialise_motors.py @ 787

# This Python file uses the following encoding: latin-1

# Defines all the functions and variables needed to control the motors with the WAGO controller
# July 2015, Nicolas Delerue
__author__ = 'delerue'

from bpm_constants import *
from read_adc import *
import numpy as np
import matplotlib.pyplot as plt
from datetime import datetime

VERBOSE_DEFAULT=0

global write_to_motor_dated_log
write_to_motor_dated_log = data_dir + './write_to_motor_log'

#from writeclient import *
import time

# basic functions
def write_to_motor(msg, verbose=VERBOSE_DEFAULT):
    "This function sends a message to the motor controller"
    ret = writeclient(motor_controller_IP, motor_controller_port, msg, verbose,datedlog=write_to_motor_dated_log)
    time.sleep(0.5)
    return ret


def motor_send(message, verbose=VERBOSE_DEFAULT):
    "Writes a formatted message to a motor"
    if (len(message) <= 12):
        print 'Message length ', len(message), ' >', message, '<'
    while len(message) < 12:
        message += '0'
    if len(message) <= 12:
        return write_to_motor(message, verbose)
    else:
        if verbose == 1:
            print 'Command too long'
        return 'Command too long'


def motor_enable(motor, verbose=0):
    "Enables a motor"
    msg = str(motor) + 'CE'
    motor_send(msg, verbose)


def motor_disable(motor, verbose=0):
    "Enables a motor"
    if motor == 2: # security to avoid issue with vertical motor
        print('are you sure to wanted to disable vertical motor ? \n yes or no')
        x = raw_input()
        while not (x == 'yes' or x == 'no' or x == 'y' or x == 'n' or x == 'YES' or x == 'NO' or x == 'Y' or x == 'N'):
            print('Do you want to disable vertical motor ?')
            x = raw_input()
        if (x == 'no' or x == 'n' or  x == 'NO' or x == 'N'):
            print('Be carful with disable vertical motor')
        else :
            move_motor_absolute(2,2000,0)
            while motor_get(2, 'status') != 'READY':
                time.sleep(1)
            msg = str(motor) + 'CD'
            motor_send(msg, verbose)
    else :
        msg = str(motor) + 'CD'
        motor_send(msg, verbose)


def motor_reset(motor, verbose=0):
    "Enables a motor"
    msg = str(motor) + 'CR'
    motor_send(msg, verbose)


def motor_start(motor, verbose=0):
    "Starts the movement"
    msg = str(motor) + 'GG'
    motor_send(msg, verbose)


def motor_stop(motor, verbose=0):
    "Stops the movement"
    msg = str(motor) + 'GS'
    motor_send(msg, verbose)

### Advaned functions
def motor_set(motor, param, value):
    "save parameter defined in the variable 'param', with value 'value'"
    motor=str(motor)
    if (param == 'velocity'):
        if (value > 65535):
            return 'value is too big'
        else:
            return motor_send(motor + ('SV%09d' % (abs(float(value)))))
    elif (param == 'acceleration'):
        if (value > 65535):
            return 'value is too big'
        else:
            return motor_send(motor + 'SA%09d' % (abs(value)))
    elif (param == 'deceleration'):
        if (value > 65535):
            return 'value is too big'
        else:
            return motor_send(motor + 'SD%09d'(abs(value)))
    elif (param == 'target_position'):
        if value > 0:
            direction = 'P'
        else:
            direction = 'p'
        return motor_send(motor + 'S' + direction + '%09d' % abs(value))
    elif (param == 'actual_position'):
        motor_enable(motor)
        motor_reset(motor)
        if value > 0:
            direction = 'C'
        else:
            direction = 'c'
        ret=motor_send(motor + 'S' + direction + '%09d' % abs(value))
        motor_disable(motor)
        return ret
    else:
        return 'Unknown parameter'


def motor_get(motor, param):
    """get parameter defined in variable param, from the motor driver"""
    motor=str(motor)
    if (param == 'velocity'):
        velocity = motor_send(motor + 'QV')
        velo=float(velocity[3:12])
        print velo
        return velo
    elif (param == 'acceleration'):
        acceleration = motor_send(motor + 'QA')
        accel=float(acceleration[3:12])
        print accel
        return accel
    elif (param == 'deceleration'):
        deceleration = motor_send([motor + 'QD'])
        return deceleration[3:12]
    elif (param == 'actual_speed'):
        actual_speed = motor_send(motor + 'QC')
        isign = 1
        if actual_speed[2] == 'c':
            isign = -1
        return isign * actual_speed[3:12]
    elif (param == 'actual_position'):
        actual_position = motor_send(motor + 'QP')
        if actual_position[2] == 'p':
            isign = -1
        else:
            isign = 1
        print actual_position[2:13]
        print actual_position[3:12]
        pos = isign * float(actual_position[3:12])
        print 'actual position=', pos
        return pos
    elif (param == 'status'):
        status = motor_send(motor + 'QS')
        status = status[3]
        if status == 'R':
            ret='READY'
        elif status == 'B':
            ret='BUSY'
        elif status == 'E':
            ret='ERROR'
        else:
            ret="Unknown "+status
        print ret
        return ret
    elif (param == 'reference'):
        reference = motor_send(motor + 'QR')
        if float(reference[3:12]) == 2:
            return 1
        else:
            return 0
    elif param == 'limit':
        limit = motor_send(motor + 'L')
        if (limit[1] == 'l'):
            return 'No limit'
        elif ((limit[1] == 'L') and (limit[2] == 'B')):
            return 'Back limit'
        elif ((limit[1] == 'L') and (limit[2] == 'F')):
            return "Front limit"
        else:
            return 'limit unknown'
    else:
        return 'Request unknown'


def motor_move_relative(motor, speed, steps):
    "Moves a motor with the given parameters"
    motor=str(motor)
    motor_set(motor, 'velocity', speed)
    motor_set(motor, 'target_position', steps)
    motor_send(motor + 'GR')
    motor_start(motor)

def move_motor_relative(motor, speed, steps):
    motor_move_relative(motor, speed, steps)
    while motor_get(motor, 'status') != 'READY':
        time.sleep(1)


def motor_move_absolute(motor, speed, steps):
    "Moves a motor with the given parameters"
    motor=str(motor)
    motor_set(motor, 'velocity', speed)
    motor_set(motor, 'target_position', steps)
    motor_send(motor + 'GA')
    motor_start(motor)

def move_motor_absolute(motor,speed,steps):
    "Moves a motor with the given parameters"
    motor_move_absolute(motor, speed, steps)
    while motor_get(motor, 'status') != 'READY':
        time.sleep(1)


def motor_wait_for_ready(motor,verbose=VERBOSE_DEFAULT):
    motor_status=motor_get(motor,'status')
    iloop=1
    while ((iloop<1000) and (motor_status!='READY')):
        time.sleep(1)
        iloop=iloop+1
        motor_status=motor_get(motor,'status')
        if (iloop%10==0):
            motor_pos=motor_status=motor_get(motor,'actual_position')
    if (motor_status=='READY'):
        return 1
    else:
        return 0

def controller_hello():
    "Sends ça_va query to the motor and display reply"
    #    write_to_motor("ça_va?",1)
    ret=write_to_motor("ca_va?", 1)
    print "controller returned: ",ret
    return ret

def position_test_horizontal():
    print('write OK if the zero possition sting is at maxima rigth or NO if it isn\'t')
    x = raw_input()

    while not (x == 'OK' or x == 'NO'):
        print('you must check that the string won\'t touch the BPM, hence that te sting is at maxima right when you look from motor side at position 0, and then write OK \n or write NO')
        x = raw_input()

    if (x == 'NO'):
        print('Please don\'t forbid to move BPM at maxima right when you look from motor side befor to shut down the motor')
        exit()    
    else:
        motor_enable(1)
        move_motor_absolute(1,5000,400000)
        while motor_get(1, 'status') != 'READY':
            time.sleep(1)
        print('verification of position')
        time.sleep(5)
        move_motor_absolute(1,5000,0)
        while motor_get(1, 'status') != 'READY':
            time.sleep(1)
    motor_disable(1)

def path(bpm_name): # create the path of the file what will contain data
    idata = 0
    start_time_str=datetime.now().strftime("%Y%m%d")
    data_name_and_path = data_dir+"data/"
    if not os.path.exists(data_name_and_path) :
        os.makedirs(data_name_and_path)
    data_name_and_path = data_name_and_path+"BPM-number_x-motor-step_x-motor-mm_y-motor-step_y-motor_mm_Va_Vb_Vc_Vd_Sum_X_Y_"+bpm_name+"_"+start_time_str+"_"
    while os.path.exists(data_name_and_path+str(idata)+".txt") :
        idata+=1
    return(data_name_and_path+str(idata)+".txt")

def write_first_line(data_name_and_path, list_of_data):
    length = len(list_of_data)
    fichier = open(data_name_and_path, "w")
    for index in range(length):
        fichier.write(list_of_data[index])
        if index != length - 1:
            fichier.write(" ")
    fichier.write("\n")
    fichier.close()
    
def horizontal_acquisition(begin, end, number_of_point, # cordinate of the the first and the last point of data taken horizontally and total number of point 
                           title_bpm_name, # a string that will appear in the name of the datafile 
                           statistic_number = 1, # nuber of repetition of taking data whith 1 set of parameter, as default no repetition
                           list_of_data = ["bpm_name","bpm_number","x_motor_step","x_motor_mm","y_motor_step","y_motor_mm","Va","Vb","Vc","Vd","Sum","x_libera_mm","y_libera_mm"], # list of the name of the data that the program will read, as default it read all
                           bpm_name1 = "BPM_E",bpm_name2 = "BPM_impr",bpm_name3 = "BPM_C",bpm_name4 = None): #name of the BPM used (as default, it the configuration of the BPM when this code was writen 

    data_name_and_path = path(title_bpm_name) #create the name of the datafile
    write_first_line(data_name_and_path, list_of_data) # write first line of the datafile with the name of the data
    bpm_list = initialise_bpm(bpm_name1,bpm_name2,bpm_name3,bpm_name4) # initialise BPM (program on read_adc.py)
    y_position = 0 # set an abirtary position for y since I can't get motor step
    motor_enable(1)
    for x_position in np.linspace(begin,end,number_of_point):
        move_motor_absolute(1,5000,x_position)
        print('x_motor going to ', x_position)
        while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'):
            time.sleep(1)
        time.sleep(1)
        acquisition_libera(x_position, y_position, data_name_and_path, statistic_number, list_of_data, bpm_list) # read_the data (program on read_adc.py)
    motor_disable(1)



def vertical_acquisition(begin, end, number_of_point, # cordinate of the the first and the last point of data taken vertically and total number of point 
                           title_bpm_name, # a string that will appear in the name of the datafile 
                           statistic_number = 1, # nuber of repetition of taking data whith 1 set of parameter, as default no repetition
                           list_of_data = ["bpm_name","bpm_number","x_motor_step","x_motor_mm","y_motor_step","y_motor_mm","Va","Vb","Vc","Vd","Sum","x_libera_mm","y_libera_mm"], # list of the name of the data that the program will read, as default it read all
                           bpm_name1 = "BPM_E",bpm_name2 = "BPM_impr",bpm_name3 = "BPM_C",bpm_name4 = None): #name of the BPM used (as default, it the configuration of the BPM when this code was writen
    
    data_name_and_path = path(title_bpm_name) #create the name of the datafile
    write_first_line(data_name_and_path, list_of_data)  # write first line of the datafile with the name of the data
    bpm_list = initialise_bpm(bpm_name1,bpm_name2,bpm_name3,bpm_name4) # initialise BPM (program on read_adc.py)
    x_position = 0 # set an abirtary position for x since I can't get motor step
    motor_enable(2)
    for y_position in np.linspace(begin, end, number_of_point):            
        motor_move_absolute(2,2000,y_position)
        print('y_moteur going to', y_position)
        while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'):
            time.sleep(1)
        time.sleep(1)
        acquisition_libera(x_position, y_position, data_name_and_path, statistic_number, list_of_data, bpm_list) # read_the data (program on read_adc.py)
    motor_disable(2)


def acquisition_2D(begin1,end1,number_of_point1, # cordinate of the the first and the last point of data taken horizontally and total number of point 
                   begin2,end2,number_of_point2, # cordinate of the the first and the last point of data taken vertically and total number of point 
                   title_bpm_name, # a string that will appear in the name of the datafile
                   statistic_number = 1,# nuber of repetition of taking data whith 1 set of parameter, as default no repetition
                   list_of_data = ["bpm_name","bpm_number","x_motor_step","x_motor_mm","y_motor_step","y_motor_mm","Va","Vb","Vc","Vd","Sum","x_libera_mm","y_libera_mm"], # list of the name of the data that the program will read, as default it read all
                   bpm_name1 = "BPM_E",bpm_name2 = "BPM_impr",bpm_name3 = "BPM_C",bpm_name4 = None):#name of the BPM used (as default, it the configuration of the BPM when this code was writen
    
    data_name_and_path = path(title_bpm_name)#create the name of the datafile
    write_first_line(data_name_and_path, list_of_data)# write first line of the datafile with the name of the data
    bpm_list = initialise_bpm(bpm_name1,bpm_name2,bpm_name3,bpm_name4)# initialise BPM (program on read_adc.py)
    motor_enable(1)
    motor_enable(2)
    for y_position in np.linspace(begin2, end2, number_of_point2):            
         motor_move_absolute(2,2000,y_position)
        print('moteur going to', y_position)
        for x_position in np.linspace(begin1,end1,number_of_point1):
            move_motor_absolute(1,5000,x_position)
            print('x_motor going to ', x_position)
            while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'): #waits will the motor move
                time.sleep(1)
            time.sleep(1)
            acquisition_libera(x_position, y_position, data_name_and_path, statistic_number, list_of_data, bpm_list) # read_the data (program on read_adc.py)
        begin1,end1 = end1,begin1 #to 
    motor_disable(1)
    motor_disable(2)
    
#write_first_line("test.txt", ["test","de","si","ca","marche","bien","ou","pas"])

#acquisition_2D(0,1,3,-3.1415,3.1415,2,"test_acquisition_2D")
#horizontal_acquisition(0,1,3,"hori")
#vertical_acquisition(-3.1415,3.1415,2,"test_verti")