source: ETALON/BPM/initialise_motors.py @ 774

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

add print_datas.py to exploit data creat with initialise_motore.py fonction and create graph
File size: 10.8 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    msg = str(motor) + 'CD'
53    motor_send(msg, verbose)
54
55
56def motor_reset(motor, verbose=0):
57    "Enables a motor"
58    msg = str(motor) + 'CR'
59    motor_send(msg, verbose)
60
61
62def motor_start(motor, verbose=0):
63    "Starts the movement"
64    msg = str(motor) + 'GG'
65    motor_send(msg, verbose)
66
67
68def motor_stop(motor, verbose=0):
69    "Stops the movement"
70    msg = str(motor) + 'GS'
71    motor_send(msg, verbose)
72
73### Advaned functions
74def motor_set(motor, param, value):
75    "save parameter defined in the variable 'param', with value 'value'"
76    motor=str(motor)
77    if (param == 'velocity'):
78        if (value > 65535):
79            return 'value is too big'
80        else:
81            return motor_send(motor + ('SV%09d' % (abs(float(value)))))
82    elif (param == 'acceleration'):
83        if (value > 65535):
84            return 'value is too big'
85        else:
86            return motor_send(motor + 'SA%09d' % (abs(value)))
87    elif (param == 'deceleration'):
88        if (value > 65535):
89            return 'value is too big'
90        else:
91            return motor_send(motor + 'SD%09d'(abs(value)))
92    elif (param == 'target_position'):
93        if value > 0:
94            direction = 'P'
95        else:
96            direction = 'p'
97        return motor_send(motor + 'S' + direction + '%09d' % abs(value))
98    elif (param == 'actual_position'):
99        motor_enable(motor)
100        motor_reset(motor)
101        if value > 0:
102            direction = 'C'
103        else:
104            direction = 'c'
105        ret=motor_send(motor + 'S' + direction + '%09d' % abs(value))
106        motor_disable(motor)
107        return ret
108    else:
109        return 'Unknown parameter'
110
111
112def motor_get(motor, param):
113    """get parameter defined in variable param, from the motor driver"""
114    motor=str(motor)
115    if (param == 'velocity'):
116        velocity = motor_send(motor + 'QV')
117        velo=float(velocity[3:12])
118        print velo
119        return velo
120    elif (param == 'acceleration'):
121        acceleration = motor_send(motor + 'QA')
122        accel=float(acceleration[3:12])
123        print accel
124        return accel
125    elif (param == 'deceleration'):
126        deceleration = motor_send([motor + 'QD'])
127        return deceleration[3:12]
128    elif (param == 'actual_speed'):
129        actual_speed = motor_send(motor + 'QC')
130        isign = 1
131        if actual_speed[2] == 'c':
132            isign = -1
133        return isign * actual_speed[3:12]
134    elif (param == 'actual_position'):
135        actual_position = motor_send(motor + 'QP')
136        if actual_position[2] == 'p':
137            isign = -1
138        else:
139            isign = 1
140        print actual_position[2:13]
141        print actual_position[3:12]
142        pos = isign * float(actual_position[3:12])
143        print 'actual position=', pos
144        return pos
145    elif (param == 'status'):
146        status = motor_send(motor + 'QS')
147        status = status[3]
148        if status == 'R':
149            ret='READY'
150        elif status == 'B':
151            ret='BUSY'
152        elif status == 'E':
153            ret='ERROR'
154        else:
155            ret="Unknown "+status
156        print ret
157        return ret
158    elif (param == 'reference'):
159        reference = motor_send(motor + 'QR')
160        if float(reference[3:12]) == 2:
161            return 1
162        else:
163            return 0
164    elif param == 'limit':
165        limit = motor_send(motor + 'L')
166        if (limit[1] == 'l'):
167            return 'No limit'
168        elif ((limit[1] == 'L') and (limit[2] == 'B')):
169            return 'Back limit'
170        elif ((limit[1] == 'L') and (limit[2] == 'F')):
171            return "Front limit"
172        else:
173            return 'limit unknown'
174    else:
175        return 'Request unknown'
176
177
178def motor_move_relative(motor, speed, steps):
179    "Moves a motor with the given parameters"
180    motor=str(motor)
181    motor_set(motor, 'velocity', speed)
182    motor_set(motor, 'target_position', steps)
183    motor_send(motor + 'GR')
184    motor_start(motor)
185
186def move_motor_relative(motor, speed, steps):
187    motor_move_relative(motor, speed, steps)
188
189def motor_move_absolute(motor, speed, steps):
190    "Moves a motor with the given parameters"
191    motor=str(motor)
192    motor_set(motor, 'velocity', speed)
193    motor_set(motor, 'target_position', steps)
194    motor_send(motor + 'GA')
195    motor_start(motor)
196
197def move_motor_absolute(motor,speed,steps):
198    "Moves a motor with the given parameters"
199    motor_move_absolute(motor, speed, steps)
200
201
202def motor_wait_for_ready(motor,verbose=VERBOSE_DEFAULT):
203    motor_status=motor_get(motor,'status')
204    iloop=1
205    while ((iloop<1000) and (motor_status!='READY')):
206        time.sleep(1)
207        iloop=iloop+1
208        motor_status=motor_get(motor,'status')
209        if (iloop%10==0):
210            motor_pos=motor_status=motor_get(motor,'actual_position')
211    if (motor_status=='READY'):
212        return 1
213    else:
214        return 0
215
216def controller_hello():
217    "Sends ça_va query to the motor and display reply"
218    #    write_to_motor("ça_va?",1)
219    ret=write_to_motor("ca_va?", 1)
220    print "controller returned: ",ret
221    return ret
222
223def position_test_horizontal():
224    print('write OK if the zero possition sting is at maxima rigth or NO if it isn\'t')
225    x = raw_input()
226
227    while not (x == 'OK' or x == 'NO'):
228        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')
229        x = raw_input()
230
231    if (x == 'NO'):
232        print('Please don\'t forbid to move BPM at maxima right when you look from motor side befor to shut down the motor')
233        exit()
234   
235    else:
236        motor_enable(1)
237        move_motor_absolute(1,5000,400000)
238        while motor_get(1, 'status') != 'READY':
239            time.sleep(1)
240        print('verification of position')
241        time.sleep(5)
242        move_motor_absolute(1,5000,0)
243        while motor_get(1, 'status') != 'READY':
244            time.sleep(1)
245    motor_disable(1)
246
247def horizontal_aquisition(begin,end,pas,bpm_name):
248    idata = 0
249    start_time_str=datetime.now().strftime("%Y%m%d_%H%M%S")
250    data_name_and_path = data_dir+"data/position_vs_tension_"+bpm_name+"_"+start_time_str+"_"+str(idata)+".txt"
251    while os.path.exists(data_name_and_path) :
252        idata+=1
253        data_name_and_path = data_dir+"position_vs_tension_"+bpm_name+start_time_str+"_"+str(idata)+".txt"
254    fichier  = open(data_name_and_path, "w")
255    motor_enable(1)
256    L = [[],[],[],[],[]]
257    for i in range(begin, end,pas):
258        move_motor_absolute(1,5000,i)
259        while motor_get(1, 'status') != 'READY':
260                time.sleep(1)
261        time.sleep(1)
262        l = read_scope_and_write(i, fichier)
263        for j in range(5):
264            L[j].append(l[j])
265    time.sleep(1)
266   
267    move_motor_absolute(1,5000,end) # pour assuré le dernier pas, en aller ou retour
268    while motor_get(1, 'status') != 'READY':
269        time.sleep(1)
270    time.sleep(1)
271    l = read_scope_and_write(end, fichier)
272    for j in range(5):
273        L[j].append(l[j])
274    time.sleep(1)
275    motor_disable(1)
276    for i in range(4):
277         plt.plot(L[0],L[i+1])
278    plt.show()
279    fichier.close()
280
281
282
283def vertical_aquisition(begin,end,pas,bpm_name):
284    idata = 0
285    start_time_str=datetime.now().strftime("%Y%m%d_%H%M%S")
286    data_name_and_path = data_dir+"data/position_vs_tension_"+bpm_name+"_"+start_time_str+"_"+str(idata)+".txt"
287    while os.path.exists(data_name_and_path) :
288        idata+=1
289        data_name_and_path = data_dir+"position_vs_tension_"+bpm_name+start_time_str+"_"+str(idata)+".txt"
290    fichier  = open(data_name_and_path, "w")
291    motor_enable(2)
292    L = [[],[],[],[],[]]
293    for i in range(begin, end,pas):
294        move_motor_absolute(1,5000,i)
295        while motor_get(2, 'status') != 'READY':
296                time.sleep(1)
297        time.sleep(1)
298        l = read_scope_and_write(i, fichier)
299        for j in range(5):
300            L[j].append(l[j])
301    time.sleep(1)
302   
303    move_motor_absolute(2,5000,end) # pour assuré le dernier pas, en aller ou retour
304    while motor_get(2, 'status') != 'READY':
305        time.sleep(1)
306    time.sleep(1)
307    l = read_scope_and_write(end, fichier)
308    for j in range(5):
309        L[j].append(l[j])
310    time.sleep(1)
311    for i in range(4):
312         plt.plot(L[0],L[i+1])
313    plt.show()
314    fichier.close()
315
316
317
318    def aquisition(begin1,end1,begin2,end2,pas1,pas2,bpm_name):
319    idata = 0
320    start_time_str=datetime.now().strftime("%Y%m%d_%H%M%S")
321    data_name_and_path = data_dir+"data/position_vs_tension_"+bpm_name+"_"+start_time_str+"_"+str(idata)+".txt"
322    while os.path.exists(data_name_and_path) :
323        idata+=1
324        data_name_and_path = data_dir+"position_vs_tension_"+bpm_name+start_time_str+"_"+str(idata)+".txt"
325    fichier  = open(data_name_and_path, "w")
326    motor_enable(1)
327    motor_enable(2)
328    L = [[],[],[],[],[]]
329    for i in range(begin1, end1,pas1):
330        move_motor_absolute(1,5000,i)
331        while motor_get(1, 'status') != 'READY':
332                time.sleep(1)
333        time.sleep(1)
334        for j in range(begin2, end2,pas2):
335            move_motor_absolute(2,5000,i)
336            while motor_get(2, 'status') != 'READY':
337                time.sleep(1)
338            time.sleep(1)
339            l = read_scope_and_write(i, fichier)
340            for k in range(5):
341                L[k].append(l[k])
342            time.sleep(1)
343    move_motor_absolute(1,5000,end1)   
344    move_motor_absolute(2,5000,end2) # pour assuré le dernier pas, en aller ou retour
345    while (motor_get(1, 'status') != 'READY') & (motor_get(2, 'status') != 'READY'): 
346        time.sleep(1)
347    time.sleep(1)
348    l = read_scope_and_write(end, fichier)
349    for j in range(5):
350        L[j].append(l[j])
351    time.sleep(1)
352    for i in range(4):
353         plt.plot(L[0],L[i+1])
354    plt.show()
355    fichier.close()
356
357
358   
359#controller_hello()
360#move_motor_relative(1,1000,-1000)
361#time.sleep(1)
362#move_motor_relative(1,1000,1000)
Note: See TracBrowser for help on using the repository browser.