# 双臂魔方机器人 **Repository Path**: hemn1990/double-armed-rubiks-cube-robot ## Basic Information - **Project Name**: 双臂魔方机器人 - **Description**: 双臂魔方机器人，使用手机采集魔方图像并求解，使用两个旋转双指机械爪还原魔方。 - **Primary Language**: C - **License**: MIT - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 54 - **Forks**: 5 - **Created**: 2024-01-21 - **Last Updated**: 2026-03-15 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # 双臂魔方机器人 ## 介绍: 双臂魔方机器人,使用摄像头采集魔方图像并求解,使用两个旋转双指机械爪还原魔方. #### 两套机械部分的方案: 1. 小体积版本:共轴步进电机方案(已经完成设计,设计文件位于 结构图纸/) 2. 高性能版本:独立步进电机方案(尚未设计) #### 两套主控板的方案: 1. RP2040 + TMC2209, 开环方案(已经完成设计,设计文件位于 原理图+PCB/) 2. RP2040 + H桥 + 电流传感 + 磁编码器, 闭环方案(尚未设计) #### 两套图像识别与魔方求解的方案: 1. 使用嵌入式SOC SG2002(正在调试) 2. 使用Android手机,支持多线程求解魔方(尚未设计) ## 软件架构: RP2040/main.c:运动控制板主程序. RP2040/pin_def.h:常量与宏定义. RP2040/step_ctrl.pio:用于产生步进电机驱动脉冲. tools/verify_cube_str_decode.c:在PC上运行,用于验证翻转魔方的算法. tools/cube_table.py:在PC上运行,用于生成魔方翻转表const char cube_status_tab[24][6]. tools/time_cost.c:在PC上运行,用于精确计算步进电机运行时间. ## 安装与调试教程: ### 1. 器件清单 ### 2. 主控板调试 以Linux操作系统为例讲解,主控板调试也可以使用Windows或者MacOS系统. 1) 测试电源是否对地短路,然后将POWER接口连接到20V/4A或以上的Type-C接口电源上,测试电压. | 电源 | 测试点 | 对地电阻（仅供参考） | 实测电压 | 实测纹波 （空载） | |--------|--------|------------------|---------|-----------------| | 3P3V | C11 | 1.9K | 3.28V | 19mV | | 1P1V | C1 | 5.9K | 1.095V | 10mV | | VIN | C51 | 5K | 19.8V | 29mV | | 5V | C55 | 52K | 5.08V | 43mV | 2) 断开POWER 3) 按下BOOT按键,连接DEBUG接口和电脑,识别U盘后松开BOOT按键.(第一次操作时,可省略按下BOOT按键这一步骤) 4) 将RP2040/RP2040.uf2复制到新识别的虚拟U盘.(这不是存储设备,只是下载固件的接口,别拿来保存其他文件,只能复制uf2文件) 5) 断开DEBUG接口,将POWER接口连接到20V/4A或以上的Type-C接口电源上. 6) 连接DEBUG接口和电脑,步骤5和步骤6的顺序不可交换.如果中途断电,需要再次进入调试模式,也要按照这个顺序操作. 如果在DEBUG接口连接始终不中断的情况下,断开POWER接口,然后再次连接POWER接口,会引起功耗异常的问题. 该问题之影响调试,调试完成后,不再需要DEBUG接口. 7) 查看电脑上新识别的串口设备: ``` [ 1949.545676] usb 1-1.1: new full-speed USB device number 5 using xhci_hcd [ 1949.683115] usb 1-1.1: New USB device found, idVendor=2e8a, idProduct=000a, bcdDevice= 1.00 [ 1949.683137] usb 1-1.1: New USB device strings: Mfr=1, Product=2, SerialNumber=3 [ 1949.683156] usb 1-1.1: Product: Pico [ 1949.683174] usb 1-1.1: Manufacturer: Raspberry Pi [ 1949.683191] usb 1-1.1: SerialNumber: E6633455075D4026 [ 1949.698765] cdc_acm 1-1.1:1.0: ttyACM0: USB ACM device ``` 7) 打开新增的虚拟串口,波特率随意设置. ``` minicom -D /dev/ttyACM0 ``` 8) 按住BUT1(SW4)不放,短按RST,出现如下内容后松开BUT1. ``` USB connect ready. tmc2209_config done, addr=0, count=14. tmc2209_config done, addr=1, count=14. tmc2209_config done, addr=2, count=14. tmc2209_config done, addr=3, count=14. _ _ ____ __ __ _ ____ _ _ _ | \ | | _ \ | \/ (_) ___ _ __ ___ / ___|| |__ ___| | | | \| | |_) | | |\/| | |/ __| '__/ _ \ \___ \| '_ \ / _ \ | | | |\ | _ < | | | | | (__| | | (_) | ___) | | | | __/ | | |_| \_|_| \_\ |_| |_|_|\___|_| \___/ |____/|_| |_|\___|_|_| robot: ``` 9) 输入help回车,可列出支持的调试指令 ***可以不重新编译程序,直接修改部分变量*** exit 退出并且保存 help 显示支持的指令 zero_point 使能电机,并且回零点 run_demo 运行魔方打乱和还原的演示程序 move_finger_to_default 移动手指位置 move_finger_to_init 移动手指位置 move_finger_to_max 移动手指位置 move_finger_to_lock 移动手指位置 twist_cube_180 拧魔方 twist_cube_90 left/right cw/ccw 0/1 拧魔方 flip_cube_180 翻转魔方 flip_cube_90 left/right cw/ccw 0/1 翻转魔方 reset 复位单片机 get 获取全部可设置的变量 set xxx xxx设置变量,不检查范围,请设置合理的取值 ***注意:***重新刷写RP2040的固件,不会造成这些变量丢失. 可修改的变量如下,具体定义可参考程序源代码. ``` cfg.SPEED_HOME = 5333.333008; cfg.SPEED_LOW = 10666.666016; cfg.ACCEL_HOME = 100000.000000; cfg.V_START_FINGER = 5333.333008; cfg.V_MAX_FINGER = 42666.664062; cfg.A_MAX_FINGER = 2500000.000000; cfg.V_START_ARM = 5333.333008; cfg.V_MAX_ARM = 16000.000000; cfg.A_MAX_ARM = 900000.000000; cfg.V_START_ARM_L = 2666.666504; cfg.V_MAX_ARM_L = 10666.666016; cfg.A_MAX_ARM_L = 300000.000000; cfg.DELAY_US_AFTER_FINGER_LOCK = 3000; cfg.SPEED_FACTOR_FULL = 1.000000; cfg.SPEED_FACTOR_SLOW = 0.125000; cfg.ARM_OFFSET = 720; cfg.FINGER_OFFSET_LOCK = 185; cfg.FINGER_OFFSET_SPIN = 210; cfg.FINGER_OFFSET_INIT = 230; cfg.FINGER_OFFSET_MAX = 1300; cfg.IRUN = 24; cfg.IHOLD = 13; cfg.SGTHRS = 105; cfg.DEBUG_MODE = 1; ``` ``` 这段内容使用AI根据程序源代码生成,是我写文档的一个新的尝试,有些问题,等我有空再改. 这些变量在机器人控制程序中定义并用于控制一个设计来操作魔方的机械臂装置。以下是各个变量及其功能的详细解释： 1. **cfg.SPEED_HOME**: 定义电机在寻找“原点”或“回家”模式下的最大速度，单位是微步每秒（μsteps/s）。在这个例子中，`cfg.SPEED_HOME` 设置为大约 5333.33 μsteps/s。 2. **cfg.SPEED_LOW**: 设定电机在低速模式下的最大速度，同样以 μsteps/s 为单位。这里设置为约 10666.67 μsteps/s，比“回家”速度更快。 3. **cfg.ACCEL_HOME**: 定义电机在寻找“原点”过程中的加速度，单位是 μsteps/s²。此处设定为 100000.00 μsteps/s²。 4. **cfg.V_START_FINGER** 和 **cfg.V_MAX_FINGER**: 分别表示手指部分电机起始速度和最大速度，用于控制机器手爪的速度变化范围，单位也是 μsteps/s。 5. **cfg.A_MAX_FINGER**: 定义手指部分电机允许的最大加速度，单位同样是 μsteps/s²。 6. **cfg.V_START_ARM** 和 **cfg.V_MAX_ARM**: 类似地，分别表示手臂主轴电机的起始速度和最大速度。 7. **cfg.A_MAX_ARM**: 定义手臂主轴电机的最大加速度。 8. **cfg.V_START_ARM_L** 和 **cfg.V_MAX_ARM_L**: 表示在低速模式下手臂主轴电机的起始速度和最大速度，可能是在执行特定动作时使用的更慢的速度区间。 9. **cfg.A_MAX_ARM_L**: 在低速模式下手臂主轴电机的最大加速度。 10. **cfg.DELAY_US_AFTER_FINGER_LOCK**: 当手指锁定魔方后等待的时间，单位是微秒（us），本例中为 3000 us。 11. **cfg.SPEED_FACTOR_FULL** 和 **cfg.SPEED_FACTOR_SLOW**: 控制电机速度调整的比例因子，全速模式和慢速模式下的倍数。 12. **cfg.ARM_OFFSET**: 定义机械臂从初始位置到光电传感器识别的零点之间的角度偏移量，单位是机械臂细分后的微步数，此例转换为约 720 度。 13. **cfg.FINGER_OFFSET_LOCK**、**cfg.FINGER_OFFSET_SPIN**、**cfg.FINGER_OFFSET_INIT** 和 **cfg.FINGER_OFFSET_MAX**: 这些变量对应于机器手指的不同工作位置，例如锁定魔方、旋转魔方、初始化位置以及手指的最大伸展位置，单位均为微步数。 14. **cfg.IRUN** 和 **cfg.IHOLD**: 定义电机驱动器在运行模式（IRUN）和保持模式（IHOLD）下的电流参数，影响电机产生的扭矩大小。 15. **cfg.SGTHRS**: 定义无极限霍尔传感器的归零阈值，与电机编码器相关，用来判断何时电机回到原点。 16. **cfg.DEBUG_MODE**: 标志位，当其值为1时，系统进入调试模式，可能会开启额外的日志记录或其他调试功能。 ``` ***小技巧:*** 将DEBUG_MODE设置为1,可以在每次启动时自动进入调试模式,无需按下BUT1(SW4),调试完成后改回0即可. ### 3. 机械部分 ### 4. 编译PR2040固件 以Linux操作系统为例讲解,编译PR2040固件也可以使用Windows或者MacOS系统. 参考raspberrypi官网搭建开发环境: https://www.raspberrypi.com/documentation/microcontrollers/rp2040.html 也可以在b站搜索RP2040 SDK,找视频教程. 搭建开发环境后,即可编译. ```` cd RP2040 rm -rf build mkdir build cd build export PICO_SDK_PATH=xxxxxxxx cmake .. make ```` 然后找到RP2040.uf2，刷写到RP2040单片机即可. 也可以使用基于vscode的集成开发环境编译(推荐). ### 5. SG2002开发环境搭建 1)推荐使用docker搭建开发环境.资源占用比虚拟机小很多. 具体步骤可参考https://wiki.sipeed.com/hardware/zh/lichee/RV_Nano/1_intro.html 如果打算使用预编译的操作系统镜像,可以选择只搭建MMF部分. 我开发的时候使用的是这一分支: https://github.com/sipeed/LicheeRV-Nano-Build/tree/v4.1.0-licheervnano 下载全部内容,解压到/home/hemn/SG2004/cvi_mmf_sdk(可以自行修改路径) 2)安装git和docker ```` apt update apt install git docker.io ```` 3)获取工具链: ```` cd /home/hemn/SG2004/cvi_mmf_sdk wget https://sophon-file.sophon.cn/sophon-prod-s3/drive/23/03/07/16/host-tools.tar.gz tar xvf host-tools.tar.gz cd /home/hemn/SG2004/cvi_mmf_sdk/docker ```` 4)如果网络速度不好,建议更换docker源: ```` gedit /etc/docker/daemon.json ```` 加入以下配置项: ```` { "registry-mirrors": [ "https://dockerproxy.com", "https://hub-mirror.c.163.com", "https://mirror.baidubce.com", "https://ccr.ccs.tencentyun.com" ] } ```` 重启 Docker 服务 ```` service docker restart ```` 查看是否成功 ```` docker info ```` 出现以下字段代表配置成功 ```` Registry Mirrors: https://dockerproxy.com/ https://hub-mirror.c.163.com/ https://mirror.baidubce.com/ https://ccr.ccs.tencentyun.com/ ```` 5)构建docker环境用于开发 ```` cd /home/hemn/SG2002/cvi_mmf_sdk/docker docker build -t cvi_mmf_sdk . ```` 6)解决busybox没有httpd的问题 修改buildroot-2021.05/package/busybox/busybox.config 增加: ```` CONFIG_HTTPD=y CONFIG_FEATURE_HTTPD_RANGES=y CONFIG_FEATURE_HTTPD_SETUID=y CONFIG_FEATURE_HTTPD_BASIC_AUTH=y CONFIG_FEATURE_HTTPD_AUTH_MD5=y CONFIG_FEATURE_HTTPD_CGI=y CONFIG_FEATURE_HTTPD_CONFIG_WITH_SCRIPT_INTERPR=y CONFIG_FEATURE_HTTPD_SET_REMOTE_PORT_TO_ENV=y CONFIG_FEATURE_HTTPD_ENCODE_URL_STR=y CONFIG_FEATURE_HTTPD_ERROR_PAGES=y CONFIG_FEATURE_HTTPD_PROXY=y CONFIG_FEATURE_HTTPD_GZIP=y CONFIG_FEATURE_HTTPD_ETAG=y CONFIG_FEATURE_HTTPD_LAST_MODIFIED=y CONFIG_FEATURE_HTTPD_DATE=y CONFIG_FEATURE_HTTPD_ACL_IP=y ```` 7)第一次编译 ```` # 启动docker sudo docker run --rm --name cvi_mmf_sdk -v /home/hemn/SG2002/cvi_mmf_sdk:/cvi_mmf_sdk -it cvi_mmf_sdk /bin/bash ```` ```` # 以下命令在docker环境里面执行 cd /cvi_mmf_sdk # 设置环境 source build/cvisetup.sh # 加载配置 defconfig cv1812cp_licheerv_nano_sd # 构建软件，第一次编译需要联网,buildroot需要从国外网站下载150M左右的数据,如果网络不稳定,中间容易报错,只需要再次运行build_all即可. # 或者有什么其他手段,能解决访问国外网站不稳定的问题也行. build_all # 生成SD卡镜像 ./build/tools/common/sd_tools/sd_gen_burn_image.sh install/soc_cv1812cp_licheerv_nano_sd/ # 编译成功后可以在install目录下看到生成的image # 生成SD卡镜像 ./build/tools/common/sd_tools/sd_gen_burn_image.sh install/soc_cv1812cp_licheerv_nano_sd/ # 编译成功后可以在/cvi_mmf_sdk/install/soc_cv1812cp_licheerv_nano_sd/images目录下看到生成的image ```` 8)将生成的images,烧写到licheerv nano的tf卡,***注意,要写SD卡,别写到电脑系统盘里*** ```` cd /home/hemn/SG2002/cvi_mmf_sdk/install/soc_cv1812cp_licheerv_nano_sd/images ls # 注意,如果自动挂载了,请先卸载 # 注意,要写SD卡,别写到电脑系统盘里 # 推荐使用lsblk指令确认设备名称 dd if=licheervnano-20240330-0934.img of=/dev/sdb ```` ### 6. SG2002 编译魔方识别上位机程序 1)将double-armed-rubiks-cube-robot/SG2002/cube_robot复制到/home/hemn/SG2002/cvi_mmf_sdk/middleware/v2/sample/cube_robot 2)启动docker ```` sudo docker run --rm --name cvi_mmf_sdk -v /home/hemn/SG2002/cvi_mmf_sdk:/cvi_mmf_sdk -it cvi_mmf_sdk /bin/bash ```` 3)加载开发环境的配置,每次启动docker后都要操作 ```` # 以下命令在docker环境里面执行 cd /cvi_mmf_sdk # 设置环境 source build/cvisetup.sh # 加载配置 defconfig cv1812cp_licheerv_nano_sd ```` 4)编译MMF例程和cube_robot程序,通常只需要执行一次,如果make clean了,要重新执行 ```` # 编译MMF例程和cube_robot程序 build_middleware ```` 5)修改cube_robot程序后的增量编译,可以这样操作,节省时间 ```` cd /cvi_mmf_sdk/middleware/v2/sample/cube_robot make ```` 6)裁减cube_robot ```` cd /cvi_mmf_sdk/middleware/v2/sample/cube_robot riscv64-unknown-linux-musl-strip cube_robot ```` 7)将如下文件,通过scp等工具,复制到licheerv nano的/root/ |文件名 |功能 | |-------------|----------------| |cube_robot |魔方识别上位机程序 | |cube_face.bin|魔方色块位置图 | |lookup.dat |二阶段算法数据文件 | |test_case.txt|性能测试的测试用例| ```` # ls /root/ cube_face.bin cube_robot lookup.dat test_case.txt ```` test_case.txt是可选的,cube_robot程序默认禁用该功能,不复制该文件也不影响使用. 参考指令如下,也可以用其他方案,nfs网络共享,或在拆下TF卡用读卡器都可以的: ```` sshpass -p root scp /home/hemn/SG2002/cvi_mmf_sdk/middleware/v2/sample/cube_robot/cube_robot root@10.217.103.1:/root/ ```` 8) 测试魔方机器人上位机 连接到licheerv nano,可选串口、RJ45网络、WIFI网络、USB网络、USB CDC等方式,以下为USB网络方式: 使用ifconfig确认网络状态: ```` enxb2b614f71b97: flags=4163 mtu 1500 inet 10.217.103.191 netmask 255.255.255.0 broadcast 10.217.103.255 inet6 fe80::261c:ceb0:49a7:ea84 prefixlen 64 scopeid 0x20 ether b2:b6:14:f7:1b:97 txqueuelen 1000 (以太网) RX packets 1047 bytes 112724 (112.7 KB) RX errors 0 dropped 0 overruns 0 frame 0 TX packets 1625 bytes 1071686 (1.0 MB) TX errors 0 dropped 0 overruns 0 carrier 0 collisions 0 ```` 使用ssh连接,***初始密码是root***: ```` ssh root@10.217.103.1 ```` 运行测试程序: ```` ./cube_robot ```` 出现Ready. input 0 or 1:后,放置魔方,输入0. 手工翻转魔方,然后输入1. 如果一切正常,会出现魔方的解法: ```` LFUDURFFBRUFLRFLBRLUURFBFRBDDDFDUDBDBLULLBRDLRLURBUBDF Find solution R L2 B F2 D R' L' B2 D' B R2 D2 U' B2 D2 F2 L2 D R2 L2 U' in 11+10=21 steps Find solution B' R' F2 D B2 D' F' U R D F L F2 R2 B2 R2 D B2 U in 12+7=19 steps Final solution is B' R' F2 D B2 D' F' U R D F L F2 R2 B2 R2 D B2 U , length = 19 color_detect() success. wrote bitmap to out0.bmp. wrote bitmap to out1.bmp. ```` 上位机具有一定的容错功能,可识别状态异常的魔方(例如扭角),并尝试还原. ```` LFBUURFUDRBUFRRRLRLLBUFRDDFBBDLDFDUUBRUBLBFDLRDUDBLFFL Twisted corner, ignore. Find solution F' U' L2 B D2 B R' L' U B' D F2 L2 U' L2 D2 R2 F2 U2 B2 L2 in 11+10=21 steps Find solution B' F' U' R L' F2 L F' R2 B' L' B2 U' B2 R2 D R2 D U2 in 11+8=19 steps Final solution is B' F' U' R L' F2 L F' R2 B' L' B2 U' B2 R2 D R2 D U2 , length = 19 color_detect() success. wrote bitmap to out0.bmp. wrote bitmap to out1.bmp. ```` 使用WEB浏览器打开http://10.217.103.1/index.htm ![WEB浏览器界面](img/web.png) ### 7. 上位机程序通信协议 使用cube_robot --uart指令启动上位机,将激活通信功能. 推荐在调试稳定后,在/etc/rc.local中添加如下内容,实现上位机的开机自启动 ```` #!/bin/sh cd /root/ nohup /root/cube_robot --uart & httpd -h /root/ & exit 0 ```` 上位机初始化完成后,通过串口向下位机发送如下数据. ```` \r\n READY\r\n ```` 下位机向上位机发送0,上位机将保存第一张魔方照片,并且在完成后回复ok. ```` OK\r\n ```` 下位机将魔方翻转,例如从红绿朝上,变为蓝橙朝上. 下位机向上位机发送1,上位机将保存第二张魔方照片,并且在完成后回复ok. ```` OK\r\n ```` 上位机识别魔方颜色,并且计算魔方的解法. 如果成功,回复解法. ```` SOLUTION+解法+\r\n ```` 例如: ```` SOLUTION:B D2 F' L F' D2 L' D2 R' D' F D B2 R2 L2 F2 L2 D F2 D2 F2\r\n ```` 如果失败,回复FAIL. ```` FAIL\r\n ```` ### 8. SG2002 上位机程序的参数调试 等star多了就更新. ## 注意事项: 1. 3D打印的齿轮的5mm D型孔,在使用一段时间后会变大松动,引起夹不紧魔方的问题,目前的解决方案是用502胶水固定. 2. 魔方表面如果有较多油脂,也会引起夹不紧魔方的问题. 3. xxxx 4. xxxx ## 主要器件清单 主要器件清单，目前比较乱，有空再整理格式。 ```` 双层叠式42步进电机微型步进马达42电机 双轴正反转控制1.8度二相 数量2个，配套线缆4根 ITR9606 光电传感器 数量2个 2.54mm间距 4pin连线2条，配合光电传感器用 gitee开源的原理图+PCB目录里BOM表格中的物料 数量1套 额外加少量2mm厚度的导热垫 M2.6x12圆头平尾自攻螺丝 16个 M3x10内六角螺丝（十字花盘头或者沉头的也行） 8个 M3x4x5滚花螺母5个 2mm厚度硅胶垫少量 Type-C接口电源适配器一个，至少80W，配套线缆一根，或者可调直流稳压电源一台 主体框架：3D打印ABS耗材若干 手臂手指：3D打印PETG或者ABS耗材若干 打印数量如下： 爪子底部.stl 2个 爪子手指.stl 4个 爪子顶部.stl 2个 爪子齿轮.stl 2个 电机支架.stl 1个 锂基润滑脂少量 双面胶少量 2mm麻花钻一根（重要，拿来修整自攻螺丝孔，没这个拧螺丝时会开裂） 80目左右的砂纸1张 M3机米螺丝4个，3mm长度 图像识别部分用Sipeed LicheeRV Nano荔枝派SG2002+配套摄像头GC4653 Type-C数据线一根，调试用 螺丝安装位置说明 M3滚花螺母(M3x4x5)，滚花螺母外径5mm，螺纹M3，光杆直径4.3mm，高度4mm 电机轴附近的：M3机米螺丝，平头，3mm长度 旋转臂上的：M2.6x12圆头平尾自攻螺丝 16个 固定电机的：M3x10内六角螺丝（十字花盘头或者沉头的也行） 8个 固定摄像头的那一个：随便找个M3的螺丝拧上去就行 手臂手指部分打印参数 打印参数（仅供参考，根据打印机、材料的情况进行设置） 材料：PETG 喷嘴：0.4mm，线宽：0.4mm 层高：0.15mm，顶层：7层，底层：7层 外壳：3圈，外壳打印顺序：从外侧到内侧，提升尺寸精度 填充：80%，提升强度，防止高速运动时震动 风扇：30%，在不影响悬垂的前提下，尽可能慢，提升强度 热端温度：245℃，热床温度70℃ 打印速度：80mm/s，加速度2000mm/s2，尽可能慢，提升精度 (平时打印对精度没有要求的模型，速度200mm/s) 齿轮单独打印，一个一个来，否则齿面上容易出现毛刺，不好处理打印速度：20mm/s 其他一样 后处理： 爪子顶部组件上的8个自攻螺丝孔，孔径控制在2.0mm±0.1mm， 太大了容易滑丝，太小了容易在拧螺丝时开裂，如果打印后的 孔径误差较大，需要用2mm麻花钻钻一下。 除了齿轮和齿条有齿的表面，其他互相接触的面的毛刺， 都需要打磨掉，打磨后需要测试滑动是否顺畅。 组装之前涂抹润滑脂 主体框架打印要求： 使用耐高温的材料打印，例如ABS，0.4喷嘴，0.2层高，外壳3-4圈，填充推荐20-40% ````