Source Code for Module Motor

  1  # Motor.java 
  2   
  3  ''' 
  4   This software is part of the EV3BrickPi library. 
  5   It is Open Source Free Software, so you may 
  6   - run the code for any purpose 
  7   - study how the code works and adapt it to your needs 
  8   - integrate all or parts of the code in your own programs 
  9   - redistribute copies of the code 
 10   - improve the code and release your improvements to the public 
 11   However the use of the code is entirely your responsibility. 
 12  ''' 
 13    
 14  from LegoRobot import LegoRobot 
 15  import SharedConstants 
 16  from Tools import * 
 17  import thread 
 18   
 19 -class MotorPort(): 
 20     A = 0 
 21     B = 1 
 22     C = 2 
 23     D = 3 
 24   
 25 -class MotorState(): 
 26      FORWARD = "FORWARD"       
 27      BACKWARD = "BACKWARD"       
 28      STOPPED = "STOPPED"       
 29      UNDEFINED = "UNDEFINED"       
 30       
 31 -class Motor(): 
 32      ''' 
 33      Class that represents a EV3LargeRegulatedMotors or a NXTRegulatedMotor. 
 34      Support for the wheel encoder that acts like a rotation counter with resolution of  
 35      720 counts per 360 degrees rotation. 
 36      ''' 
 37 -    def __init__(self, port): 
 38          ''' 
 39          Creates a motor instance at given motor port (MotorPort.A, MotorPort.B, MotorPort.C, MotorPort.D). 
 40          The rotation count is set to zero. 
 41          ''' 
 42          self.bp = LegoRobot._bp 
 43          self.port = port 
 44          if port == 0: 
 45              self.portLabel = "A" 
 46          elif port == 1: 
 47              self.portLabel = "B" 
 48          elif port == 2: 
 49              self.portLabel = "C" 
 50          elif port == 3: 
 51              self.portLabel = "D" 
 52          self.speed = SharedConstants.MOTOR_DEFAULT_SPEED 
 53          self.motorState = MotorState.STOPPED 
 54          self.resetMotorCount() 
 55   
 56 -    def getPortId(self): 
 57          ''' 
 58          Returns the port number [0..3]. 
 59          ''' 
 60          return self.port 
 61   
 62 -    def getPortLabel(self): 
 63          ''' 
 64          Returns the port label ("A", "B", "C", "D"). 
 65          ''' 
 66          return self.portLabel 
 67   
 68 -    def _getPort(self): 
 69          id = self.getPortId() 
 70          bp_port = 0x01 
 71          return (bp_port << id) 
 72   
 73 -    def forward(self): 
 74          ''' 
 75          Starts the forward rotation with current speed. 
 76          Method returns, while the rotation continues. 
 77          (If motor is already rotating, returns immediately.) 
 78          The rotation counter continues to be increased from its start value. 
 79          ''' 
 80          if self.motorState == MotorState.FORWARD: 
 81              return 
 82          self.motorState = MotorState.FORWARD 
 83          self.bp.set_motor_power(self._getPort(), self._speedToPower(self.speed)) 
 84   
 85 -    def backward(self): 
 86          ''' 
 87          Starts the backward rotation with current speed. 
 88          Method returns, while the rotation continues. 
 89          (If motor is already rotating, returns immediately.) 
 90          The rotation counter continues to be decreased from its start value. 
 91          ''' 
 92          if self.motorState == MotorState.BACKWARD: 
 93              return 
 94          self.motorState = MotorState.BACKWARD 
 95          self.bp.set_motor_power(self._getPort(), -self._speedToPower(self.speed)) 
 96   
 97 -    def stop(self): 
 98          '''  
 99          Stops the motor. 
100          ''' 
101          if self.motorState == MotorState.STOPPED: 
102              return 
103          self.motorState = MotorState.STOPPED 
104          self.bp.set_motor_power(self._getPort(), 0) 
105   
106 -    def _speedToPower(self, speed): 
107          return int(SharedConstants.MOTOR_POWER_TO_SPEED_FACTOR * speed) 
108   
109 -    def setSpeed(self, speed): 
110          ''' 
111          Sets the speed to the given value (arbitrary units). 
112          The speed will be changed to the new value at the next movement call only. 
113          @param speed: the speed 0..100 
114          ''' 
115          self.speed = min(abs(speed), 100) 
116          self.motorState = MotorState.UNDEFINED 
117   
118 -    def getSpeed(self): 
119          ''' 
120          Returns the current speed (arbitrary units). 
121          ''' 
122          return self.speed 
123   
124 -    def resetMotorCount(self): 
125          ''' 
126          Resets the rotation count to zero. 
127          ''' 
128          self.encoderCount = self.bp.get_motor_encoder(self._getPort()) 
129   
130 -    def getMotorCount(self): 
131          ''' 
132          Returns the current rotation count. 
133          ''' 
134          count = self.bp.get_motor_encoder(self._getPort()) 
135          return count - self.encoderCount # Forward rotation gives decreasing count 
136    
137 -    def rotateTo(self, count, blocking = True): 
138          ''' 
139          Sets the rotation counter to zero and rotates the motor until the given count is reached. 
140          If count is negative, the motor turns backwards. 
141          If blocking = True (default), the method blocks until the count is reached and the motor stops. 
142          Keep in mind that the motor will overrun the given count, because its inertia. The amount depends of its speed. 
143          ''' 
144          self._rotateTo(count, blocking, True) 
145   
146 -    def continueTo(self, count, blocking = True): 
147          ''' 
148          Same as rotateTo(count, blocking), but the rotation counter is not set to zero. 
149          The given count is the absolute value of the rotation counter to be reached. 
150          If the current count is higher than the count to reach, the motor turns backward. 
151          Keep in mind that the motor will overrun the given count, because its inertia. The amount depends of its speed. 
152          ''' 
153          self._rotateTo(count, blocking, False) 
154       
155 -    def continueRelativeTo(self, count, blocking = True): 
156          ''' 
157          Same as rotateTo(int count), but the rotation counter 
158          is not set to zero. 
159          The given count is the relative increasement/decreasement 
160          from the current value of the rotation counter. 
161          For count < 0 the motor turns backward. 
162          ''' 
163          curCount = self.getMotorCount() 
164          self.continueTo(curCount + count, blocking) 
165       
166 -    def _rotateTo(self, count, blocking, reset): 
167          if reset: 
168              self.resetMotorCount() 
169          if count > 0: 
170              self.forward() 
171              if blocking: 
172                  self._advance(count)     
173              else: 
174                  thread.start_new_thread(self._advance, (count,)) 
175          else: 
176              self.backward() 
177              if blocking: 
178                  self._reverse(count)     
179              else: 
180                  thread.start_new_thread(self._reverse, (count,))            
181    
182 -    def _advance(self, count): 
183          while self.getMotorCount() < count: 
184              Tools.delay(10) 
185          self.stop() 
186   
187 -    def _reverse(self, count): 
188          while self.getMotorCount() > count: 
189              Tools.delay(10) 
190          self.stop() 
191