source: ETALON/CLIO/control/initialise_motors.py @ 691

Last change on this file since 691 was 691, checked in by delerue, 7 years ago

CLIO control updated
File size: 8.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 clio_constants import *
9
10
11VERBOSE_DEFAULT=0
12
13global write_to_motor_dated_log
14write_to_motor_dated_log= data_dir + "write_to_motor_log"
15
16from writeclient import *
17import time
18
19# basic functions
20def write_to_motor(msg, verbose=VERBOSE_DEFAULT):
21    "This function sends a message to the motor controller"
22    ret = writeclient(motor_controller_IP, motor_controller_port, msg, verbose,datedlog=write_to_motor_dated_log)
23    time.sleep(0.5)
24    return ret
25
26def motor_send_float(message, sendType=0,verbose=VERBOSE_DEFAULT):
27    itry=0
28    result=''
29    value=-1
30    valueOK=0
31    while((itry<5)and((len(result)<2)or(valueOK==0))):
32        itry+=1
33        try:
34            if (sendType==0):
35                result=motor_send(message, verbose=verbose)
36            else:
37                result=write_to_motor(message)
38               
39            if ((result[2] == 'p')or(result[2] == 'c')):
40                isign = -1
41            else:
42                isign = 1
43                       
44            value=isign*float(result[3:12])
45            valueOK=1
46        except:
47            print 'Result with error '+result
48            valueOK=0
49            result=''
50    return value
51   
52
53def motor_send(message, verbose=VERBOSE_DEFAULT):
54    "Writes a formatted message to a motor"
55#    if (len(message) <= 12):
56#        print('Message length ', len(message), ' >', message, '<')
57    while len(message) < 12:
58        message += '0'
59#    print('Message length ', len(message), ' >', message, '<')
60    if len(message) <= 12:
61        return write_to_motor(message, verbose)
62    else:
63        if verbose == 1:
64            print('Command too long')
65        return 'Command too long'
66
67
68def motor_enable(motor, verbose=0):
69    "Enables a motor"
70    msg = str(motor) + 'CE'
71    motor_send(msg, verbose)
72
73
74def motor_disable(motor, verbose=0):
75    "Enables a motor"
76    msg = str(motor) + 'CD'
77    motor_send(msg, verbose)
78
79
80def motor_reset(motor, verbose=0):
81    "Enables a motor"
82    msg = str(motor) + 'CR'
83    motor_send(msg, verbose)
84
85
86def motor_start(motor, verbose=0):
87    "Starts the movement"
88    msg = str(motor) + 'GG'
89    motor_send(msg, verbose)
90
91
92def motor_stop(motor, verbose=0):
93    "Stops the movement"
94    msg = str(motor) + 'GS'
95    motor_send(msg, verbose)
96
97### Advaned functions
98def motor_set(motor, param, value):
99    "save parameter defined in the variable 'param', with value 'value'"
100    motor=str(motor)
101    if (param == 'velocity'):
102        if (value > 65535):
103            return 'value is too big'
104        else:
105            return motor_send(motor + ('SV%09d' % (abs(float(value)))))
106    elif (param == 'acceleration'):
107        if (value > 65535):
108            return 'value is too big'
109        else:
110            return motor_send(motor + 'SA%09d' % (abs(value)))
111    elif (param == 'deceleration'):
112        if (value > 65535):
113            return 'value is too big'
114        else:
115            return motor_send(motor + 'SD%09d'(abs(value)))
116    elif (param == 'target_position'):
117        if value > 0:
118            direction = 'P'
119        else:
120            direction = 'p'
121        return motor_send(motor + 'S' + direction + '%09d' % abs(value))
122    elif (param == 'actual_position'):
123        motor_enable(motor)
124        motor_reset(motor)
125        if value > 0:
126            direction = 'C'
127        else:
128            direction = 'c'
129        ret=motor_send(motor + 'S' + direction + '%09d' % abs(value))
130        motor_disable(motor)
131        return ret
132    else:
133        return 'Unknown parameter'
134
135
136def motor_get(motor, param):
137    """get parameter defined in variable param, from the motor driver"""
138    motor=str(motor)
139    if (param == 'velocity'):
140        velocity = motor_send(motor + 'QV')
141        print(velocity)
142        velo=float(velocity[3:12])
143        print(velo)
144        return velo
145    elif (param == 'acceleration'):
146        acceleration = motor_send(motor + 'QA')
147        accel=float(acceleration[3:12])
148        print accel
149        return accel
150    elif (param == 'deceleration'):
151        deceleration = motor_send([motor + 'QD'])
152        return deceleration[3:12]
153    elif (param == 'actual_speed'):
154        actual_speed_float = motor_send_float(motor + 'QC')
155        return actual_speed_float
156#        actual_speed = motor_send(motor + 'QC')
157#        isign = 1
158#        if actual_speed[2] == 'c':
159#            isign = -1
160#        return isign * actual_speed[3:12]
161    elif (param == 'actual_position'):
162        actual_position_float = motor_send_float(motor + 'QP')
163        print 'actual position=', actual_position_float
164        return actual_position_float
165#        itry=0
166#        actual_position = None
167#        actual_position = ''
168#        while ((itry<5)and((len(actual_position)<100)or(ord(actual_position[0])<20))):
169#            actual_position = motor_send(motor + 'QP')
170#            itry+=1
171           
172#        if actual_position[2] == 'p':
173#            isign = -1
174#        else:
175#            isign = 1
176#        pos = isign * float(actual_position[3:12])
177#        print 'actual position=', pos
178#        return pos
179    elif (param == 'current'):
180        motor_current_float = motor_send_float('cur0?',verbose=1,sendType=1)
181#        itry=0
182#        motor_current = ''
183#        while ((itry<5)and(len(motor_current)<10)):
184#            itry+=1
185#            motor_current = write_to_motor('cur0?',0)
186#            try:
187#                pos = float(motor_current[3:12])
188#            except:
189#                motor_current = ''
190        print 'current=', motor_current_float
191        return motor_current_float
192    elif (param == 'current'):
193        actual_position = motor_send('CUR0?')
194        print 'actual position=', actual_position
195        return pos
196    elif (param == 'status'):
197        status = motor_send(motor + 'QS')
198        status = status[3]
199        if status == 'R':
200            ret='READY'
201        elif status == 'B':
202            ret='BUSY'
203        elif status == 'E':
204            ret='ERROR'
205        else:
206            ret="Unknown "+status
207        print ret
208        return ret
209    elif (param == 'reference'):
210        reference = motor_send(motor + 'QR')
211        if float(reference[3:12]) == 2:
212            return 1
213        else:
214            return 0
215    elif param == 'limit':
216        limit = motor_send(motor + 'L')
217        if (limit[1] == 'l'):
218            return 'No limit'
219        elif ((limit[1] == 'L') and (limit[2] == 'B')):
220            return 'Back limit'
221        elif ((limit[1] == 'L') and (limit[2] == 'F')):
222            return "Front limit"
223        else:
224            return 'limit unknown'
225    else:
226        return 'Request unknown'
227
228
229def motor_move_relative(motor, speed, steps):
230    "Moves a motor with the given parameters"
231    motor=str(motor)
232    motor_set(motor, 'velocity', speed)
233    motor_set(motor, 'target_position', steps)
234    motor_send(motor + 'GR')
235    motor_start(motor)
236
237def move_motor_relative(motor, speed, steps):
238    motor_move_relative(motor, speed, steps)
239
240def motor_move_absolute(motor, speed, steps):
241    "Moves a motor with the given parameters"
242    motor=str(motor)
243    motor_set(motor, 'velocity', speed)
244    motor_set(motor, 'target_position', steps)
245    motor_send(motor + 'GA')
246    motor_start(motor)
247
248def move_motor_absolute(motor,speed,steps):
249    "Moves a motor with the given parameters"
250    motor_move_absolute(motor, speed, steps)
251
252
253def motor_wait_for_ready(motor,verbose=VERBOSE_DEFAULT):
254    motor_status=motor_get(motor,'status')
255    iloop=1
256    while ((iloop<1000) and (motor_status!='READY')):
257        time.sleep(1)
258        iloop=iloop+1
259        motor_status=motor_get(motor,'status')
260        if (iloop%10==0):
261            motor_pos=motor_status=motor_get(motor,'actual_position')
262    if (motor_status=='READY'):
263        return 1
264    else:
265        return 0
266
267def controller_hello():
268    "Sends ca_va query to the motor and display reply"
269    #    write_to_motor("ça_va?",1)
270    ret=write_to_motor("ca_va?", 0)
271    print "controller returned: ",ret
272    return ret
273
Note: See TracBrowser for help on using the repository browser.