Source Code for Module nxt.motor

  1  # nxt.motor module -- Class to control LEGO Mindstorms NXT motors 
  2  # Copyright (C) 2006  Douglas P Lau 
  3  # Copyright (C) 2009  Marcus Wanner 
  4  # 
  5  # This program is free software: you can redistribute it and/or modify 
  6  # it under the terms of the GNU General Public License as published by 
  7  # the Free Software Foundation, either version 3 of the License, or 
  8  # (at your option) any later version. 
  9  # 
 10  # This program is distributed in the hope that it will be useful, 
 11  # but WITHOUT ANY WARRANTY; without even the implied warranty of 
 12  # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
 13  # GNU General Public License for more details. 
 14   
 15  'Use for motor control' 
 16   
 17  import time 
 18   
 19  PORT_A = 0x00 
 20  PORT_B = 0x01 
 21  PORT_C = 0x02 
 22  PORT_ALL = 0xFF 
 23   
 24  MODE_IDLE = 0x00 
 25  MODE_MOTOR_ON = 0x01 
 26  MODE_BRAKE = 0x02 
 27  MODE_REGULATED = 0x04 
 28   
 29  REGULATION_IDLE = 0x00 
 30  REGULATION_MOTOR_SPEED = 0x01 
 31  REGULATION_MOTOR_SYNC = 0x02 
 32   
 33  RUN_STATE_IDLE = 0x00 
 34  RUN_STATE_RAMP_UP = 0x10 
 35  RUN_STATE_RUNNING = 0x20 
 36  RUN_STATE_RAMP_DOWN = 0x40 
 37   
 38  LIMIT_RUN_FOREVER = 0 
 39   
 40 -class Motor(object): 
 41   
 42 -    def __init__(self, brick, port): 
 43          self.brick = brick 
 44          self.port = port 
 45          self.power = 0 
 46          self.mode = MODE_IDLE 
 47          self.regulation = REGULATION_IDLE 
 48          self.turn_ratio = 0 
 49          self.run_state = RUN_STATE_IDLE 
 50          self.tacho_limit = LIMIT_RUN_FOREVER 
 51          self.tacho_count = 0 
 52          self.block_tacho_count = 0 
 53          self.rotation_count = 0 
 54          self.debug = 0 
 55   
 56 -    def _debug_out(self, message): 
 57          if self.debug: 
 58              print message 
 59   
 60 -    def set_output_state(self): 
 61          self._debug_out('Setting brick output state...') 
 62          self.brick.set_output_state(self.port, self.power, self.mode, self.regulation, self.turn_ratio, self.run_state, self.tacho_limit) 
 63          self._debug_out('State set.') 
 64   
 65 -    def get_output_state(self): 
 66          self._debug_out('Getting brick output state...') 
 67          values = self.brick.get_output_state(self.port) 
 68          (self.port, self.power, self.mode, self.regulation, 
 69              self.turn_ratio, self.run_state, self.tacho_limit, 
 70              self.tacho_count, self.block_tacho_count, 
 71              self.rotation_count) = values 
 72          self._debug_out('State got.') 
 73          return values 
 74   
 75 -    def reset_position(self, relative): 
 76          self.brick.reset_motor_position(self.port, relative) 
 77   
 78 -    def run(self, power=100, regulated=1): 
 79          '''Unlike update(), set_power() tells the motor to run continuously.''' 
 80          self.power = power 
 81          if regulated: 
 82              self.mode = MODE_MOTOR_ON | MODE_REGULATED 
 83              self.regulation = REGULATION_MOTOR_SPEED 
 84          else: 
 85              self.mode = MODE_MOTOR_ON 
 86              self.regulation = REGULATION_IDLE 
 87          self.turn_ratio = 0 
 88          self.run_state = RUN_STATE_RUNNING 
 89          self.tacho_limit = LIMIT_RUN_FOREVER 
 90          self.set_output_state() 
 91   
 92 -    def stop(self, braking=1): 
 93          '''Tells the motor to stop.''' 
 94          self.power = 0 
 95          if braking: 
 96              self.mode = MODE_MOTOR_ON | MODE_BRAKE | MODE_REGULATED 
 97              self.regulation = REGULATION_MOTOR_SPEED 
 98              self.run_state = RUN_STATE_RUNNING 
 99          else: 
100              self.mode = MODE_IDLE 
101              self.regulation = REGULATION_IDLE 
102              self.run_state = RUN_STATE_IDLE 
103          self.turn_ratio = 0 
104          self.tacho_limit = LIMIT_RUN_FOREVER 
105          self.set_output_state() 
106   
107 -    def update(self, power, tacho_limit, braking=False, max_retries=-1): 
108          '''Use this to run a motor. power is a value between -127 and 128, tacho_limit is 
109  the number of degrees to apply power for. Braking is wether or not to stop the 
110  motor after turning the specified degrees (unreliable). max_retries is the 
111  maximum times an internal loop of the braking function runs, so it doesn't get 
112  caught in an infinite loop (deprecated, do not use).''' 
113          if max_retries != -1: 
114              print 'Warning: max_retries is deprecated and is not longer needed, please do not use it!' 
115   
116          if braking: 
117              direction = (power > 0)*2-1 
118              if abs(power) < 50: 
119                  power = 50 * direction 
120              self.get_output_state() 
121              starting_tacho_count = self.tacho_count 
122              self._debug_out('tachocount: '+str(starting_tacho_count)) 
123              tacho_target = starting_tacho_count + tacho_limit*direction 
124              self._debug_out('tacho target: '+str(tacho_target)) 
125   
126          # Update modifiers even if they aren't used, might have been changed 
127          self.mode = MODE_MOTOR_ON 
128          self.regulation = REGULATION_IDLE 
129          self.turn_ratio = 0 
130          self.run_state = RUN_STATE_RUNNING 
131          self.power = power 
132          self.tacho_limit = tacho_limit 
133          self._debug_out('Updating motor information...') 
134           
135          if braking: 
136              self.run(power) 
137              current_tacho = 0 
138              retries = 0 
139              delta = abs(power)*0.5 #the amount of error allowed in stopping (to correct for latency) 
140               
141              while 1: 
142                  self._debug_out('checking tachocount...') 
143                  self.get_output_state() 
144                  current_tacho = self.tacho_count 
145                  self._debug_out('tachocount: '+str(current_tacho)) 
146                   
147                  if ((power >= 0 and (current_tacho >= tacho_target - delta)) or 
148                      (power <  0 and (current_tacho <= tacho_target + delta))): 
149                      self._debug_out('tachocount good, breaking from loop...') 
150                      break 
151                  else: 
152                      self._debug_out('tachocount bad, trying again...') 
153    
154              self.stop(1) 
155              self._debug_out('difference from goal: '+str(self.get_output_state()[7]-tacho_target)) 
156   
157          else: 
158              self.set_output_state() 
159              self._debug_out('Updating finished. ending tachocount: '+str(self.get_output_state()[7])) 
160