#! /usr/bin/env python # ================================================================== # # LSST / StarDICE # # Micro-Controle motors (MM2500) # # Authors: Laurent Le Guillou # Date: 2019-02-18 # # ================================================================== import sys import os, os.path import time import serial # ================================================================== from exceptions import Exception class MotorError(Exception): pass # ============ Class Pollux controller ============================== class MM2500(object): """ The class represents one MM2500 controller axis. """ # ---------- Constructor --------------------------------- def __init__(self, port = '/dev/ttyS0', axis_id = 1, serial_port = None, # to provide if serial port already created debug = True): self.port = port self.serial_port = serial_port self.baudrate = 9600 self.timeout = 0.5 # Non-blocking & non waiting mode self.repr_mode = 0 self.parity = serial.PARITY_NONE self.bytesize = serial.EIGHTBITS self.stopbits = serial.STOPBITS_ONE self.serial_port = None self.EOL = '\r' # ---- debug mode self.debug = debug # # ---- action timeout # self.action_timeout = 10 self.moving_tick = 0.5 # delay before asking again the status while moving # ---- default axis self.axis_id = axis_id self.axis_prefix = "%02d" % self.axis_id # ---- limits (range) self.__limits = None # ---------- Open the controller device ------------------ def open(self): """ Open the Micro-Controle MM2500 controler device. Check if there's something connected (echotest). """ # if serial port has already been created # do not open it again if self.serial_port == None: if self.debug: print("Opening port %s ..." % self.port, file=sys.stderr) self.serial_port = serial.Serial(port = self.port, baudrate = self.baudrate, bytesize = self.bytesize, parity = self.parity, stopbits = self.stopbits, timeout = self.timeout) if ( (self.serial_port == None) or not(self.serial_port.isOpen()) ): raise IOError("Failed to open serial port %s" % self.port) # then the serial port is open (or was already open) self.serial_port.flushOutput() if not(self.echotest()): raise IOError(("Not echoing on serial port %s") % self.port) current_status = self.get_status() if ( current_status & (64+128) ): # left & right limits reached together -> no axis raise IOError(("Axis not present on serial port %s") % self.port) if self.debug: print(( "Opening port %s done." % self.port ), file=sys.stderr) # self.clear() # ---------- Close the controller device ----------------- def close(self): if ( self.serial_port and self.serial_port.isOpen() ): self.serial_port.close() # ----------------- write command ----------------------- def write(self, command): """ Send a command through the serial port. """ if not( self.serial_port and self.serial_port.isOpen() ): raise IOError("Port %s not yet opened" % self.port) if self.debug: print("Sending command [" + command + "]", file=sys.stderr) # A trailing space is required self.serial_port.write(command + " " + self.EOL) # ----------------- read command ----------------------- def read(self, timeout = None): """ Read the answer from the serial port. Return it as a string. If is specified, the function will wait for data with the specified timeout (instead of the default one). """ if not( self.serial_port and self.serial_port.isOpen() ): raise IOError("Port %s not yet opened" % self.port) if self.debug: print("Reading serial port buffer", file=sys.stderr) if timeout != None: self.serial_port.timeout = timeout if self.debug: print("Timeout specified: ", timeout, file=sys.stderr) answer = self.serial_port.readlines() # return buffer answer = "".join(answer) # joining lines # Restoring timeout to default one self.serial_port.timeout = self.timeout # if self.debug: print("Received [" + str(answer) + "]", file=sys.stderr) return answer # ----------------- Purge the serial port --------------- def purge(self): """ Purge the serial port to avoid framing errors. """ self.serial_port.flushOutput() self.serial_port.flushInput() self.serial_port.readlines() # ---------- Echo test ---------------------------------- def echotest(self): self.write(self.axis_prefix + "VE") answer = self.read() if len(answer) < 1: return False return True # ---------- Send a command and get the answer ----------- def send(self, cmd): """ Send a command (Micro-Controle MM2500 language) to the controler and return the answer (if any). @param cmd: the command to send. """ if len(cmd) < 2: raise ValueError("Invalid command") command = cmd # Now send it self.write(command) # Parsing the answer (to detect errors) answer = self.read() # if len(answer) < 1: # return answer # Detect errors and raise exception if any # TODO : properly parse the answer here ! # parts = answer.split() # if len(parts) > 1: # if parts[0][0] == '?': # An error occured # # if error ("?") -> raise Exception # raise MCAPIError(int(parts[0][1:])) # Remove the useless prompt '>' (not always present) # if answer[-1] == '>': # answer = answer[:-1] return answer # ---------- Get the motor serial number ----------------- def get_serial(self): """ Return the current axis position. """ answer = self.send(self.axis_prefix + "VE") if len(answer) < 1: raise IOError(("Not responding to " + "VE command on serial port %s") % self.port) serial = (answer).strip() return serial serial = property(get_serial, doc="Controler serial number") # ---------- Setup the motors ---------------------------- def setup(self): """ Initialize the controller. Setup the min/max speed and acceleration ramp parameters. """ # min speed when looking for origin [step/s] self.send(("%d" % self.axis_id) + "OL" + "250") # max speed when looking for origin [step/s] self.send(("%d" % self.axis_id) + "OH" + "2500") # temporisation origin [ms] self.send(("%d" % self.axis_id) + "OT" + "100") # min speed [step/s] self.send(("%d" % self.axis_id) + "RV" + "400") # max speed [step/s] self.send(("%d" % self.axis_id) + "XV" + "4000") # ramp duration [ms] self.send(("%d" % self.axis_id) + "RW" + "100") # normal speed (at max) (is it ok ?) [step/s] self.send(("%d" % self.axis_id) + "SV" + "4000") # Request to compute the ramp (??? useful ??? now or before each move ??) self.send(("%d" % self.axis_id) + "RX") # ---------- Current motor position ---------------------- def get_position(self): """ Return the current axis position. """ answer = self.send(self.axis_prefix + "TP") if len(answer) < 4: raise IOError(("Not responding to " + self.axis_prefix + "TP on serial port %s") % self.port) # answer : aaTPvalue position = int(answer[4:]) return position position = property(get_position, doc="Axis current position") # ---------- Current motor status ------------------------ def get_status(self): """ Return the current axis status. """ answer = self.send(self.axis_prefix + "TS") if len(answer) < 4: raise IOError(("Not responding to " + self.axis_prefix + "TS on serial port %s") % self.port) # answer : aaTSvalue status = int(answer[4:]) return status status = property(get_status, doc="Axis current status") # ---------- Move absolute and relative ------------------ def is_moving(self): """ True if the axis is moving, False otherwise. """ current_status = self.get_status() return bool( current_status & 1 ) def move_absolute(self, position, wait=True, check = False): """ Move the axis to absolute position 'position'. @param position: target position. @param wait: control is returned only when the movement is finished. @param check: to take into account the limits of the range, if False, move without constraints. """ # if check and self.__limits: # # Limits are already known so we can check # if ( (position < self.__limits['down']) or # (position > self.__limits['up']) ): # raise ValueError("Invalid position (out of range)") command = self.axis_prefix + "PA" + "%d" % position # if wait: # command += "," + self.axis_prefix + "WS" answer = self.send(command) if wait: while self.is_moving(): time.sleep(self.moving_tick) # return answer def move_relative(self, offset, wait = True, check = False): """ Move the axis of relative offset 'offset'. @param offset: position offset (positive or negative) @param check: to take into account the limits of the range. If False, move without constraints. """ # if check and self.__limits: # target = self.get_position() + offset # # Limits are already known so we can check # if ( (target < self.__limits['down']) or # (target > self.__limits['up']) ): # raise ValueError("Invalid position (out of range)") command = self.axis_prefix + "PR" + "%d" % offset # if wait: # command += "," + self.axis_prefix + "WS" answer = self.send(command) if wait: while self.is_moving(): time.sleep(self.moving_tick) # return answer # ========================================================= # ---------- Home : find and set zero --------------------- def home(self, wait=True): """ Looking for the origin index, and defining the zero. Should be done before any movement. """ command = self.axis_prefix + "OR" # if wait: # command += "," + self.axis_prefix + "WS" answer = self.send(command) if wait: while self.is_moving(): time.sleep(self.moving_tick) # return answer # ===================================================================