Source Code for Module Motor

  1  # Motor.java 
  2   
  3  ''' 
  4  Class that represents a motor. 
  5   
  6   This software is part of the raspibrick module. 
  7   It is Open Source Free Software, so you may 
  8   - run the code for any purpose 
  9   - study how the code works and adapt it to your needs 
 10   - integrate all or parts of the code in your own programs 
 11   - redistribute copies of the code 
 12   - improve the code and release your improvements to the public 
 13   However the use of the code is entirely your responsibility. 
 14   ''' 
 15   
 16  from Tools import Tools 
 17  import SharedConstants 
 18  from RobotInstance import RobotInstance 
 19   
 20  # ------------------------   Class MotorState  ---------------------------------------------- 
 21 -class MotorState(): 
 22      FORWARD = 0 
 23      BACKWARD = 1 
 24      STOPPED = 2 
 25      UNDEFINED = 3 
 26   
 27  # ------------------------   Class Motor  --------------------------------------------------- 
 28 -class Motor(): 
 29      ''' 
 30      Class that represents a motor. 
 31      ''' 
 32 -    def __init__(self, id): 
 33          ''' 
 34          Creates a motor instance with given id. 
 35          @param id: 0 for left motor, 1 for right motor 
 36          ''' 
 37          self.id = id 
 38          self.speed = SharedConstants.MOTOR_DEFAULT_SPEED 
 39          self.state = MotorState.UNDEFINED 
 40          self.pwm = [0] * 2 
 41          if self.id == SharedConstants.MOTOR_LEFT: 
 42              self.pwm[0] = SharedConstants.LEFT_MOTOR_PWM[0] 
 43              self.pwm[1] = SharedConstants.LEFT_MOTOR_PWM[1] 
 44          else: 
 45              self.pwm[0] = SharedConstants.RIGHT_MOTOR_PWM[0] 
 46              self.pwm[1] = SharedConstants.RIGHT_MOTOR_PWM[1] 
 47   
 48 -    def forward(self): 
 49          ''' 
 50          Starts the forward rotation with preset speed. 
 51          The method returns immediately, while the rotation continues. 
 52            ''' 
 53          Tools.debug("Calling Motor.forward()") 
 54          if self.state == MotorState.FORWARD: 
 55              return 
 56          self._checkRobot() 
 57          duty = self.speedToDutyCycle(self.speed) 
 58          if self.id == SharedConstants.MOTOR_LEFT: 
 59              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(duty) 
 60              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0) 
 61          else: 
 62              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(duty) 
 63              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0) 
 64   
 65          self.state = MotorState.FORWARD 
 66          Tools.debug("Done Motor.forward()") 
 67   
 68 -    def backward(self): 
 69          ''' 
 70          Starts the backward rotation with preset speed. 
 71          The method returns immediately, while the rotation continues. 
 72          ''' 
 73          Tools.debug("Calling Motor.backward(). MotorID: " + str(self.id)) 
 74          if self.state == MotorState.BACKWARD: 
 75              return 
 76          self._checkRobot() 
 77          duty = self.speedToDutyCycle(self.speed) 
 78          if self.id == SharedConstants.MOTOR_LEFT: 
 79              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0) 
 80              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(duty) 
 81          else: 
 82              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0) 
 83              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(duty) 
 84          self.state = MotorState.BACKWARD 
 85          Tools.debug("Done Motor.backward()") 
 86   
 87 -    def stop(self): 
 88          ''' 
 89          Stops the motor. 
 90          (If motor is already stopped, returns immediately.) 
 91          ''' 
 92          Tools.debug("Calling Motor.stop(). MotorID: " + str(self.id)) 
 93          if self.state == MotorState.STOPPED: 
 94              return 
 95          self._checkRobot() 
 96          if self.id == SharedConstants.MOTOR_LEFT: 
 97              SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0) 
 98              SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0) 
 99          else: 
100              SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0) 
101              SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0) 
102          self.state = MotorState.STOPPED 
103          Tools.debug("Done Motor.stop()") 
104   
105 -    def setSpeed(self, speed): 
106          ''' 
107          Sets the speed to the given value (arbitrary units). 
108          The speed will be changed to the new value at the next movement call only. 
109          The speed is limited to 0..100. 
110          @param speed: the new speed 0..100 
111          ''' 
112          if speed > 100: 
113              speed = 100 
114          if speed < 0: 
115              speed = 0 
116          if self.speed == speed: 
117              return 
118          self.speed = speed 
119          self.state = MotorState.UNDEFINED 
120   
121 -    def speedToDutyCycle(self, speed): 
122          ''' 
123          Linear relationship for mapping speed 0..100 to duty cycle 
124          ''' 
125          if speed < 0: 
126              return 0 
127          elif speed > 100: 
128              return 100 
129          return speed 
130   
131 -    def _checkRobot(self): 
132          if RobotInstance.getRobot() == None: 
133              raise Exception("Create Robot instance first") 
134