python-nxt 2.2.2-1 / usr / share / pyshared / nxt / motor.py

# nxt.motor module -- Class to control LEGO Mindstorms NXT motors
# Copyright (C) 2006  Douglas P Lau
# Copyright (C) 2009  Marcus Wanner, rhn
# Copyright (C) 2010  rhn
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.

"""Use for motor control"""

import time

PORT_A = 0x00
PORT_B = 0x01
PORT_C = 0x02
PORT_ALL = 0xFF

MODE_IDLE = 0x00
MODE_MOTOR_ON = 0x01
MODE_BRAKE = 0x02
MODE_REGULATED = 0x04

REGULATION_IDLE = 0x00
REGULATION_MOTOR_SPEED = 0x01
REGULATION_MOTOR_SYNC = 0x02

RUN_STATE_IDLE = 0x00
RUN_STATE_RAMP_UP = 0x10
RUN_STATE_RUNNING = 0x20
RUN_STATE_RAMP_DOWN = 0x40

LIMIT_RUN_FOREVER = 0

class BlockedException(Exception):
    pass

class OutputState(object):
    """An object holding the internal state of a motor, not including rotation
    counters.
    """
    def __init__(self, values):
        (self.power, self.mode, self.regulation,
            self.turn_ratio, self.run_state, self.tacho_limit) = values
    
    def to_list(self):
        """Returns a list of properties that can be used with set_output_state.
        """
        return [self.power, self.mode, self.regulation,
            self.turn_ratio, self.run_state, self.tacho_limit]
        
    def __str__(self):
        modes = []
        if self.mode & MODE_MOTOR_ON:
            modes.append('on')
        if self.mode & MODE_BRAKE:
            modes.append('brake')
        if self.mode & MODE_REGULATED:
            modes.append('regulated')
        if not modes:
            modes.append('idle')
        mode = '&'.join(modes)
        regulation = 'regulation: ' + \
                            ['idle', 'speed', 'sync'][self.regulation]
        run_state = 'run state: ' + {0: 'idle', 0x10: 'ramp_up',
                            0x20: 'running', 0x40: 'ramp_down'}[self.run_state]
        return ', '.join([mode, regulation, str(self.turn_ratio), run_state] + [str(self.tacho_limit)])


class TachoInfo:
    """An object containing the information about the rotation of a motor"""
    def __init__(self, values):
        self.tacho_count, self.block_tacho_count, self.rotation_count = values
    
    def get_target(self, tacho_limit, direction):
        """Returns a TachoInfo object which corresponds to tacho state after
        moving for tacho_limit ticks. Direction can be 1 (add) or -1 (subtract)
        """
        # TODO: adjust other fields
        if abs(direction) != 1:
            raise ValueError('Invalid direction')
        new_tacho = self.tacho_count + direction * tacho_limit
        return TachoInfo([new_tacho, None, None])
    
    def is_greater(self, target, direction):
        return direction * (self.tacho_count - target.tacho_count) > 0
    
    def is_near(self, target, threshold):
        difference = abs(target.tacho_count - self.tacho_count)
        return difference < threshold
    
    def __str__(self):
        return str((self.tacho_count, self.block_tacho_count,
                   self.rotation_count))


class SynchronizedTacho(object):
    def __init__(self, leader_tacho, follower_tacho):
        self.leader_tacho = leader_tacho
        self.follower_tacho = follower_tacho
        
    def get_target(self, tacho_limit, direction):
        """This method will leave follower's target as None"""
        leader_tacho = self.leader_tacho.get_target(tacho_limit, direction)
        return SynchronizedTacho(leader_tacho, None)
    
    def is_greater(self, other, direction):
        return self.leader_tacho.is_greater(other.leader_tacho, direction)

    def is_near(self, other, threshold):
        return self.leader_tacho.is_near(other.leader_tacho, threshold)

    def __str__(self):
        if self.follower_tacho is not None:
            t2 = str(self.follower_tacho.tacho_count)
        else:
            t2 = 'None'
        t1 = str(self.leader_tacho.tacho_count)
        return 'tacho: ' + t1 + ' ' + t2


def get_tacho_and_state(values):
    """A convenience function. values is the list of values from
    get_output_state. Returns both OutputState and TachoInfo.
    """
    return OutputState(values[1:7]), TachoInfo(values[7:])


class BaseMotor(object):
    """Base class for motors"""
    debug = 0
    def _debug_out(self, message):
        if self.debug:
            print message

    def turn(self, power, tacho_units, brake=True, timeout=1, emulate=True):
        """Use this to turn a motor. The motor will not stop until it turns the
        desired distance. Accuracy is much better over a USB connection than
        with bluetooth...
        power is a value between -127 and 128 (an absolute value greater than
                 64 is recommended)
        tacho_units is the number of degrees to turn the motor. values smaller
                 than 50 are not recommended and may have strange results.
        brake is whether or not to hold the motor after the function exits
                 (either by reaching the distance or throwing an exception).
        timeout is the number of seconds after which a BlockedException is
                 raised if the motor doesn't turn
        emulate is a boolean value. If set to False, the motor is aware of the
                 tacho limit. If True, a run() function equivalent is used.
                 Warning: motors remember their positions and not using emulate
                 may lead to strange behavior, especially with synced motors
        """
  
        tacho_limit = tacho_units
 
        if tacho_limit < 0:
            raise ValueError, "tacho_units must be greater than 0!"
        #TODO Calibrate the new values for ip socket latency.
        if self.method == 'bluetooth':
            threshold = 70
        elif self.method == 'usb':
            threshold = 5
        elif self.method == 'ipbluetooth':
            threshold = 80
        elif self.method == 'ipusb':
            threshold = 15
        else:
            threshold = 30 #compromise

        tacho = self.get_tacho()
        state = self._get_new_state()

        # Update modifiers even if they aren't used, might have been changed
        state.power = power
        if not emulate:
            state.tacho_limit = tacho_limit

        self._debug_out('Updating motor information...')
        self._set_state(state)
       
        direction = 1 if power > 0 else -1
        self._debug_out('tachocount: ' + str(tacho))
        current_time = time.time()
        tacho_target = tacho.get_target(tacho_limit, direction)
        
        blocked = False
        try:
            while True:
                time.sleep(self._eta(tacho, tacho_target, power) / 2)
                
                if not blocked: # if still blocked, don't reset the counter
                    last_tacho = tacho
                    last_time = current_time
                
                tacho = self.get_tacho()
                current_time = time.time()
                blocked = self._is_blocked(tacho, last_tacho, direction)
                if blocked:
                    self._debug_out(('not advancing', last_tacho, tacho))
                    # the motor can be up to 80+ degrees in either direction from target when using bluetooth
                    if current_time - last_time > timeout:
                        if tacho.is_near(tacho_target, threshold):
                            break
                        else:
                            raise BlockedException("Blocked!")
                else:
                    self._debug_out(('advancing', last_tacho, tacho))
                if tacho.is_near(tacho_target, threshold) or tacho.is_greater(tacho_target, direction):
                    break
        finally:
            if brake:
                self.brake()
            else:
                self.idle()


class Motor(BaseMotor):
    def __init__(self, brick, port):
        self.brick = brick
        self.port = port
        self._read_state()
        self.sync = 0
        self.turn_ratio = 0
        try:
            self.method = brick.sock.type
        except:
            print "Warning: Socket did not report a type!"
            print "Please report this problem to the developers!"
            print "For now, turn() accuracy will not be optimal."
            print "Continuing happily..."
            self.method = None

    def _set_state(self, state):
        self._debug_out('Setting brick output state...')
        list_state = [self.port] + state.to_list()
        self.brick.set_output_state(*list_state)
        self._debug_out(state)
        self._state = state
        self._debug_out('State set.')

    def _read_state(self):
        self._debug_out('Getting brick output state...')
        values = self.brick.get_output_state(self.port)
        self._debug_out('State got.')
        self._state, tacho = get_tacho_and_state(values)
        return self._state, tacho
    
    #def get_tacho_and_state here would allow tacho manipulation
    
    def _get_state(self):
        """Returns a copy of the current motor state for manipulation."""
        return OutputState(self._state.to_list())
    
    def _get_new_state(self):
        state = self._get_state()
        if self.sync:
            state.mode = MODE_MOTOR_ON | MODE_REGULATED
            state.regulation = REGULATION_MOTOR_SYNC
            state.turn_ratio = self.turn_ratio
        else:
            state.mode = MODE_MOTOR_ON | MODE_REGULATED
            state.regulation = REGULATION_MOTOR_SPEED
        state.run_state = RUN_STATE_RUNNING
        state.tacho_limit = LIMIT_RUN_FOREVER
        return state
        
    def get_tacho(self):
        return self._read_state()[1]
        
    def reset_position(self, relative):
        """Resets the counters. Relative can be True or False"""
        self.brick.reset_motor_position(self.port, relative)

    def run(self, power=100, regulated=False):
        '''Tells the motor to run continuously. If regulated is True, then the
        synchronization starts working.
        '''
        state = self._get_new_state()
        state.power = power
        if not regulated:
            state.mode = MODE_MOTOR_ON
        self._set_state(state)

    def brake(self):
        """Holds the motor in place"""
        state = self._get_new_state()
        state.power = 0
        state.mode = MODE_MOTOR_ON | MODE_BRAKE | MODE_REGULATED
        self._set_state(state)

    def idle(self):
        '''Tells the motor to stop whatever it's doing. It also desyncs it'''
        state = self._get_new_state()
        state.power = 0
        state.mode = MODE_IDLE
        state.regulation = REGULATION_IDLE
        state.run_state = RUN_STATE_IDLE
        self._set_state(state)

    def weak_turn(self, power, tacho_units):
        """Tries to turn a motor for the specified distance. This function
        returns immediately, and it's not guaranteed that the motor turns that
        distance. This is an interface to use tacho_limit without
        REGULATION_MODE_SPEED
        """
        tacho_limit = tacho_units
        tacho = self.get_tacho()
        state = self._get_new_state()

        # Update modifiers even if they aren't used, might have been changed
        state.mode = MODE_MOTOR_ON
        state.regulation = REGULATION_IDLE
        state.power = power
        state.tacho_limit = tacho_limit

        self._debug_out('Updating motor information...')
        self._set_state(state)
    
    def _eta(self, current, target, power):
        """Returns time in seconds. Do not trust it too much"""
        tacho = abs(current.tacho_count - target.tacho_count)
        return (float(tacho) / abs(power)) / 5
    
    def _is_blocked(self, tacho, last_tacho, direction):
        """Returns if any of the engines is blocked"""
        return direction * (last_tacho.tacho_count - tacho.tacho_count) >= 0


class SynchronizedMotors(BaseMotor):
    """The object used to make two motors run in sync. Many objects may be
    present at the same time but they can't be used at the same time.
    Warning! Movement methods reset tacho counter.
    THIS CODE IS EXPERIMENTAL!!!
    """
    def __init__(self, leader, follower, turn_ratio):
        """Turn ratio can be >= 0 only! If you want to have it reversed,
        change motor order.
        """
        if follower.brick != leader.brick:
            raise ValueError('motors belong to different bricks')
        self.leader = leader
        self.follower = follower
        self.method = self.leader.method #being from the same brick, they both have the same com method.
        
        if turn_ratio < 0:
            raise ValueError('Turn ratio <0. Change motor order instead!')

        if self.leader.port == self.follower.port:
            raise ValueError("The same motor passed twice")
        elif self.leader.port > self.follower.port:
            self.turn_ratio = turn_ratio
        else:
            self._debug_out('reversed')
            self.turn_ratio = -turn_ratio

    def _get_new_state(self):
        return self.leader._get_new_state()
        
    def _set_state(self, state):
        self.leader._set_state(state)
        self.follower._set_state(state)

    def get_tacho(self):
        leadertacho = self.leader.get_tacho()
        followertacho = self.follower.get_tacho()
        return SynchronizedTacho(leadertacho, followertacho)

    def reset_position(self, relative):
        """Resets the counters. Relative can be True or False"""
        self.leader.reset_position(relative)
        self.follower.reset_position(relative)

    def _enable(self): # This works as expected. I'm not sure why.
        #self._disable()
        self.reset_position(True)
        self.leader.sync = True
        self.follower.sync = True
        self.leader.turn_ratio = self.turn_ratio
        self.follower.turn_ratio = self.turn_ratio        

    def _disable(self): # This works as expected. (tacho is reset ok)
        self.leader.sync = False
        self.follower.sync = False
        #self.reset_position(True)
        self.leader.idle()
        self.follower.idle()
        #self.reset_position(True)
    
    def run(self, power=100):
        """Warning! After calling this method, make sure to call idle. The
        motors are reported to behave wildly otherwise.
        """
        self._enable()
        self.leader.run(power, True)
        self.follower.run(power, True)
    
    def brake(self):
        self._disable() # reset the counters
        self._enable()
        self.leader.brake() # brake both motors at the same time
        self.follower.brake()
        self._disable() # now brake as usual
        self.leader.brake()
        self.follower.brake()

    def idle(self):
        self._disable()

    def turn(self, power, tacho_units, brake=True, timeout=1):
        self._enable()
        # non-emulation is a nightmare, tacho is being counted differently
        try:
            if power < 0:
                self.leader, self.follower = self.follower, self.leader
            BaseMotor.turn(self, power, tacho_units, brake, timeout, emulate=True)
        finally:
            if power < 0:
                self.leader, self.follower = self.follower, self.leader
    
    def _eta(self, tacho, target, power):
        return self.leader._eta(tacho.leader_tacho, target.leader_tacho, power)
    
    def _is_blocked(self, tacho, last_tacho, direction):
        # no need to check both, they're synced
        return self.leader._is_blocked(tacho.leader_tacho, last_tacho.leader_tacho, direction)