レゴカーコントローラーの抜粋(サーボモータ)

legocar_controller.py

class NewServoMotor:
    '''
    サーボモータを制御するクラス
    少しだけ仕組みを理解した上で、かいてみる
    SG5010、SG90のサーボモータに対応している。
    具体的には以下の仕様に依存している。

        制御パルス:0.5ms〜2.4ms
        PWMサイクル:20ms

    '''

    #制御パルス、PWMサイクル、pwmWrite関数の仕様により求められるduty比
    MIN_DUTY = 26;   #-90度
    MAX_DUTY = 123;  # 90度
    MID_DUTY =  MAX_DUTY - MIN_DUTY   # 0度

    #サーボモータを曲げる角度を定義しておく。
    # 大胆に曲げたい場合は、この数字を大きくしよう。最大90度かな。
    # 細かく制御したい場合は、この数字を小さくしよう。最小1度かな。
    ANGLE_BEND = 10;

    #ANGLE_BENDが全体(180度)に対して占める割合
    RATIO_OF_ANGLE_BEND = int( 180 / ANGLE_BEND)

    #ANGLE_BENDを実現するためduty比
    ANGLE_BEND_DUTY = int( (MAX_DUTY - MIN_DUTY) / RATIO_OF_ANGLE_BEND)

    #もしduty比が1未満になっちゃったらduty比1としよう。
    if ANGLE_BEND_DUTY == 0 : ANGLE_BEND_DUTY = 1



    def __init__(self, servo_motor_pin1):
        self.motor_pin1 = servo_motor_pin1
        #ピンの割り当て
        pi.pinMode( self.motor_pin1, pi.GPIO.PWM_OUTPUT )

        #duty比を変更すると周波数がかわってしまうので、固定するための設定、あまりわかってない。
        pi.pwmSetMode( pi.GPIO.PWM_MODE_MS )

        #周波数を50Hzにすると、18750/周波数=375 あまりわかってない。
        pi.pwmSetClock(375)

        #起動時にタイヤをまっすぐにしとく
        self.dutyState = NewServoMotor.MID_DUTY;
        pi.pwmWrite(self.motor_pin1,self.dutyState);


    def minusRotation(self):
        '''
        マイナス側にサーボモータを回転させる。
        曲がる角度は、クラス変数 ANGLE_BENDで指定した角度。
        :return:
        '''
        self.dutyState -= NewServoMotor.ANGLE_BEND_DUTY;
        print(NewServoMotor.ANGLE_BEND_DUTY)
        print(self.dutyState)

        #限界(-90度)を超える場合は、-90度とする
        if(self.dutyState < NewServoMotor.MIN_DUTY):
            self.dutyState =  NewServoMotor.MIN_DUTY

        pi.pwmWrite(self.motor_pin1,self.dutyState);

    def plusRotation(self):
        '''
        プラス側にサーボモータを回転させる。
        minusRotationと同様。
        :return:
        '''
        self.dutyState += NewServoMotor.ANGLE_BEND_DUTY;
        print(self.dutyState)

        #限界(90度)を超える場合は、90度とする
        if(self.dutyState > NewServoMotor.MAX_DUTY):
            self.dutyState =  NewServoMotor.MAX_DUTY

        pi.pwmWrite(self.motor_pin1,self.dutyState);