source: ETALON/BPM/initialise_motors.py @ 762

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

Code written by Alexandre Moutardier
File size: 6.3 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 *
9
10
11VERBOSE_DEFAULT=0
12
13global write_to_motor_dated_log
14write_to_motor_dated_log='/Users/delerue/Downloads/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
26
27def motor_send(message, verbose=VERBOSE_DEFAULT):
28    "Writes a formatted message to a motor"
29    if (len(message) <= 12):
30        print 'Message length ', len(message), ' >', message, '<'
31    while len(message) < 12:
32        message += '0'
33    if len(message) <= 12:
34        return write_to_motor(message, verbose)
35    else:
36        if verbose == 1:
37            print 'Command too long'
38        return 'Command too long'
39
40
41def motor_enable(motor, verbose=0):
42    "Enables a motor"
43    msg = str(motor) + 'CE'
44    motor_send(msg, verbose)
45
46
47def motor_disable(motor, verbose=0):
48    "Enables a motor"
49    msg = str(motor) + 'CD'
50    motor_send(msg, verbose)
51
52
53def motor_reset(motor, verbose=0):
54    "Enables a motor"
55    msg = str(motor) + 'CR'
56    motor_send(msg, verbose)
57
58
59def motor_start(motor, verbose=0):
60    "Starts the movement"
61    msg = str(motor) + 'GG'
62    motor_send(msg, verbose)
63
64
65def motor_stop(motor, verbose=0):
66    "Stops the movement"
67    msg = str(motor) + 'GS'
68    motor_send(msg, verbose)
69
70### Advaned functions
71def motor_set(motor, param, value):
72    "save parameter defined in the variable 'param', with value 'value'"
73    motor=str(motor)
74    if (param == 'velocity'):
75        if (value > 65535):
76            return 'value is too big'
77        else:
78            return motor_send(motor + ('SV%09d' % (abs(float(value)))))
79    elif (param == 'acceleration'):
80        if (value > 65535):
81            return 'value is too big'
82        else:
83            return motor_send(motor + 'SA%09d' % (abs(value)))
84    elif (param == 'deceleration'):
85        if (value > 65535):
86            return 'value is too big'
87        else:
88            return motor_send(motor + 'SD%09d'(abs(value)))
89    elif (param == 'target_position'):
90        if value > 0:
91            direction = 'P'
92        else:
93            direction = 'p'
94        return motor_send(motor + 'S' + direction + '%09d' % abs(value))
95    elif (param == 'actual_position'):
96        motor_enable(motor)
97        motor_reset(motor)
98        if value > 0:
99            direction = 'C'
100        else:
101            direction = 'c'
102        ret=motor_send(motor + 'S' + direction + '%09d' % abs(value))
103        motor_disable(motor)
104        return ret
105    else:
106        return 'Unknown parameter'
107
108
109def motor_get(motor, param):
110    """get parameter defined in variable param, from the motor driver"""
111    motor=str(motor)
112    if (param == 'velocity'):
113        velocity = motor_send(motor + 'QV')
114        velo=float(velocity[3:12])
115        print velo
116        return velo
117    elif (param == 'acceleration'):
118        acceleration = motor_send(motor + 'QA')
119        accel=float(acceleration[3:12])
120        print accel
121        return accel
122    elif (param == 'deceleration'):
123        deceleration = motor_send([motor + 'QD'])
124        return deceleration[3:12]
125    elif (param == 'actual_speed'):
126        actual_speed = motor_send(motor + 'QC')
127        isign = 1
128        if actual_speed[2] == 'c':
129            isign = -1
130        return isign * actual_speed[3:12]
131    elif (param == 'actual_position'):
132        actual_position = motor_send(motor + 'QP')
133        if actual_position[2] == 'p':
134            isign = -1
135        else:
136            isign = 1
137        print actual_position[2:13]
138        print actual_position[3:12]
139        pos = isign * float(actual_position[3:12])
140        print 'actual position=', pos
141        return pos
142    elif (param == 'status'):
143        status = motor_send(motor + 'QS')
144        status = status[3]
145        if status == 'R':
146            ret='READY'
147        elif status == 'B':
148            ret='BUSY'
149        elif status == 'E':
150            ret='ERROR'
151        else:
152            ret="Unknown "+status
153        print ret
154        return ret
155    elif (param == 'reference'):
156        reference = motor_send(motor + 'QR')
157        if float(reference[3:12]) == 2:
158            return 1
159        else:
160            return 0
161    elif param == 'limit':
162        limit = motor_send(motor + 'L')
163        if (limit[1] == 'l'):
164            return 'No limit'
165        elif ((limit[1] == 'L') and (limit[2] == 'B')):
166            return 'Back limit'
167        elif ((limit[1] == 'L') and (limit[2] == 'F')):
168            return "Front limit"
169        else:
170            return 'limit unknown'
171    else:
172        return 'Request unknown'
173
174
175def motor_move_relative(motor, speed, steps):
176    "Moves a motor with the given parameters"
177    motor=str(motor)
178    motor_set(motor, 'velocity', speed)
179    motor_set(motor, 'target_position', steps)
180    motor_send(motor + 'GR')
181    motor_start(motor)
182
183def move_motor_relative(motor, speed, steps):
184    motor_move_relative(motor, speed, steps)
185
186def motor_move_absolute(motor, speed, steps):
187    "Moves a motor with the given parameters"
188    motor=str(motor)
189    motor_set(motor, 'velocity', speed)
190    motor_set(motor, 'target_position', steps)
191    motor_send(motor + 'GA')
192    motor_start(motor)
193
194def move_motor_absolute(motor,speed,steps):
195    "Moves a motor with the given parameters"
196    motor_move_absolute(motor, speed, steps)
197
198
199def motor_wait_for_ready(motor,verbose=VERBOSE_DEFAULT):
200    motor_status=motor_get(motor,'status')
201    iloop=1
202    while ((iloop<1000) and (motor_status!='READY')):
203        time.sleep(1)
204        iloop=iloop+1
205        motor_status=motor_get(motor,'status')
206        if (iloop%10==0):
207            motor_pos=motor_status=motor_get(motor,'actual_position')
208    if (motor_status=='READY'):
209        return 1
210    else:
211        return 0
212
213def controller_hello():
214    "Sends ça_va query to the motor and display reply"
215    #    write_to_motor("ça_va?",1)
216    ret=write_to_motor("ca_va?", 1)
217    print "controller returned: ",ret
218    return ret
219
220#controller_hello()
221#move_motor_relative(1,1000,-1000)
222#time.sleep(1)
223#move_motor_relative(1,1000,1000)
Note: See TracBrowser for help on using the repository browser.