树莓派代码分析

银晗

已于 2023-05-19 00:37:53 修改

阅读量312

点赞数 1

文章标签： python 开发语言

于 2023-05-10 12:48:31 首次发布

本文链接：https://blog.csdn.net/RandyHan/article/details/130534989

版权

运动学的库：

import time
import numpy as np
import kinematics  # 运动学库已加密，只供调用

ik = kinematics.IK()  # 实例化逆运动学库
ik.stand(ik.initial_pos, t=500)  # 立正，姿态为ik.initial_pos，时间500ms
print('姿态:\n', np.array(ik.initial_pos))  # 打印查看姿态

# 姿态数据为3x6的数组，表示6条腿的的末端的x，y，z坐标，单位mm
# 头部朝前的方向为x轴， 头部朝前位置为负方向，右边为y轴正， 竖直朝上为z轴正， 从中间两条腿的连线的中点做垂线与上下板相交，取连线中心为零点
# 第一条腿表示头部朝前时左上角所在腿, 逆时针表示1-6
 [
  [-199.53, -177.73, -100.0],
  [0.0,     -211.27, -100.0],
  [199.53,  -177.73, -100.0],
  [199.53,  177.73,  -100.0],
  [0.0,     211.27,  -100.0],
  [-199.53, 177.73,  -100.0]
 ]

参数1：姿态;

参数2：模式;
	2为六足模式;
	4为四足模式，当为4足模式时相应的姿态需要为initial_pos_quadruped

参数3：步幅，单位mm （转弯时为角度，单位度）；

参数4：速度，单位mm/s；

参数5：执行次数，单位0时表示无限循环

ik.go_forward(ik.initial_pos, 2, 50, 80, 1)  # 朝前直走50mm

ik.back(ik.initial_pos, 2, 100, 80, 1)  # 朝后直走100mm

ik.turn_left(ik.initial_pos, 2, 30, 100, 1)  # 原地左转30度

ik.turn_right(ik.initial_pos, 2, 30, 100, 1)  # 原地右转30读

ik.left_move(ik.initial_pos, 2, 100, 100, 1)  # 左移100mm

ik.right_move(ik.initial_pos, 2, 100, 100, 1)  # 右移100mm

ik.stand(ik.initial_pos, t=500)

纠偏主要函数

逻辑：

运动函数直接开守护线程，一直运行；
超声波函数测距 HWSONAR.getDistance()
摄像头返回实时情况

def move():
    while True:
        if __isRunning:
            if 0 < distance < Threshold:
                while distance < 25: # 小于25cm时后退
                    ik.back(ik.initial_pos, 2, 80, 50, 1)
                for i in range(6): # 左转6次，每次15度，一共90度
                    if __isRunning:
                        ik.turn_left(ik.initial_pos, 2, 15, 50, 1)
            else: 
                ik.go_forward(ik.initial_pos, 2, 80, 50, 1)
        else:
            time.sleep(0.01)

threading.Thread(target=move, daemon=True).start()

机械臂

ArmPi AI 智能视觉机械臂

ArmPi 是一款以 OpenCV 为核心，搭配逆运动学算法、计算机视觉和 Python 编程，可以实现图像识别、远程控制、画面回传等功能，能完成颜色分拣、追踪夹取、堆叠方块等创
意玩法。

拓展板

在这里插入图片描述

传感器及引脚控制


import RPi.GPIO as GPIO
from time import sleep
 
servo_pin = 14                # 舵机信号线接树莓派GPIO 17 ,引脚编号为14
 
GPIO.setmode(GPIO.BCM) # 引脚连接方式
GPIO.setwarnings(False)
GPIO.setup(servo_pin, GPIO.OUT, initial=False) # 初始化GPIO接口

# 旋转角度转换到PWM占空比
def angleToDutyCycle(angle):
    return 2.5 + angle / 180.0 * 10

p = GPIO.PWM(servo_pin, 50)    # 初始频率为50HZ
p.start(angleToDutyCycle(90))  # 舵机初始化角度为90
sleep(0.5)
p.ChangeDutyCycle(0)           # 清空当前占空比，使舵机停止抖动

angle = int(0)
p.ChangeDutyCycle(angleToDutyCycle(angle))
sleep(1)
p.ChangeDutyCycle(0) # 清空当前占空比，使舵机停止抖动
sleep(1)
            
angle = int(90)
p.ChangeDutyCycle(angleToDutyCycle(angle))
sleep(1)
p.ChangeDutyCycle(0) # 清空当前占空比，使舵机停止抖动
sleep(1)

GPIO中断

import RPi.GPIO as GPIO
from time import sleep,time
 
servo_pin = 14                # 舵机信号线接树莓派GPIO17
 
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(servo_pin, GPIO.OUT, initial=False)

KEY = 18
GPIO.setup(KEY,GPIO.IN,GPIO.PUD_UP)  
 
 
# 旋转角度转换到PWM占空比
def angleToDutyCycle(angle):
    return 2.5 + angle / 180.0 * 10
 
p = GPIO.PWM(servo_pin, 50)    # 初始频率为50HZ
p.start(angleToDutyCycle(90))  # 舵机初始化角度为90
sleep(0.5)
p.ChangeDutyCycle(0)           # 清空当前占空比，使舵机停止抖动


def event(Key):
    print("Key Press ,door turn on ")
    turn_on()
    sleep(1)
    print("door close...")
    turn_off()
    sleep(1)
    
def turn_on():
    #Turn
    angle = int(0)
    p.ChangeDutyCycle(angleToDutyCycle(angle))
    sleep(0.1)
    #p.ChangeDutyCycle(0) # 清空当前占空比，使舵机停止抖动
    sleep(1)

def turn_off():
    #black
    angle = int(90)
    p.ChangeDutyCycle(angleToDutyCycle(angle))
    sleep(0.1)
    #p.ChangeDutyCycle(0) # 清空当前占空比，使舵机停止抖动
    sleep(1)
    
try:
    GPIO.add_event_detect(KEY,GPIO.FALLING,event,200) # add press event
    while True:
        #turn_on()
        #turn_off()
        
        sleep(1)
except KeyboardInterrupt :
    GPIO.cleanup()

多线程控制


if __name__ == "__main__":
    try:
        global object_num
        object_num= {"red":2,"blue":2,"green":2,"yellow":2,"ball":2}
        GPIO.setmode(GPIO.BCM)
        #setup data
        config = setup_pwm()
        camera=camera_config()
        
        flag = multiprocessing.Value("d",0)
 
        num=20
        processed_img = Processing_Img()
               
        thread1 = multiprocessing.Process(target=Main,args=(processed_img,camera["color_hsv"],config["pic_path"]))
        thread1.start()
        
        thread2 = multiprocessing.Process(target=ConveyorBelt,args=(0,num))
        thread2.start()
           
        thread3 = multiprocessing.Process(target=save_img,args=(config["pic_path"],flag))
        thread3.start()
        count=0
        move_num=3
        while True:
            count+=1
            if flag:
                shape = detect_corlor(config["pic_path"],config)
                print("object shape:",shape)
                try:
                    control_classify(config,shape)
                    print("object num:",object_num.values())
                    if move_num in object_num.values():
                        print("object box is full ,you should move box !",end=" ")
                        for i,j in object_num.items():
                            if j>=3:
                                print(f"{i} box is ready to move")
                                
                                hand_move(config["pwm_up"],config["pwm_median"],config["pwm_down"])
                                #if input("are you want to move box? (y/n):")=='y' else print("no move")
                                load_codes(i,j,shape)
                                break
                    
                except Exception as e:
                    print(e)
            else:
                time.sleep(2)
            flag.Value=0
            
    except KeyboardInterrupt:
        cv2.destroyAllWindows()
        GPIO.cleanup()
        
    finally:
        GPIO.cleanup()
        thread1.kill()
        thread2.kill()
        thread3.kill()