6DofArm Raspberry pi SoftwarePWM測試程式python版

#-*- coding: UTF-8 -*- 

import RPi.GPIO as GPIO

import time

def angle_to_duty_cycle(angle=0):

    duty_cycle = (0.05 * SERVO_MOTOR_FREQUENCY) + (0.19 * SERVO_MOTOR_FREQUENCY * angle /  SERVO_MOTOR_ANGLE_MAX)

    return duty_cycle

def move_servo(pin, pwm, angle):

    dc = angle_to_duty_cycle(angle)

    pwm.ChangeDutyCycle(dc)

    print('第{: >2}軸 角度={: >3}, 工作週期={:.2f}'.format(pin, angle, dc))

    

def manual_control(pin, pwm):

    print('Move the servo manually (press "x" to exit).')

    input_data = None

    while True:

        input_data = input("Enter the angle: ")

        if input_data.lower() == 'x':

            break

        try:

            angle = int(input_data)

        except Exception:

            print('Either enter an integer or "x" to exit.')

            continue

        try:

            move_servo(pin, pwm, angle)

        except ValueError:

            print('The inputted angle is not valid.')

            continue

def automatic_control(pin, pwm):

    print('Move the servo with a test pattern.')

    angle=SERVO_MOTOR_ANGLE_MIN

    move_servo(pin, pwm, angle)

    time.sleep(3)

    angle=SERVO_MOTOR_ANGLE_MAX

    move_servo(pin, pwm, angle)

    time.sleep(3)

    angle=90

    move_servo(pin, pwm, angle)

    time.sleep(3)

    angle=110

    move_servo(pin, pwm, angle)

    time.sleep(3)

try:

    CONTROL_PIN1 = 4

    CONTROL_PIN2 = 17

    CONTROL_PIN3 = 22

    CONTROL_PIN4 = 23

    CONTROL_PIN5 = 24

    CONTROL_PIN6 = 27

    SERVO_MOTOR_ANGLE_MIN = 0  

    SERVO_MOTOR_ANGLE_MAX = 270 

    SERVO_MOTOR_FREQUENCY = 50

    GPIO.setmode(GPIO.BCM)

    GPIO.setup(CONTROL_PIN1, GPIO.OUT)

    GPIO.setup(CONTROL_PIN2, GPIO.OUT)

    GPIO.setup(CONTROL_PIN3, GPIO.OUT)

    GPIO.setup(CONTROL_PIN4, GPIO.OUT)

    GPIO.setup(CONTROL_PIN5, GPIO.OUT)

    GPIO.setup(CONTROL_PIN6, GPIO.OUT)

    pwmList=list()

    pwmList.append(GPIO.PWM(CONTROL_PIN1, SERVO_MOTOR_FREQUENCY))

    pwmList.append(GPIO.PWM(CONTROL_PIN2, SERVO_MOTOR_FREQUENCY))

    pwmList.append(GPIO.PWM(CONTROL_PIN3, SERVO_MOTOR_FREQUENCY))

    pwmList.append(GPIO.PWM(CONTROL_PIN4, SERVO_MOTOR_FREQUENCY))

    pwmList.append(GPIO.PWM(CONTROL_PIN5, SERVO_MOTOR_FREQUENCY))

    pwmList.append(GPIO.PWM(CONTROL_PIN6, SERVO_MOTOR_FREQUENCY))

    for pwm in pwmList:

        pwm.start(0)

        #pwm.ChangeDutyCycle(0)

    move_servo(1, pwmList[1], 50)

    #time.sleep(3)

    move_servo(2, pwmList[2], 45)

    #time.sleep(2)

    move_servo(3, pwmList[3], 90)

    #time.sleep(2)

    move_servo(0, pwmList[0], 75)

    #time.sleep(2)

    while True:

        input_data = input("What servo motor (channel) you want to control? ")

        try:

            servo_aix = int(input_data)

            if servo_aix < 0 or servo_aix > 5:

                raise ValueError

        except Exception:

            print('Enter an integer between 0 and 5.')

            continue

        input_data = str(input("What do you want to do?\n[1] Manual test\n[2] Automatic test\n[x] Exit\nAnswer: "))

        if input_data == '1':

            manual_control(servo_aix, pwmList[servo_aix])

        elif input_data == '2':

            automatic_control(servo_aix, pwmList[servo_aix])

        elif input_data.lower() == 'x':

            break

        else:

            print('Unrecognized input.')

            continue

except KeyboardInterrupt:

    print('關閉程式')

finally:

    for pwm in pwmList:

        pwm.stop(0)

    GPIO.cleanup()