table_control.py

#!/usr/bin/env python

import time
# For serial connection to motor controller
import aml
import arduino_control as ac

""" //////////// CONSTANTS ///////////////// """
# NOTE I found that rotating the pitch such that the platform is 90 degrees 
# to the table is set to "233 degrees", or almost exactly 20000 steps! Figure this out.
# NOTE I similarly found that 90 degree of table roll corresponds to "161 degrees" 
# or 15950 steps. Again, figure this out.

ROLL_GEAR_RATIO = 64200.0/360            # Ratio from Lisa's MATLAB files
ROLL_GEAR_RATIO *= (180.5/180)*161.0/90  # This is a correction factor from measurement

PITCH_GEAR_RATIO = 55620.0/360           # Ratio from Lisa's MATLAB files
PITCH_GEAR_RATIO *= 233.0/90             # This is a correction factor from measurement

DEGREES_PER_STEP = 1.8 # Same for both motors? According to the MATLAB code.

class Table(object):

    def __init__(self, port = '/dev/tty.USA19H1464P1.1'):
        self.port = port
        self.relay = None
        self.aml = aml.AML(port = self.port)
        if self.check_ready():
            # Presetting slew rate to prevent slippage.
            self.set_slew_rate(150)
            self.relay = ac.Arduino()
        else:
            print("Couldn't connect to the Table...")

    """
    Motor selector. This tells the AML to activate the given motor.
    Roll motor is "1", pitch motor is "2".
    """
    def select_motor(self, motor):
        response = None
        # First select the motor
        response = self.aml.write_and_read('B%s' %motor)
        time.sleep(.5) # Wait while the motor switches, necessary
        if response[0] == 'Y':
            # Send request for position counter
            return True
        else:
            if response == '' or response[0] == 'E':
                print("Bad response. Response to command %s is %s" %('B%s' %motor, response))
                return False
            else:
                print("Shouldn't get here...")
                print("Response: %s" %response)
                return False

    """
    Reads position from a given motor on the table. 
    Takes the serial connection to the AML and the motor (1 or 2), 
    where motor 1 is the roll and motor 2 is the pitch.
    """

    def get_motor_position(self, motor):
        motor_ready = self.select_motor(motor)
        if motor_ready:
            # Send request for position counter
            self.aml.write('V1')
            V1 = self.aml.read()
            #print("V1 for motor %s = %s" %(motor, V1))
            # Parse and return motor position
            return int(V1[2:-2].replace(' ', ''))
        else:
            print("AML didn't configure motor correctly")
            return None


    """
    Return roll motor angle. This is motor 1
    """
    def get_roll_motor_angle(self):
        V1 = self.get_motor_position(1)
        if V1 != None:
            return V1*DEGREES_PER_STEP
        else:
            print("Something went wrong in get_roll_motor_angle()")
            return None


    """
    Return pitch motor angle. This is motor 2
    """
    def get_pitch_motor_angle(self):
        V1 = self.get_motor_position(2)
        if V1 != None:
            return V1*DEGREES_PER_STEP
        else:
            print("Something went wrong in get_pitch_motor_angle()")
            return None


    """
    Return roll angle.
    """
    def get_roll_angle(self):
        V1 = self.get_motor_position(1)
        if V1 != None:
            return V1*DEGREES_PER_STEP/ROLL_GEAR_RATIO
        else:
            print("Something went wrong in get_roll_angle()")
            return None


    """
    Return pitch angle.
    """
    def get_pitch_angle(self):
        V1 = self.get_motor_position(2)
        if V1 != None:
            return V1*DEGREES_PER_STEP/PITCH_GEAR_RATIO
        else:
            print("Something went wrong in get_pitch_angle()")
            return None


    """
    Set the position counter of a motor. 
    """
    def set_position_counter(self, motor, set_position):
        motor_ready = self.select_motor(motor)
        if motor_ready:
            # Send request to change position counter
            self.aml.write_and_read('f%s' %set_position)
            #print("Position for motor %s is now preset to %s" %(motor, set_position))
        else:
            print("AML didn't configure motor correctly")
            return None
        

    """
    Move motor to a specfied position. 
    """
    def move_motor_to_position(self, motor, position):
        motor_ready = self.select_motor(motor)
        if motor_ready:
            # Send request to change position counter
            self.aml.write_and_read('G%s' %position)
            #print("Motor %s has moved to position %s" %(motor, position))
        else:
            print("AML didn't configure motor correctly")


    """
    Change roll angle to specified angle.
    Roll angles limited to -180 and 180
    """
    def move_to_roll_angle(self, angle):
        # Limit angle
        if angle >= 180:
            angle = 180
        elif angle <= -180:
            angle = -180
        # Find closest step to the desired angle
        motor_angle = angle*ROLL_GEAR_RATIO
        motor_position = int(motor_angle/DEGREES_PER_STEP)
        self.move_motor_to_position(1, motor_position)


    """
    Change pitch angle to specified angle.
    """
    def move_to_pitch_angle(self, angle):
        # Limit angle
        if angle >= 90:
            angle = 90
        elif angle <= -90:
            angle = -90
        # Find closest step to the desired angle
        motor_angle = angle*PITCH_GEAR_RATIO
        motor_position = int(motor_angle/DEGREES_PER_STEP)
        self.move_motor_to_position(2, motor_position)


    """
    Set slew rate in steps per second. Limited to 10-6000 steps per second.
    Note that the slew rate is reset on the AML on power off. This must be reset each time!
    This should be checked each time before a run! Easy to implement with 'V5' call.
    """
    def set_slew_rate(self, rate):
        # Limit the rate
        if rate > 6000:
            print("Max slew rate is 6000 steps per second")
            rate = 6000
        elif rate < 10:
            print("Min slew rate is 10 steps per second")
            rate = 10

        self.aml.write_and_read('T%s' %rate)
        print("Slew rate set to %s steps per second." %rate)
        

    """
    Check to see if AML ready. 
    KEYWORDS:
    hang=True forces waiting while 'Busy'.
    check_rate is the wait time in seconds between checks (not really a rate).
    """
    def check_ready(self, hang=False, check_rate=15):
        response = None
        # Send initial check
        response = self.aml.write_and_read('F')
        time.sleep(.5) 
        if response == '':
            return False
        if response[0] == 'Y':
            return True
        elif response[0] == 'E':
            print("There is an error, %s" %response)
            return False
        elif response[0] == 'B':
            #print("AML is busy")
            if hang:
                while response[0] != 'Y':
                    time.sleep(check_rate)
                    response = self.aml.write_and_read('F')
                    #print("Still busy...")
                #print("Ready!")
                return True
        else:
            print("Should never get here...")
            return False

    def shutdown(self):
        self.aml.close_serial()