예제 #1
0
파일: Servo.py 프로젝트: edwardchor/Zeusy
class Servo(object):
    '''Servo driver class'''
    _MIN_PULSE_WIDTH = 600
    _MAX_PULSE_WIDTH = 2400
    _DEFAULT_PULSE_WIDTH = 1500
    _FREQUENCY = 60

    _DEBUG = False
    _DEBUG_INFO = 'DEBUG "Servo.py":'

    def __init__(self,
                 channel,
                 offset=-19,
                 lock=True,
                 bus_number=None,
                 address=0x40):
        ''' Init a servo on specific channel, this offset '''
        if channel < 0 or channel > 16:
            raise ValueError(
                "Servo channel \"{0}\" is not in (0, 15).".format(channel))
        if self._DEBUG:
            print(self._DEBUG_INFO, "Debug on")
        self.channel = channel
        self.offset = offset
        self.lock = lock

        self.pwm = PWM(bus_number=bus_number, address=address)
        self.frequency = self._FREQUENCY
        self.write(90)

    def setup(self):
        self.pwm.setup()

    def _angle_to_analog(self, angle):
        ''' Calculate 12-bit analog value from giving angle '''
        pulse_wide = self.pwm.map(angle, 0, 180, self._MIN_PULSE_WIDTH,
                                  self._MAX_PULSE_WIDTH)
        analog_value = int(float(pulse_wide) / 1000000 * self.frequency * 4096)
        if self._DEBUG:
            print(self._DEBUG_INFO,
                  'Angle %d equals Analog_value %d' % (angle, analog_value))
        return analog_value

    @property
    def frequency(self):
        return self._frequency

    @frequency.setter
    def frequency(self, value):
        self._frequency = value
        self.pwm.frequency = value

    @property
    def offset(self):
        return self._offset

    @offset.setter
    def offset(self, value):
        ''' Set offset for much user-friendly '''
        self._offset = value
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Set offset to %d' % self.offset)

    def write(self, angle):
        ''' Turn the servo with giving angle. '''
        if self.lock:
            if angle > 180:
                angle = 180
            if angle < 0:
                angle = 0
        else:
            if angle < 0 or angle > 180:
                raise ValueError(
                    "Servo \"{0}\" turn angle \"{1}\" is not in (0, 180).".
                    format(self.channel, angle))
        val = self._angle_to_analog(angle)
        val += self.offset
        self.pwm.write(self.channel, 0, val)
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Turn angle = %d' % angle)

    @property
    def debug(self):
        return self._DEBUG

    @debug.setter
    def debug(self, debug):
        ''' Set if debug information shows '''
        if debug in (True, False):
            self._DEBUG = debug
        else:
            raise ValueError(
                'debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'
                .format(debug))

        if self._DEBUG:
            print(self._DEBUG_INFO, "Set debug on")
        else:
            print(self._DEBUG_INFO, "Set debug off")
예제 #2
0
class Back_Wheels(object):
    ''' Back wheels control class '''
    Motor_A = 17
    Motor_B = 27

    PWM_A = 4
    PWM_B = 5

    _DEBUG = False
    _DEBUG_INFO = 'DEBUG "back_wheels.py":'

    def __init__(self, debug=False, bus_number=1, db="config"):
        ''' Init the direction channel and pwm channel '''
        self.forward_A = True
        self.forward_B = True

        self.db = fileDB(db=db)

        self.forward_A = int(self.db.get('forward_A', default_value=1))
        self.forward_B = int(self.db.get('forward_B', default_value=1))

        self.left_wheel = Motor(self.Motor_A, offset=self.forward_A)
        self.right_wheel = Motor(self.Motor_B, offset=self.forward_B)

        self.pwm = PWM(bus_number=bus_number)

        def _set_a_pwm(value):
            pulse_wide = self.pwm.map(value, 0, 100, 0, 4095)
            self.pwm.write(self.PWM_A, 0, pulse_wide)

        def _set_b_pwm(value):
            pulse_wide = self.pwm.map(value, 0, 100, 0, 4095)
            self.pwm.write(self.PWM_B, 0, pulse_wide)

        self.left_wheel.pwm = _set_a_pwm
        self.right_wheel.pwm = _set_b_pwm

        self._speed = 0

        self.debug = debug
        if self._DEBUG:
            print(
                self._DEBUG_INFO, 'Set left wheel to #%d, PWM channel to %d' %
                (self.Motor_A, self.PWM_A))
            print(
                self._DEBUG_INFO, 'Set right wheel to #%d, PWM channel to %d' %
                (self.Motor_B, self.PWM_B))

    def forward(self):
        ''' Move both wheels forward '''
        self.left_wheel.forward()
        self.right_wheel.forward()
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Running forward')

    def backward(self):
        ''' Move both wheels backward '''
        self.left_wheel.backward()
        self.right_wheel.backward()
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Running backward')

    def stop(self):
        ''' Stop both wheels '''
        self.left_wheel.stop()
        self.right_wheel.stop()
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Stop')

    @property
    def speed(self, speed):
        return self._speed

    @speed.setter
    def speed(self, speed):
        self._speed = speed
        ''' Set moving speeds '''
        self.left_wheel.speed = self._speed
        self.right_wheel.speed = self._speed
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Set speed to', self._speed)

    @property
    def debug(self):
        return self._DEBUG

    @debug.setter
    def debug(self, debug):
        ''' Set if debug information shows '''
        if debug in (True, False):
            self._DEBUG = debug
        else:
            raise ValueError(
                'debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'
                .format(debug))

        if self._DEBUG:
            print(self._DEBUG_INFO, "Set debug on")
            self.left_wheel.debug = True
            self.right_wheel.debug = True
            self.pwm.debug = True
        else:
            print(self._DEBUG_INFO, "Set debug off")
            self.left_wheel.debug = False
            self.right_wheel.debug = False
            self.pwm.debug = False

    def ready(self):
        ''' Get the back wheels to the ready position. (stop) '''
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Turn to "Ready" position')
        self.left_wheel.offset = self.forward_A
        self.right_wheel.offset = self.forward_B
        self.stop()

    def calibration(self):
        ''' Get the front wheels to the calibration position. '''
        if self._DEBUG:
            print(self._DEBUG_INFO, 'Turn to "Calibration" position')
        self.speed = 50
        self.forward()
        self.cali_forward_A = self.forward_A
        self.cali_forward_B = self.forward_B

    def cali_left(self):
        ''' Reverse the left wheels forward direction in calibration '''
        self.cali_forward_A = (1 + self.cali_forward_A) & 1
        self.left_wheel.offset = self.cali_forward_A
        self.forward()

    def cali_right(self):
        ''' Reverse the right wheels forward direction in calibration '''
        self.cali_forward_B = (1 + self.cali_forward_B) & 1
        self.right_wheel.offset = self.cali_forward_B
        self.forward()

    def cali_ok(self):
        ''' Save the calibration value '''
        self.forward_A = self.cali_forward_A
        self.forward_B = self.cali_forward_B
        self.db.set('forward_A', self.forward_A)
        self.db.set('forward_B', self.forward_B)
        self.stop()

    def test(self):
        import time
        back_wheels = Back_Wheels()
        DELAY = 0.01
        try:
            back_wheels.forward()
            for i in range(0, 100):
                back_wheels.speed = i
                print("Forward, speed =", i)
                time.sleep(DELAY)
            for i in range(100, 0, -1):
                back_wheels.speed = i
                print("Forward, speed =", i)
                time.sleep(DELAY)

            back_wheels.backward()
            for i in range(0, 100):
                back_wheels.speed = i
                print("Backward, speed =", i)
                time.sleep(DELAY)
            for i in range(100, 0, -1):
                back_wheels.speed = i
                print("Backward, speed =", i)
                time.sleep(DELAY)
        except KeyboardInterrupt:
            print("KeyboardInterrupt, motor stop")
            back_wheels.stop()
        finally:
            print("Finished, motor stop")
            back_wheels.stop()