写字机器人——简易版demo全流程实现

# JFZero

已于 2022-05-15 18:57:21 修改

阅读量4.9k

点赞数 7

分类专栏：硬件机器人文章标签： python 硬件工程

于 2022-05-14 20:41:35 首次发布

本文链接：https://blog.csdn.net/weixin_50348308/article/details/124769247

版权

硬件机器人专栏收录该内容

3 篇文章

订阅专栏

基本思路：turtle画图——机械臂绘制

需要包含模块：
1、turtle绘图并记录坐标
2、坐标转换为机械臂角度
3、机械臂角度转为舵机的pwm值
4、将pwm值传入pca9685控制芯片，并执行

硬件部分：
需要3个舵机，分别为大臂、小臂、以及控制抬笔落笔（简称“抬落臂”，谐音“抬落笔”。。。）
在这里插入图片描述
核心计算：
1、坐标转为机械臂角度
2、角度转为pwm值（难）

（1）坐标转化为大、小臂的角度

灯灯大神图请添加图片描述

class Angle():
    def __init__(self,position):
        # 大臂与x正轴的夹角为alpha，小臂与大臂夹角为beta,根据终点坐标position，计算出夹角alpha和beta
        self.x,self.y = position
        # 大臂臂长OB,小臂BA
        self.OB = 50
        self.BA = 50
        self.OA = (self.x**2+self.y**2)**0.5
        # 机械臂默认只能向上绘画，且距离不应该超过大臂+小臂的长度之和
        assert self.y >= 0 and self.OA <= self.OB+self.BA,"the position out of the drawhand range!"
    # 计算大臂的alpha
    def get_alpha(self):
        # alpha = XOA-AOB，因此需要计算XOA和AOB
        XOA = atan(self.y/self.x)
        AOB = acos((self.OA**2+self.OB**2-self.AB**2)/(2*self.OA*self.OB))
        alpha = XOA-AOB
        return alpha
    # 计算小臂的beta
    def get_beta(self):
        # beta = 180-ABO，因此需要计算ABO
        ABO = acos((self.BA**2+self.OB**2-self.BA**2)/(2*self.BA*self.OB))
        beta = 180-ABO
        return beta

（2）角度转为pwm值

需要解决第1个问题：什么是pwm
请添加图片描述
PWM（Pulse Width Modulation），脉冲宽度调制，简称脉宽调制，即通过调整脉冲信号的宽度来实现对电机功率、舵机角度、灯的功率的调节

简单说，PWM值就是可以控制电路的数字。
原理：将角度转换为PWM值后，再传入pca9685中，在内部完成pwm值转为十六进制数，再将十六进制中的每一个数字转为二进制，存入对应的寄存器内。接着会将存储的二进制数据，用于控制高低电平电路，交给舵机去执行。

详细原理：
一般传入的pwm值，是一个数字，例如200，转化为十六进制就是，0xC8，并最终打包存入对应的寄存器后。寄存器内部再将0xC8转为二进制，则是11001000，根据之前提前设置的舵机频率（其实也就是pca9685根据舵机频率计算出这段pwm值的脉宽）
在这里插入图片描述
但以上解释都不重要，大概知道pwm值用于控制舵机运行即可。最重要的是下边：
pwm值和舵机角度之间为线性关系，一般0°到180°对应的pwm值为102到512。
一般会给舵机设置初始状态，初始状态可以设置为水平的如下图。
在这里插入图片描述
后续控制舵机进行一定角度的转动都需要基于该数值进行计算。
已知目标角度，计算相应的pwm的计算的公式（简称角度转pwm值）为：
object_pwm = init_pwm + angle*k
这个k值，是pwm值和舵机角度之间为线性关系的斜率，就相当于是，每度的单位pwm值，例如控制小臂舵机角度设置为5度，则转化为pwm值，则是init_pwm[2]+5*k。
一般按理说，相同规格的舵机，k值应该是一样的，一般0°到180°对应的pwm值为102到512，那么k值应该是（512-102）/180=2.73…
但是！！！使用这个k值去进行验证时，发现舵机转动的角度是不对的。
于是深刻怀疑，市面上的舵机不规范，没有统一标准的k值（也有可能pwm与角度的关系不是一元线性方程那样简单，但不管了）

所以，便独立对每个舵机进行测量运算，让每个舵机都转动90度后，记录下每个舵机对应的pwm值，然后计算出每个舵机的k值：pwm/90
最终得出了每个舵机的初始pwm值，以及k值
在这里插入图片描述
计算出的大小臂object_pwm保存在列表中，控制抬落臂的舵机，暂时不做改变。（简易版，是一笔画完，所以抬落臂只在开始画时落臂，在结束时抬臂）

class Pwm:
    def __init__(self,position):
        self.init_pwm = [330,185,390]
        self.k = [205/90,180/90,160/90]
        self.object_pwm = self.init_pwm
        self.leg = Angle(position)
    def get_pwm(self):
        leg_angles = self.leg.get_angles()
        # 计算控制大臂和小臂的舵机object_pwm
        for index in range(2):
            self.object_pwm[index+1] = self.init_pwm[index+1]+self.k[index]*leg_angles[index]

将转化好的pwm值，传入pca9685寄存器

这里涉及硬件简单的相关知识
首先是认识esp32主控芯片，和舵机之间，数据传输的途径。
首先，esp32是个主控芯片，但严格意义来说，不是类似于Arduino开发板。
作为硬件小白，简单认识esp32和Arduino的关系
esp32和Arduino的区别与关系

不重要，重要的是，esp32可以将计算出的pwm值，通过引脚传出去。
引脚是什么？就是esp32上的接线口，要将两根线分别接在SCL、和SDA引脚上，实现esp32与硬件的信息传递
在这里插入图片描述

这里简单了解几个硬件数据传输的概念，
I2C总线：I2C总线是由Philips公司开发的一种简单、双向二线制同步串行总线。它只需要两根线即可在连接于总线上的器件之间传送信息。
传送信息的两个线：SCL（控制线）、SDA（数据线）
在这里插入图片描述

硬件上，引脚连线SCL和SDA后，还要将I2C与pca9685进行绑定
这其中，esp32、I2C、引脚、pca9685、舵机之间的关系
硬件部分：
esp32是主控芯片，编程数据计算都在里边完成
引脚使用I2C总线，是通过两根线将esp32和舵机控制器pca9685，完成SCL和SDA的连接
pca9685与舵机连接，将PWM输出，控制舵机
编程部分：
I2C的实现，是通过machine库里，导入I2C、PIN两个模块

		
		from pca9685 import PCA9685
        from machine import Pin, I2C
        # 舵机控制
        self.i2c = I2C(sda=Pin(21),scl=Pin(22),freq=10000)  #i2c通讯,21和22是根据芯片的引脚标识所规定的
        self.pca = PCA9685(self.i2c)    #舵机对象（猜想：这里不是舵机对象，而是将i2c对象与pca9685进行绑定，由i2c向pca9685的对应寄存器写入数据）
        self.pca.freq(50)   #设置舵机的频率为50，这里的freq函数，主要用于设置pca9685内部的单位计时（详见pca9685.py解析）

舵机运行

舵机运行，是通过向pca9685的寄存器存储pwm值即可，这里只需要调用pca9685内的duty方法，传入pwm

    def servo_exc(self,servo_index,duty):
        '''传入舵机编号和pwm值（即duty-占空比）'''
        # self.pca.duty是传入十进制数值，而在pca的对象方法中，有self.pwm，自动转化数字为对应的十六进制，并自动写入pca9685的寄存器中
        # 每次在同一位置传入数据，该位置的寄存器都会更新。
        # duty中传入的舵机编号，是从0开始的，在pca9685的self.pwm方法中会自动根据编号，对应到相应的寄存器（寄存器内部不是从0开始，具体参见pca9685），
        self.pca.duty(servo_index,duty)

至此，整个写字机器人的核心部分拆分讲解已完成，开始根据模块组装重写代码
流程为：

turtle画图

#绘制并获取坐标

Paint.py——绘制简单的正方形

# 绘制简单的正方形
import turtle
pen = turtle.Pen()
for i in range(4): # 循环4次
	pen.forward(100) # 前进
	pen.left(360/4)	# 左转90度
turtle.done()

但要记录坐标，就不能这么简单了，需要将绘制的线段截取成一个个的点。
需要设置一个变量n，表示每次forward前进的线段，被分为n次获取坐标。再将获取的坐标，存入txt文档里。因此改变后的程序如下

# 绘制一个正方形，并将点坐标记录下来
import turtle
pen = turtle.Pen()
n = 30 # 人为设置
positions = [] # 存储坐标
# 绘制边长为100的正方形
for i in range(4):
    # 画长度为100的正方形单边
    for j in range(n):
        positions.append(pen.pos())
        pen.forward(100/n)
    pen.left(90)
# 坐标存入positions.txt文件中
with open("positions.txt", "w", encoding="utf-8") as file:
    file.write(str(positions))
turtle.done()
# 优化建议：根据线段长短，对n进行等比例的缩放，但是，暂时懒得搞了

机械臂绘制

# pos_to_angle——坐标转换成机械臂角度
# angle_to_pwm——角度转为舵机的pwm值
# esp32与pca9685的数据连接——I2C总线
# 将pwm值对应传入pca9685控制芯片中，舵机执行

calculate.py
将坐标转为角度，角度转为pwm放在calculate.py

from math import acos,atan
class Angle():
    def __init__(self):
        # 大臂臂长OB,小臂BA
        self.OB = 50
        self.BA = 50
        self.OA = self.OB+self.BA
        self.x,self.y = self.OA,0
        # 设置三个舵机的初始pwm值、k值属性、方向属性muti
        self.init_pwm = [330,185,390]
        self.k = [205/90,180/90,160/90]
        # 由于第2个舵机是反着装的，因此pwm值应当是与初始pwm相减,其实在pca9685的duty中有一个invert参数，可以很好的解决这个问题，但没用过
        self.multi = [1,1,-1]
        self.object_pwm = self.init_pwm
    # 数据预处理
    def pre_data(self,position):
        # 大臂与x正轴的夹角为alpha，小臂与大臂夹角为beta,根据终点坐标position，计算出夹角alpha和beta
        self.x, self.y = position
        self.OA = (self.x**2+self.y**2)**0.5
        # 机械臂默认只能向上绘画，且距离不应该超过大臂+小臂的长度之和
        assert self.y >= 0 and self.OA < self.OB+self.BA,"the position out of the drawhand range!"
    # 计算大臂的alpha
    def get_alpha(self):
        # alpha = XOA-AOB，因此需要计算XOA和AOB
        XOA = atan(self.y/self.x)
        AOB = acos((self.OA**2+self.OB**2-self.AB**2)/(2*self.OA*self.OB))
        alpha = XOA-AOB
        return alpha
    # 计算小臂的beta
    def get_beta(self):
        # beta = 180-ABO，因此需要计算ABO
        ABO = acos((self.BA**2+self.OB**2-self.BA**2)/(2*self.BA*self.OB))
        beta = 180-ABO
        return beta
    # 同时获取alpha和beta
    def get_angles(self,position):
        self.pre_data(position) # 对坐标数据进行预处理，算出self.OA值
        alpha = self.get_alpha()
        beta = self.get_beta()
        return alpha,beta
    # 获取舵机的最终pwm值
    def get_pwms(self,position):
        leg_angles = self.get_angles(position)
        # 计算控制大臂和小臂的舵机object_pwm，先暂时不管第一个舵机的pwm值
        for index in range(2):
            self.object_pwm[index+1] = self.init_pwm[index+1]+self.k[index]*leg_angles[index]*self.multi[index+1]
        return self.object_pwm

servo.py——伺服，可以说是I2C总线，对esp32与pca9685的连接，与执行

from pca9685 import PCA9685
from machine import Pin, I2C
class Servo:
    def __init__(self):
        # 舵机控制
        self.i2c = I2C(sda=Pin(21),scl=Pin(22),freq=10000)  #i2c通讯,21和22是根据芯片的引脚标识所规定的
        self.pca = PCA9685(self.i2c)    #舵机对象（猜想：这里不是舵机对象，而是将i2c对象与pca9685进行绑定，由i2c向pca9685的对应寄存器写入数据）
        self.pca.freq(50)   #设置舵机的频率为50，这里的freq函数，主要用于设置pca9685内部的单位计时（详见pca9685.py解析）
    # 传入寄存器后，舵机执行    
    def servo_exc(self,servo_index,duty):
        '''传入舵机编号和pwm值（即duty-占空比）'''
        # self.pca.duty是传入十进制数值，而在pca的对象方法中，有self.pwm，自动转化数字为对应的十六进制，并自动写入pca9685的寄存器中
        # 每次在同一位置传入数据，该位置的寄存器都会更新。
        # duty中传入的舵机编号，是从0开始的，在pca9685的self.pwm方法中会自动根据编号，对应到相应的寄存器（寄存器内部不是从0开始，具体参见pca9685），
        self.pca.duty(servo_index,duty)

main.py——将各个py文件综合组装

from calculate_topwn import *
from servo import *
import time
import Paint
hand = Angle()
servo = Servo()
if __name__=="__main__":
    with open("positions.txt", "r", encoding="utf-8") as file:
        text = file.read()
    positions = eval(text)  # 字符串转为列表
    for pos in positions:  # 落笔到抬笔的所有坐标
        pwms = hand.get_pwms(pos)
        # 两个舵机的角度转pwm
        for index in range(1,3):
            servo.servo_exc(index, pwms[index])
            # 0号舵机放下
            servo.servo_exc(0, pwms[0])
        time.sleep(0.05)
    # 0号舵机抬起
    servo.servo_exc(0, pwms[0] + 30)
    time.sleep(0.5)