source: ETALON/BPM/initialise_motors.py @ 783

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

add line to wait until motor stop move befor doing anything else
File size: 14.0 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
7
8from bpm_constants import *
9from scope_function import *
10import numpy as np
11import matplotlib.pyplot as plt
12
13
14VERBOSE_DEFAULT=0
15
16global write_to_motor_dated_log
17write_to_motor_dated_log = data_dir + './write_to_motor_log'
18
19from writeclient import *
20import time
21
22# basic functions
23def write_to_motor(msg, verbose=VERBOSE_DEFAULT):
24    "This function sends a message to the motor controller"
25    ret = writeclient(motor_controller_IP, motor_controller_port, msg, verbose,datedlog=write_to_motor_dated_log)
26    time.sleep(0.5)
27    return ret
28
29
30def motor_send(message, verbose=VERBOSE_DEFAULT):
31    "Writes a formatted message to a motor"
32    if (len(message) <= 12):
33        print 'Message length ', len(message), ' >', message, '<'
34    while len(message) < 12:
35        message += '0'
36    if len(message) <= 12:
37        return write_to_motor(message, verbose)
38    else:
39        if verbose == 1:
40            print 'Command too long'
41        return 'Command too long'
42
43
44def motor_enable(motor, verbose=0):
45    "Enables a motor"
46    msg = str(motor) + 'CE'
47    motor_send(msg, verbose)
48
49
50def motor_disable(motor, verbose=0):
51    "Enables a motor"
52    if motor == 2: # security to avoid issue with vertical motor
53        print('are you sure to wanted to disable vertical motor ? \n yes or no')
54        x = raw_input()
55        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'):
56            print('Do you want to disable vertical motor ?')
57            x = raw_input()
58        if (x == 'no' or x == 'n' or  x == 'NO' or x == 'N'):
59            print('Be carful with disable vertical motor')
60        else :
61            move_motor_absolute(2,10000,-10000)
62            while motor_get(2, 'status') != 'READY':
63                time.sleep(1)
64            print('Say ok when motor can be put in position 0')
65            x = raw_input()
66            while not (x == 'OK' or x == 'ok' or x == 'Ok'):
67                print('Say ok when motor can be put in position 0')
68                x = raw_input()
69            move_motor_absolute(2,5000,0)
70            while motor_get(2, 'status') != 'READY':
71                time.sleep(1)
72            msg = str(motor) + 'CD'
73            motor_send(msg, verbose)
74    else :
75        msg = str(motor) + 'CD'
76        motor_send(msg, verbose)
77
78
79def motor_reset(motor, verbose=0):
80    "Enables a motor"
81    msg = str(motor) + 'CR'
82    motor_send(msg, verbose)
83
84
85def motor_start(motor, verbose=0):
86    "Starts the movement"
87    msg = str(motor) + 'GG'
88    motor_send(msg, verbose)
89
90
91def motor_stop(motor, verbose=0):
92    "Stops the movement"
93    msg = str(motor) + 'GS'
94    motor_send(msg, verbose)
95
96### Advaned functions
97def motor_set(motor, param, value):
98    "save parameter defined in the variable 'param', with value 'value'"
99    motor=str(motor)
100    if (param == 'velocity'):
101        if (value > 65535):
102            return 'value is too big'
103        else:
104            return motor_send(motor + ('SV%09d' % (abs(float(value)))))
105    elif (param == 'acceleration'):
106        if (value > 65535):
107            return 'value is too big'
108        else:
109            return motor_send(motor + 'SA%09d' % (abs(value)))
110    elif (param == 'deceleration'):
111        if (value > 65535):
112            return 'value is too big'
113        else:
114            return motor_send(motor + 'SD%09d'(abs(value)))
115    elif (param == 'target_position'):
116        if value > 0:
117            direction = 'P'
118        else:
119            direction = 'p'
120        return motor_send(motor + 'S' + direction + '%09d' % abs(value))
121    elif (param == 'actual_position'):
122        motor_enable(motor)
123        motor_reset(motor)
124        if value > 0:
125            direction = 'C'
126        else:
127            direction = 'c'
128        ret=motor_send(motor + 'S' + direction + '%09d' % abs(value))
129        motor_disable(motor)
130        return ret
131    else:
132        return 'Unknown parameter'
133
134
135def motor_get(motor, param):
136    """get parameter defined in variable param, from the motor driver"""
137    motor=str(motor)
138    if (param == 'velocity'):
139        velocity = motor_send(motor + 'QV')
140        velo=float(velocity[3:12])
141        print velo
142        return velo
143    elif (param == 'acceleration'):
144        acceleration = motor_send(motor + 'QA')
145        accel=float(acceleration[3:12])
146        print accel
147        return accel
148    elif (param == 'deceleration'):
149        deceleration = motor_send([motor + 'QD'])
150        return deceleration[3:12]
151    elif (param == 'actual_speed'):
152        actual_speed = motor_send(motor + 'QC')
153        isign = 1
154        if actual_speed[2] == 'c':
155            isign = -1
156        return isign * actual_speed[3:12]
157    elif (param == 'actual_position'):
158        actual_position = motor_send(motor + 'QP')
159        if actual_position[2] == 'p':
160            isign = -1
161        else:
162            isign = 1
163        print actual_position[2:13]
164        print actual_position[3:12]
165        pos = isign * float(actual_position[3:12])
166        print 'actual position=', pos
167        return pos
168    elif (param == 'status'):
169        status = motor_send(motor + 'QS')
170        status = status[3]
171        if status == 'R':
172            ret='READY'
173        elif status == 'B':
174            ret='BUSY'
175        elif status == 'E':
176            ret='ERROR'
177        else:
178            ret="Unknown "+status
179        print ret
180        return ret
181    elif (param == 'reference'):
182        reference = motor_send(motor + 'QR')
183        if float(reference[3:12]) == 2:
184            return 1
185        else:
186            return 0
187    elif param == 'limit':
188        limit = motor_send(motor + 'L')
189        if (limit[1] == 'l'):
190            return 'No limit'
191        elif ((limit[1] == 'L') and (limit[2] == 'B')):
192            return 'Back limit'
193        elif ((limit[1] == 'L') and (limit[2] == 'F')):
194            return "Front limit"
195        else:
196            return 'limit unknown'
197    else:
198        return 'Request unknown'
199
200
201def motor_move_relative(motor, speed, steps):
202    "Moves a motor with the given parameters"
203    motor=str(motor)
204    motor_set(motor, 'velocity', speed)
205    motor_set(motor, 'target_position', steps)
206    motor_send(motor + 'GR')
207    motor_start(motor)
208
209def move_motor_relative(motor, speed, steps):
210    motor_move_relative(motor, speed, steps)
211    while motor_get(motor, 'status') != 'READY':
212        time.sleep(1)
213
214def motor_move_absolute(motor, speed, steps):
215    "Moves a motor with the given parameters"
216    motor=str(motor)
217    motor_set(motor, 'velocity', speed)
218    motor_set(motor, 'target_position', steps)
219    motor_send(motor + 'GA')
220    motor_start(motor)
221
222def move_motor_absolute(motor,speed,steps):
223    "Moves a motor with the given parameters"
224    motor_move_absolute(motor, speed, steps)
225    while motor_get(motor, 'status') != 'READY':
226        time.sleep(1)
227
228def motor_wait_for_ready(motor,verbose=VERBOSE_DEFAULT):
229    motor_status=motor_get(motor,'status')
230    iloop=1
231    while ((iloop<1000) and (motor_status!='READY')):
232        time.sleep(1)
233        iloop=iloop+1
234        motor_status=motor_get(motor,'status')
235        if (iloop%10==0):
236            motor_pos=motor_status=motor_get(motor,'actual_position')
237    if (motor_status=='READY'):
238        return 1
239    else:
240        return 0
241
242def controller_hello():
243    "Sends ça_va query to the motor and display reply"
244    #    write_to_motor("ça_va?",1)
245    ret=write_to_motor("ca_va?", 1)
246    print "controller returned: ",ret
247    return ret
248
249def position_test_horizontal():
250    print('write OK if the zero possition sting is at maxima rigth or NO if it isn\'t')
251    x = raw_input()
252
253    while not (x == 'OK' or x == 'NO'):
254        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')
255        x = raw_input()
256
257    if (x == 'NO'):
258        print('Please don\'t forbid to move BPM at maxima right when you look from motor side befor to shut down the motor')
259        exit()
260   
261    else:
262        motor_enable(1)
263        move_motor_absolute(1,5000,400000)
264        while motor_get(1, 'status') != 'READY':
265            time.sleep(1)
266        print('verification of position')
267        time.sleep(5)
268        move_motor_absolute(1,5000,0)
269        while motor_get(1, 'status') != 'READY':
270            time.sleep(1)
271    motor_disable(1)
272
273def path(bpm_name): # create the path of the file what will contain data
274    idata = 0
275    start_time_str=datetime.now().strftime("%Y%m%d")
276    data_name_and_path = data_dir+"data/"
277    if not os.path.exists(data_name_and_path) :
278        os.makedirs(data_name_and_path)
279    data_name_and_path = data_name_and_path+"position_vs_tension_"+bpm_name+"_"+start_time_str+"_"+str(idata)+".txt"
280    while os.path.exists(data_name_and_path) :
281        idata+=1
282        data_name_and_path = data_dir+"data/position_vs_tension_"+bpm_name+"_"+start_time_str+"_"+str(idata)+".txt"
283    return(data_name_and_path)
284   
285   
286def horizontal_acquisition(begin,end,pas,bpm_name):
287    if pas == 0: #position test
288        motor_enable(1)
289        move_motor_absolute(1,10000,begin)
290        while motor_get(1, 'status') != 'READY':
291                time.sleep(1)
292        print("position ok ?")
293        time.sleep(15)
294        move_motor_absolute(1,10000,end)
295        while motor_get(1, 'status') != 'READY':
296            time.sleep(1)
297        print("position ok ?")
298        motor_disable(1)
299    else :
300        if (end - begin)*pas < 0 :
301            pas = -pas
302        data_name_and_path = path(bpm_name)
303        motor_enable(1)
304        L = [[],[],[],[],[]]
305        for i in range(begin, end,pas):
306            move_motor_absolute(1,5000,i)
307            while motor_get(1, 'status') != 'READY':
308                time.sleep(1)
309            time.sleep(1)
310            l = read_scope_and_write(i, data_name_and_path)
311            for j in range(5):
312                L[j].append(l[j])
313        time.sleep(1)
314        move_motor_absolute(1,5000,end) # pour assuré le dernier pas, en aller ou retour
315        while motor_get(1, 'status') != 'READY':
316            time.sleep(1)
317        time.sleep(1)
318        l = read_scope_and_write(end, data_name_and_path)
319        for j in range(5):
320            L[j].append(l[j])
321        motor_disable(1)
322        for i in range(4):
323            plt.plot(L[0],L[i+1])
324        plt.show()
325
326
327
328
329def vertical_acquisition(begin,end,pas,bpm_name):
330    if pas == 0: #position test
331        motor_enable(2)
332        move_motor_absolute(2,10000,begin)
333        while motor_get(2, 'status') != 'READY':
334            time.sleep(1)
335        print("position ok ?")
336        time.sleep(15)
337        move_motor_absolute(2,10000,end)
338        while motor_get(2, 'status') != 'READY':
339                time.sleep(1)
340        print("position ok ?")
341        motor_disable(2)
342    else:
343        if (end - begin)*pas < 0 :
344            pas = -pas
345        data_name_and_path = path(bpm_name)
346        motor_enable(2)
347        L = [[],[],[],[],[]]
348        for i in range(begin,end,pas):
349            move_motor_absolute(2,5000,i)
350            while motor_get(2, 'status') != 'READY':
351                time.sleep(1)
352            time.sleep(1)
353            l = read_scope_and_write(i, data_name_and_path)
354            for j in range(5):
355                L[j].append(l[j])
356        time.sleep(1)
357        move_motor_absolute(2,5000,end) # pour assuré le dernier pas, en aller ou retour
358        while motor_get(2, 'status') != 'READY':
359            time.sleep(1)
360        time.sleep(1)
361        l = read_scope_and_write(end, data_name_and_path)
362        for j in range(5):
363            L[j].append(l[j])
364        time.sleep(1)
365        for i in range(4):
366            plt.plot(L[0],L[i+1])
367        plt.show()
368        motor_disable(2)
369
370def acqui_column(begin, end, pas, data_path, other_position): # use in aquisition to read a column
371    if (end - begin)*pas < 0 :
372        pas = -pas
373    for i in range(begin,end,pas):
374        move_motor_absolute(2,5000,i)
375        while motor_get(2, 'status') != 'READY':
376            time.sleep(1)
377        time.sleep(1)
378        l = read_scope_and_write_2D(other_position, i, data_path)
379    time.sleep(1)
380    move_motor_absolute(2,5000,end) # pour assuré le dernier pas, en aller ou retour
381    while motor_get(2, 'status') != 'READY':
382        time.sleep(1)
383    time.sleep(1)
384    read_scope_and_write_2D(other_position, end, data_path)   
385
386def acquisition(begin1,end1,pas1,begin2,end2,pas2,bpm_name):
387    if pas1 == 0 or pas2 == 0: #position test
388        motor_enable(1)
389        motor_enable(2)
390        move_motor_absolute(1,10000,begin1)
391        move_motor_absolute(2,10000,begin2)
392        while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'):
393            time.sleep(1)
394        print("position ok ?")
395        time.sleep(15)
396        move_motor_absolute(1,10000,end1)
397        move_motor_absolute(2,10000,end2)
398        while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'):
399            time.sleep(1)
400        print("position ok ?")
401        motor_disable(1)
402        motor_disable(2)
403    else:
404        if (end1 - begin1)*pas1 < 0 :
405            pas1 = -pas1
406        if (end2 - begin2)*pas2 < 0 :
407            pas2 = -pas2
408        data_name_and_path = path(bpm_name)
409        motor_enable(1)
410        motor_enable(2)
411        move_motor_absolute(1,10000,begin1)
412        move_motor_absolute(2,10000,begin2)
413        while (motor_get(1, 'status') != 'READY') or (motor_get(2, 'status') != 'READY'):
414            time.sleep(1)
415        print("bagin position ok ?")
416        time.sleep(15)
417#        L = [[],[],[],[],[],[]]
418        for i in range(begin1, end1, pas1):           
419            move_motor_absolute(1,5000,i)
420            while motor_get(1, 'status') != 'READY':
421                time.sleep(1)
422            acqui_column(begin2, end2, pas2, data_name_and_path, i)
423            begin2,end2 = end2,begin2
424        move_motor_absolute(1,5000,end1) # pour assuré le dernier pas, en aller ou retour
425        while motor_get(1, 'status') != 'READY': 
426            time.sleep(1)
427        time.sleep(1)
428        acqui_column(begin2, end2, pas2, data_name_and_path, end1)
429        print("end position ok ?")
430        time.sleep(15)
431        motor_disable(1)
432        motor_disable(2)
433#        for j in range(6):
434#L[j].append(l[j])
435#        for i in range(4):
436#            plt.plot(L[0],L[i+2])
437#        plt.show()
438
439
440
441   
442#controller_hello()
443#move_motor_relative(1,1000,-1000)
444#time.sleep(1)
445#move_motor_relative(1,1000,1000)
Note: See TracBrowser for help on using the repository browser.