source: ETALON/BPM/initialise_motors.py @ 790

Last change on this file since 790 was 790, checked in by moutardier, 6 years ago

code final version
File size: 14.6 KB
Line 
1# This Python file uses the following encoding: latin-1
2
3# Defines all the functions and variables needed to control the motors with the WAGO controller
4# July 2015, Nicolas Delerue
5__author__ = 'delerue'
6
7from bpm_constants import *
8from read_adc import *
9import numpy as np
10#import matplotlib.pyplot as plt
11from datetime import datetime
12
13VERBOSE_DEFAULT=0
14
15global write_to_motor_dated_log
16write_to_motor_dated_log = data_dir + './write_to_motor_log'
17
18from writeclient import *
19import time
20
21# basic functions
22def write_to_motor(msg, verbose=VERBOSE_DEFAULT):
23    "This function sends a message to the motor controller"
24    ret = writeclient(motor_controller_IP, motor_controller_port, msg, verbose,datedlog=write_to_motor_dated_log)
25    time.sleep(0.5)
26    return ret
27
28
29def motor_send(message, verbose=VERBOSE_DEFAULT):
30    "Writes a formatted message to a motor"
31    if (len(message) <= 12):
32        print 'Message length ', len(message), ' >', message, '<'
33    while len(message) < 12:
34        message += '0'
35    if len(message) <= 12:
36        return write_to_motor(message, verbose)
37    else:
38        if verbose == 1:
39            print 'Command too long'
40        return 'Command too long'
41
42
43def motor_enable(motor, verbose=0):
44    "Enables a motor"
45    msg = str(motor) + 'CE'
46    motor_send(msg, verbose)
47
48
49def motor_disable(motor, verbose=0):
50    "Enables a motor"
51    if motor == 2: # security to avoid issue with vertical motor
52        print('are you sure to wanted to disable vertical motor ? \n yes or no')
53        x = raw_input()
54        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'):
55            print('Do you want to disable vertical motor ?')
56            x = raw_input()
57        if (x == 'no' or x == 'n' or  x == 'NO' or x == 'N'):
58            print('Be carful with disable vertical motor')
59        else :
60            move_motor_absolute(2,2000,0)
61            while motor_get(2, 'status') != 'READY':
62                time.sleep(1)
63            msg = str(motor) + 'CD'
64            motor_send(msg, verbose)
65    else :
66        msg = str(motor) + 'CD'
67        motor_send(msg, verbose)
68
69
70def motor_reset(motor, verbose=0):
71    "Enables a motor"
72    msg = str(motor) + 'CR'
73    motor_send(msg, verbose)
74
75
76def motor_start(motor, verbose=0):
77    "Starts the movement"
78    msg = str(motor) + 'GG'
79    motor_send(msg, verbose)
80
81
82def motor_stop(motor, verbose=0):
83    "Stops the movement"
84    msg = str(motor) + 'GS'
85    motor_send(msg, verbose)
86
87### Advaned functions
88def motor_set(motor, param, value):
89    "save parameter defined in the variable 'param', with value 'value'"
90    motor=str(motor)
91    if (param == 'velocity'):
92        if (value > 65535):
93            return 'value is too big'
94        else:
95            return motor_send(motor + ('SV%09d' % (abs(float(value)))))
96    elif (param == 'acceleration'):
97        if (value > 65535):
98            return 'value is too big'
99        else:
100            return motor_send(motor + 'SA%09d' % (abs(value)))
101    elif (param == 'deceleration'):
102        if (value > 65535):
103            return 'value is too big'
104        else:
105            return motor_send(motor + 'SD%09d'(abs(value)))
106    elif (param == 'target_position'):
107        if value > 0:
108            direction = 'P'
109        else:
110            direction = 'p'
111        return motor_send(motor + 'S' + direction + '%09d' % abs(value))
112    elif (param == 'actual_position'):
113        motor_enable(motor)
114        motor_reset(motor)
115        if value > 0:
116            direction = 'C'
117        else:
118            direction = 'c'
119        ret=motor_send(motor + 'S' + direction + '%09d' % abs(value))
120        motor_disable(motor)
121        return ret
122    else:
123        return 'Unknown parameter'
124
125
126def motor_get(motor, param):
127    """get parameter defined in variable param, from the motor driver"""
128    motor=str(motor)
129    if (param == 'velocity'):
130        velocity = motor_send(motor + 'QV')
131        velo=float(velocity[3:12])
132        print velo
133        return velo
134    elif (param == 'acceleration'):
135        acceleration = motor_send(motor + 'QA')
136        accel=float(acceleration[3:12])
137        print accel
138        return accel
139    elif (param == 'deceleration'):
140        deceleration = motor_send([motor + 'QD'])
141        return deceleration[3:12]
142    elif (param == 'actual_speed'):
143        actual_speed = motor_send(motor + 'QC')
144        isign = 1
145        if actual_speed[2] == 'c':
146            isign = -1
147        return isign * actual_speed[3:12]
148    elif (param == 'actual_position'):
149        actual_position = motor_send(motor + 'QP')
150        if actual_position[2] == 'p':
151            isign = -1
152        else:
153            isign = 1
154        print actual_position[2:13]
155        print actual_position[3:12]
156        pos = isign * float(actual_position[3:12])
157        print 'actual position=', pos
158        return pos
159    elif (param == 'status'):
160        status = motor_send(motor + 'QS')
161        status = status[3]
162        if status == 'R':
163            ret='READY'
164        elif status == 'B':
165            ret='BUSY'
166        elif status == 'E':
167            ret='ERROR'
168        else:
169            ret="Unknown "+status
170        print ret
171        return ret
172    elif (param == 'reference'):
173        reference = motor_send(motor + 'QR')
174        if float(reference[3:12]) == 2:
175            return 1
176        else:
177            return 0
178    elif param == 'limit':
179        limit = motor_send(motor + 'L')
180        if (limit[1] == 'l'):
181            return 'No limit'
182        elif ((limit[1] == 'L') and (limit[2] == 'B')):
183            return 'Back limit'
184        elif ((limit[1] == 'L') and (limit[2] == 'F')):
185            return "Front limit"
186        else:
187            return 'limit unknown'
188    else:
189        return 'Request unknown'
190
191
192def motor_move_relative(motor, speed, steps):
193    "Moves a motor with the given parameters"
194    motor=str(motor)
195    motor_set(motor, 'velocity', speed)
196    motor_set(motor, 'target_position', steps)
197    motor_send(motor + 'GR')
198    motor_start(motor)
199
200def move_motor_relative(motor, speed, steps):
201    motor_move_relative(motor, speed, steps)
202    while motor_get(motor, 'status') != 'READY':
203        time.sleep(1)
204
205
206def motor_move_absolute(motor, speed, steps):
207    "Moves a motor with the given parameters"
208    motor=str(motor)
209    motor_set(motor, 'velocity', speed)
210    motor_set(motor, 'target_position', steps)
211    motor_send(motor + 'GA')
212    motor_start(motor)
213
214def move_motor_absolute(motor,speed,steps):
215    "Moves a motor with the given parameters"
216    motor_move_absolute(motor, speed, steps)
217    while motor_get(motor, 'status') != 'READY':
218        time.sleep(1)
219
220
221def motor_wait_for_ready(motor,verbose=VERBOSE_DEFAULT):
222    motor_status=motor_get(motor,'status')
223    iloop=1
224    while ((iloop<1000) and (motor_status!='READY')):
225        time.sleep(1)
226        iloop=iloop+1
227        motor_status=motor_get(motor,'status')
228        if (iloop%10==0):
229            motor_pos=motor_status=motor_get(motor,'actual_position')
230    if (motor_status=='READY'):
231        return 1
232    else:
233        return 0
234
235def controller_hello():
236    "Sends ça_va query to the motor and display reply"
237    #    write_to_motor("ça_va?",1)
238    ret=write_to_motor("ca_va?", 1)
239    print "controller returned: ",ret
240    return ret
241
242def position_test_horizontal():
243    print('write OK if the zero possition sting is at maxima rigth or NO if it isn\'t')
244    x = raw_input()
245
246    while not (x == 'OK' or x == 'NO'):
247        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')
248        x = raw_input()
249
250    if (x == 'NO'):
251        print('Please don\'t forbid to move BPM at maxima right when you look from motor side befor to shut down the motor')
252        exit()   
253    else:
254        motor_enable(1)
255        move_motor_absolute(1,5000,400000)
256        while motor_get(1, 'status') != 'READY':
257            time.sleep(1)
258        print('verification of position')
259        time.sleep(5)
260        move_motor_absolute(1,5000,0)
261        while motor_get(1, 'status') != 'READY':
262            time.sleep(1)
263    motor_disable(1)
264
265def path(bpm_name, list_of_data): # create the path of the file what will contain data
266    idata = 0
267    start_time_str=datetime.now().strftime("%Y%m%d")
268    data_name_and_path = data_dir+"data/"
269    string_of_data = ""
270    for data_name in list_of_data:
271        string_of_data += data_name + "_"
272    if not os.path.exists(data_name_and_path) :
273        os.makedirs(data_name_and_path)
274    data_name_and_path = data_name_and_path+string_of_data+bpm_name+"_"+start_time_str+"_"
275    while os.path.exists(data_name_and_path+str(idata)+".txt") :
276        idata+=1
277    return(data_name_and_path+str(idata)+".txt")
278
279def write_first_line(data_name_and_path, list_of_data):
280    length = len(list_of_data)
281    fichier = open(data_name_and_path, "w")
282    for index in range(length):
283        fichier.write(list_of_data[index])
284        if index != length - 1:
285            fichier.write(" ")
286    fichier.write("\n")
287    fichier.close()
288   
289def 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
290                           title_bpm_name, # a string that will appear in the name of the datafile
291                           statistic_number = 1, # nuber of repetition of taking data whith 1 set of parameter, as default no repetition
292                           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
293                           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
294
295    data_name_and_path = path(title_bpm_name, list_of_data) #create the name of the datafile
296    write_first_line(data_name_and_path, list_of_data) # write first line of the datafile with the name of the data
297    bpm_list = initialise_bpm(bpm_name1,bpm_name2,bpm_name3,bpm_name4) # initialise BPM (program on read_adc.py)
298    y_position = 0 # set an abirtary position for y since I can't get motor step
299    motor_enable(1)
300    for x_position in np.linspace(begin,end,number_of_point):
301        move_motor_absolute(1,5000,x_position)
302        print('x_motor going to ', x_position)
303        while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'):
304            time.sleep(1)
305        time.sleep(1)
306        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)
307    motor_disable(1)
308
309
310
311def 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
312                           title_bpm_name, # a string that will appear in the name of the datafile
313                           statistic_number = 1, # nuber of repetition of taking data whith 1 set of parameter, as default no repetition
314                           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
315                           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
316   
317    data_name_and_path = path(title_bpm_name, list_of_data) #create the name of the datafile
318    write_first_line(data_name_and_path, list_of_data)  # write first line of the datafile with the name of the data
319    bpm_list = initialise_bpm(bpm_name1,bpm_name2,bpm_name3,bpm_name4) # initialise BPM (program on read_adc.py)
320    x_position = 0 # set an abirtary position for x since I can't get motor step
321    motor_enable(2)
322    for y_position in np.linspace(begin, end, number_of_point):           
323        motor_move_absolute(2,2000,y_position)
324        print('y_moteur going to', y_position)
325        while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'):
326            time.sleep(1)
327        time.sleep(1)
328        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)
329    motor_disable(2)
330
331
332def 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
333                   begin2,end2,number_of_point2, # cordinate of the the first and the last point of data taken vertically and total number of point
334                   title_bpm_name, # a string that will appear in the name of the datafile
335                   statistic_number = 1,# nuber of repetition of taking data whith 1 set of parameter, as default no repetition
336                   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
337                   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
338   
339    data_name_and_path = path(title_bpm_name, list_of_data)#create the name of the datafile
340    write_first_line(data_name_and_path, list_of_data)# write first line of the datafile with the name of the data
341    bpm_list = initialise_bpm(bpm_name1,bpm_name2,bpm_name3,bpm_name4)# initialise BPM (program on read_adc.py)
342    motor_enable(1)
343    motor_enable(2)
344    for y_position in np.linspace(begin2, end2, number_of_point2):           
345        motor_move_absolute(2,2000,y_position)
346        print('moteur going to', y_position)
347        for x_position in np.linspace(begin1,end1,number_of_point1):
348            move_motor_absolute(1,5000,x_position)
349            print('x_motor going to ', x_position)
350            while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'): #waits will the motor move
351                time.sleep(1)
352            time.sleep(1)
353            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)
354        begin1,end1 = end1,begin1 #to go and back
355    motor_disable(1)
356    motor_disable(2)
357   
358#write_first_line("test.txt", ["test","de","si","ca","marche","bien","ou","pas"])
359
360#acquisition_2D(0,1,3,-3.1415,3.1415,2,"test_acquisition_2D")
361#horizontal_acquisition(0,1,3,"hori")
362#vertical_acquisition(-3.1415,3.1415,2,"test_verti")
Note: See TracBrowser for help on using the repository browser.