Source Code for Module Gear

  1  # Gear.java 
  2   
  3  ''' 
  4  Class that represents the combination of two motors on an axis 
  5  to perform a car-like movement. 
  6   
  7   This software is part of the raspibrick module. 
  8   It is Open Source Free Software, so you may 
  9   - run the code for any purpose 
 10   - study how the code works and adapt it to your needs 
 11   - integrate all or parts of the code in your own programs 
 12   - redistribute copies of the code 
 13   - improve the code and release your improvements to the public 
 14   However the use of the code is entirely your responsibility. 
 15   ''' 
 16   
 17   
 18  from Tools import Tools 
 19  import SharedConstants 
 20  from RobotInstance import RobotInstance 
 21   
 22 -class GearState(): 
 23      FORWARD = 0 
 24      BACKWARD = 1 
 25      STOPPED = 2 
 26      LEFT = 3 
 27      RIGHT = 4 
 28      LEFTARC = 5 
 29      RIGHTARC = 6 
 30      UNDEFINED = 7 
 31   
 32  # ------------------------   Class Gear   -------------------------------------------------- 
 33 -class Gear(object): 
 34      ''' 
 35      Class that represents the combination of two motors on an axis 
 36      to perform a car-like movement. 
 37      ''' 
 38 -    def __init__(self): 
 39          ''' 
 40          Creates a gear instance. 
 41          ''' 
 42          self.speed = SharedConstants.GEAR_DEFAULT_SPEED 
 43          self.state = GearState.UNDEFINED 
 44          self.arcRadius = 0 
 45          Tools.debug("Gear instance created") 
 46   
 47 -    def forward(self, duration = 0): 
 48          ''' 
 49          Starts the forward rotation with preset speed. 
 50          If duration = 0, the method returns immediately, while the rotation continues. 
 51          Otherwise the method blocks until the duration is expired. Then the gear stops. 
 52          @param duration: if greater than 0, the method blocks for the given duration (in ms) 
 53          @type duration: int 
 54          ''' 
 55          Tools.debug("Calling Gear.forward() with speed " + str(self.speed)) 
 56          self._checkRobot() 
 57          if self.state != GearState.FORWARD: 
 58              leftDuty = self.speedToDutyCycle(self.speed + SharedConstants.GEAR_FORWARD_SPEED_DIFF) 
 59              rightDuty = self.speedToDutyCycle(self.speed) 
 60              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(leftDuty) 
 61              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0) 
 62              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(rightDuty) 
 63              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0) 
 64              self.state = GearState.FORWARD 
 65          if duration > 0: 
 66              Tools.delay(duration) 
 67              self.stop() 
 68   
 69 -    def backward(self, duration = 0): 
 70          ''' 
 71          Starts the backward rotation with preset speed. 
 72          If duration = 0, the method returns immediately, while the rotation continues. 
 73          Otherwise the method blocks until the duration is expired. Then the gear stops. 
 74          @param duration if greater than 0, the method blocks for the given duration (in ms) 
 75          ''' 
 76          Tools.debug("Calling Gear.backward() with speed " + str(self.speed)) 
 77          self._checkRobot() 
 78          if self.state != GearState.BACKWARD: 
 79              leftDuty = self.speedToDutyCycle(self.speed + SharedConstants.GEAR_BACKWARD_SPEED_DIFF) 
 80              rightDuty = self.speedToDutyCycle(self.speed) 
 81              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0) 
 82              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(leftDuty) 
 83              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0) 
 84              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(rightDuty) 
 85              self.state = GearState.BACKWARD 
 86          if duration > 0: 
 87              Tools.delay(duration) 
 88              self.stop() 
 89   
 90 -    def left(self, duration = 0): 
 91          ''' 
 92          Starts turning left with right motor rotating forward and 
 93          left motor rotating backward at preset speed. 
 94          If duration = 0, the method returns immediately, while the rotation continues. 
 95          Otherwise the method blocks until the duration is expired. Then the gear stops. 
 96          @param duration if greater than 0, the method blocks for the given duration (in ms) 
 97          ''' 
 98          Tools.debug("Calling Gear.left()") 
 99          self._checkRobot() 
100          if self.state != GearState.LEFT: 
101              duty = self.speedToDutyCycle(self.speed) 
102              duty = self.speedToDutyCycle(self.speed) 
103              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0) 
104              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(duty) 
105              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(duty) 
106              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0) 
107              self.state = GearState.LEFT 
108          if duration > 0: 
109              Tools.delay(duration) 
110              self.stop() 
111   
112 -    def right(self, duration = 0): 
113          ''' 
114          Starts turning right with left motor rotating forward and 
115          right motor rotating backward at preset speed. 
116          If duration = 0, the method returns immediately, while the rotation continues. 
117          Otherwise the method blocks until the duration is expired. Then the gear stops. 
118          @param duration if greater than 0, the method blocks for the given duration (in ms) 
119          ''' 
120          Tools.debug("Calling Gear.right()") 
121          self._checkRobot() 
122          if self.state != GearState.RIGHT: 
123              duty = self.speedToDutyCycle(self.speed) 
124              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(duty) 
125              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0) 
126              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0) 
127              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(duty) 
128              self.state = GearState.RIGHT 
129          if duration > 0: 
130              Tools.delay(duration) 
131              self.stop() 
132   
133 -    def leftArc(self, radius, duration = 0): 
134          ''' 
135          Starts turning to the left on an arc with given radius (in m) with preset speed. 
136          If duration = 0, the method returns immediately, while the rotation continues. 
137          Otherwise the method blocks until the duration is expired. Then the gear stops. 
138          If the radius is negative, turns left backwards. 
139          @param duration: 
140          @return: 
141          ''' 
142          Tools.debug("Calling Gear.leftArc() with radius: " + str(radius)) 
143          self._checkRobot() 
144          speed1 = \ 
145              self.speed * (abs(radius) - SharedConstants.GEAR_AXE_LENGTH) / (abs(radius) + SharedConstants.GEAR_AXE_LENGTH) 
146          Tools.debug("Calling leftArc(). Left speed: " + str(speed1) + ". Right speed: " + str(self.speed)) 
147          if self.state != GearState.LEFTARC or radius != self.arcRadius: 
148              self.arcRadius = radius 
149              leftDuty = self.speedToDutyCycle(speed1) 
150              rightDuty = self.speedToDutyCycle(self.speed) 
151              if radius >= 0: 
152                  SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(leftDuty) 
153                  SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0) 
154                  SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(rightDuty) 
155                  SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0) 
156              else: 
157                  SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0) 
158                  SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(rightDuty) 
159                  SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0) 
160                  SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(leftDuty) 
161              self.state = GearState.LEFTARC 
162          if duration > 0: 
163              Tools.delay(duration) 
164              self.stop() 
165   
166 -    def leftArcMilli(self, radius, duration = 0): 
167          ''' 
168          Same as leftArc(radius, duration), but radius in mm 
169          @param radius in mm 
170          ''' 
171          self.leftArc(radius / 1000.0, duration) 
172   
173 -    def rightArc(self, radius, duration = 0): 
174          ''' 
175          Starts turning to the right on an arc with given radius (in m) with preset speed. 
176          If duration = 0, the method returns immediately, while the rotation continues. 
177          Otherwise the method blocks until the duration is expired. Then the gear stops. 
178          If the radius is negative, turns right backwards. 
179          @param duration: 
180          ''' 
181          Tools.debug("Calling Gear.rigthArc() with radius: " + str(radius)) 
182          self._checkRobot() 
183          speed1 = \ 
184              self.speed * (abs(radius) - SharedConstants.GEAR_AXE_LENGTH) / (abs(radius) + SharedConstants.GEAR_AXE_LENGTH) 
185          Tools.debug("Calling rightArc(). Left speed: " + str(self.speed) + ". Right speed: " + str(speed1)) 
186          if self.state != GearState.RIGHTARC or self.arcRadius != radius: 
187              self.arcRadius = radius 
188              leftDuty = self.speedToDutyCycle(self.speed) 
189              rightDuty = self.speedToDutyCycle(speed1) 
190              if radius >= 0: 
191                  SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(leftDuty) 
192                  SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0) 
193                  SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(rightDuty) 
194                  SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0) 
195              else: 
196                  SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0) 
197                  SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(rightDuty) 
198                  SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0) 
199                  SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(leftDuty) 
200              self.state = GearState.RIGHTARC 
201          if duration > 0: 
202              Tools.delay(duration) 
203              self.stop() 
204   
205 -    def rightArcMilli(self, radius, duration = 0): 
206          ''' 
207          Sama as rightArc(radius, duration), but radius in mm 
208          @param radius in mm 
209          ''' 
210          self.rightArc(radius / 1000.0, duration) 
211   
212 -    def stop(self): 
213          ''' 
214          Stops the gear. 
215          (If gear is already stopped, returns immediately.) 
216          ''' 
217          Tools.debug("Calling Gear.stop()") 
218          self._checkRobot() 
219          if self.state != GearState.STOPPED: 
220              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0) 
221              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0) 
222              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0) 
223              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0) 
224              self.state = GearState.STOPPED 
225   
226 -    def setSpeed(self, speed): 
227          ''' 
228          Sets the speed to the given value (arbitrary units). 
229          The speed will be changed to the new value at the next movement call only. 
230          The speed is limited to 0..100. 
231          @param speed: the new speed 0..100 
232          ''' 
233          Tools.debug("Calling Gear.setSpeed with speed: " + str(speed)) 
234          if self.speed == speed: 
235              return 
236          if speed > 100: 
237              speed = 100 
238          if speed < 0: 
239              speed = 0 
240          self.speed = speed 
241          self.state = GearState.UNDEFINED 
242   
243 -    def speedToDutyCycle(self, speed): 
244          ''' 
245          Linear relationship for mapping speed 0..100 to duty cycle 
246          ''' 
247          if speed < 0: 
248              return 0 
249          elif speed > 100: 
250              return 100 
251          return speed 
252   
253 -    def _checkRobot(self): 
254          if RobotInstance.getRobot() == None: 
255              raise Exception("Create Robot instance first") 
256