技能梳理27@体感机械臂_机械臂的背景及意义

最近在抽时间集中整理之前做过的东西，有纯粹入门学习的、课程设计的、老师项目的、帮忙同学的、业余瞎搞的、毕业设计的（自己和“他人”）、之前在破产公司搞的，所以难度质量啥的参差不齐，一些半成品残次品正在考虑要不要整理，都算上预计这个系列能更六七十篇吧

这次这个没怎么参与，属于帮同学的，舍友赵硕搞的，他现在转java了。所以只有当时给整理的文档，具体工程忘保存了，有需要的可以去他博客管他要哈哈

1. 机械臂的概述

1.1 研究背景、简介及意义

机械臂是传统的机械结构学结合现代电子技术、电机学、计算机科学、控制理论、信息科学和传感器技术等多学科综合性高新技术产物，它是一种拟生结构、高速运行、重复操作和高精度机电一体化的自动化设备。

机械臂是模仿人手的动作，按照给定程序、轨迹和要求能实现自动抓取、搬运的机械装置。在实际生产中，机械臂可以提高生产的机械臂可以提高自动化水平和实际生产效率，可以减轻劳动强度、保证产品质量、实现安全生产。尤其在高温、高压、低温、低压、粉尘、易爆、有毒气体及放射性等恶劣环境下，它能代替人正常工作，意义更为重大，随着生产的发展，功能和性能的不断改善和提高，在机械加工、冲压、锻、铸、焊接、热处理、电镀、喷漆、装配、轻工业及交通运输等领域得到越来越广泛的应用。

国内外对机器人及机械臂的定义不尽相同：国际标准化组织对机器人的定义是：机器人是一种能自动定位、可控的可编程的多功能操作机。这类操作机具有几个轴在可编程序操作下，能处理各种材料、零件、工具和专用装置，以执行各种任务。

近年来机械臂在工厂自动化改革中发挥着巨大的作用，代替人处理一些重复性，精密性、高工作负荷的工作，大大加快了生产效率，缩减了生产周期。如汽车自动化生产线中机械臂的无缝焊接，钢厂里的钢材分拣机器人的搬运打捆，都用到了机械臂。

1.2机械臂制作主要工作

本制作和以往的机械臂有所不同，以往的机械臂是上位机控制，而此次采用的是体感控制。主要任务是使机械臂能够模仿人手的动作。整个系统以单片机STM32F103C8T6控制，NRF24L01无线射频模块和MPU6050模块配置相应的外设及接口电路，用C语言开发，组成一个自由度为4的可控制机械臂。本设计的主要工作如下：

1. 根据课题提出的指标和要求进行研究，分析其可行性。
2. 对系统的各个功能模块进行了深入的分析和研究，在对课题所采用的方
   案进行详细的研究后设计具体的功能电路。
   3.熟悉所选择的各集成电路芯片并完成具体电路的设计、PCB板布局与模块结构设计，对各模块电路进行调试。
   4.使系统能过够产生0Hz～60MHz的正弦波并且可用键盘进行频率调节，最小调节步进间距为1Hz。
   5.输出信号频率误差：优于10-4；输出幅度误差：优于10-1；输出波形用于显示系统能过够产生0Hz～60MHz的正弦波并且可用键盘进行频率调节，最小调节步进间距为1Hz。
   6.输出信号频率误差：优于10-4；输出幅度误差：优于10-1；输出波形用于显示示波器观察室无明显失真。

设计要求：
机械臂的主要工作是模仿人的手臂动作，完成物品的抓取和易位，整个机械臂安装在一个基座上，回转角度范围是0到180度，小臂相对于大臂摆动，摆动范围是70-150度，手腕的末端安装一个机械手，机械手具有开闭能力，用于对直径30-50mm工件的抓取。最大承重为50g。
系统共有四个自由度，分别是加紧、大臂俯仰、小臂俯仰、基座的回转。基座的回转可进行0到180度的回转。最后的是摇摆与手指部分，通过控制手指的张开和夹紧来进行物体的抓取和易位。

2.方案设计

四自由度机械手系统有机械本体结构，伺服驱动系统，单片机控制系统，无线通信部分组成。机械手本体机械结构的动作主要是关节驱动，机械手的关节驱动单元是各种伺服电机。各关节的伺服驱动指令值主要由单片机控制。对于无线通信模块，发送端单片机将加速度仪数据读取出来，通过无线射频模块将数据发送到接收端。接收端无线射频模块接收来自无线模块的信号，将其转变为相应的脉冲宽度，并发送给舵机，舵机进行角度转换，并控制机械臂执行动作。机械手运动机构的升降、俯仰等独立的运动方式，称为机械手的自由度。自由度是机械手电子设计的关键因素。自由度越多，机械手的灵活性越大，通用性越广，其结构也越复杂。本次设计采用的是四个自由度，末端执行机构可以自由开合能完成物体的抓、移、放等动作。

3.系统的硬件设计

3.1 STM32F103C8T6单片机控制电路

STM32系列基于专为要求高性能、低成本、低功耗的嵌入式应用专门设计的ARM Cortex-M3内核，属于中低端的32位ARM微控制器。工作频率为72MHz，内置高速存储器(高达128K字节的闪存和20K字节的SRAM)，丰富的增强I/O端口和联接到两条APB总线的外设。所有型号的器件都包含2个12位的ADC、3个通用16位定时器和一个PWM定时器，还包含标准和先进的通信接口：多达2个I2C和SPI、3个USART、一个USB和一个CAN。
内核：ARM32位Cortex-M3 CPU，最高工作频率72MHz，1.25DMIPS/MHz。单周期乘法和硬件除法。
存储器：片上集成32-512KB的Flash存储器。6-64KB的SRAM存储器。
时钟、复位和电源管理：2.0-3.6V的电源供电和I/O接口的驱动电压。上电复位（POR）、掉电复位（PDR）和可编程的电压探测器（PVD）。4-16MHz的晶振。内嵌出厂前调校的8MHz RC振荡电路。内部40 kHz的RC振荡电路。用于CPU时钟的PLL。带校准用于RTC的32kHz的晶振。
低功耗：3种低功耗模式：休眠，停止，待机模式。为RTC和备份寄存器供电的VBAT。
调试模式：串行调试（SWD）和JTAG接口。
DMA：12通道DMA控制器。支持的外设：定时器，ADC，DAC，SPI，IIC和UART。
3个12位的us级的A/D转换器（16通道）：A/D测量范围：0-3.6V。双采样和保持能力。片上集成一个温度传感器。
最多高达112个的快速I/O端口：根据型号的不同，有26，37，51，80，和112的I/O端口，所有的端口都可以映射到16个外部中断向量。除了模拟输入，所有的都可以接受5V以内的输入。
最多多达11个定时器：4个16位定时器，每个定时器有4个IC/OC/PWM或者脉冲计数器。2个16位的6通道高级控制定时器：最多6个通道可用于PWM输出。2个看门狗定时器（独立看门狗和窗口看门狗）。Systick定时器：24位倒计数器。2个16位基本定时器用于驱动DAC。
最多多达13个通信接口：2个IIC接口（SMBus/PMBus）。5个USART接口（ISO7816接口，LIN，IrDA兼容，调试控制）。3个SPI接口（18 Mbit/s），两个和IIS复用。CAN接口（2.0B）。USB 2.0全速接口。SDIO接口。

3.1.1 STM32F103的开发环境- Keil MDK

MDK 源自德国的 KEIL 公司，是 RealView MDK 的简称。在全球 MDK 被超过 10 万的嵌入式开发工程师使用。目前最新版本为：MDK5.14，该版本使用 uVision5 IDE 集成开发环境，是目前针对 ARM 处理器，尤其是 Cortex M 内核处理器的最佳开发工具。Keil MDK提供了包括C编译器、宏汇编、连接器、库管理和一个功能强大的仿真调试器在内的完整开发方案，通过一个集成开发环境将这些功能组合在一起。它的界面和常用的微软VC++的界面相似，界面友好，易学易用，在调试程序，软件仿真方面也有很强大的功能。因此此设计采用这个编程环境。

3.1.2时钟电路

XTAL1是片内振荡器的反相放大器输入端，XTAL2则是输出端，使用外部振荡器时，外部振荡信号应直接加到XTAL1，而XTAL2悬空。内部方式时，时钟发生器对振荡脉冲二分频，如晶振8MHz，时钟频率就为72MHz。电容取22PF左右。STM32中有一个用于构成内部振荡器的高增益反相放大器，引脚XTAL1和XTAL2分别是该放大器的输入端和输出端。这个放大器与作为反馈元件的片外石英晶体或者陶瓷谐振器一起构成自激振荡器。片外石英晶体或者陶瓷谐振器及电容C1、C2接在放大器的反馈回路中构成并联振荡电路。对外接电容C1、C2虽然没有十分严格的要求，但电容容量的大小会轻微影响振荡频率的高低、振荡器工作的稳定性、起振的难易程序及温度稳定性，这里采用电容22pF，晶振采用8MHz，如图3-1。

图3-1

3.1.3复位电路

STM32采用上电复位，其电路如图2所示，只要在RST复位输入引脚上接一电容至Vcc端，下接一个电阻到地即可。对于CMOS型单片机，由于在RST端内部有一个下拉电阻，故可将外部电阻去掉，而将外接电容减至1µF。上电复位的工作过程是在加电时，复位电路通过电 容加给RST端一个短暂的高电平信号，此高电平信号随着Vcc对电容的充电过程而逐渐回落，即RST端的高电平持续时间取决于电容的充电时间。为了保证系统能够可靠地复位，RST端的高电平信号必须维持足够长的时间。上电时，Vcc的上升时间约为10ms，而振荡器的起振时间取决于振荡频率。在图2的复位电路中，当Vcc掉电时，必然会使RST端电压迅速下降到0V以下，但是，由于内部电路的限制作用，这个负电压将不会对器件产生损害。另外，在复位期间，端口引脚处于随机状态，复位后，系统将端口置为全“l”态。如果系统在上电时得不到有效的复位，则程序计数器PC将得不到一个合适的初值，因此，CPU可能会从一个未被定义的位置开始执行程序。

图3-2

3.2无线模块的电路设计及与单片机之间的通信

3.2.1无线模块的简介

NRF24L01 是一款工作在 2.4~2.5GHz 世界通用 ISM 频段的单片无线收发器芯片。无线收发器包括:频率发生器、增强型 SchockBurst TM 模式控制器、功率放大器、晶体振荡器、调制器、解调器。输出功率、频道选择和协议的设置可以通过 SPI 接口进行设置。
极低的电流消耗：当工作在发射模式下发射功率为-6dBm 时电流消耗为 9mA，接收模式时为 12.3mA。掉电模式和待机模式下电流消耗更低。本设计用无线模块实现发送端与控制系统之间的数据传递。

表3-1NRF24L01引脚功能

表3-2 电气特性参数

3.2.2无线模块与单片机的电气连接

图3-3无线模块与单片机连接图

3.2.3无线模块与单片机之间的通信-SPI

SPI 接口一般使用 4 条线通信： MISO 主设备数据输入，从设备数据输出。MOSI 主设备数据输出，从设备数据输入。SCLK 时钟信号，由主设备产生。CS 从设备片选信号，由主设备控制。其工作原理是：主机和从机都有一个串行移位寄存器，主机通过向它的 SPI 串行寄存器，写入一个字节来发起一次传输。寄存器通过 MOSI 信号线将字节传送给从机，从机也将自己的移位寄存器中的内容通过 MISO 信号线返回给主机。这样，两个移位寄存器中的内容就被交换。外设的写操作和读操作是同步完成的。如果只进行写操作，主机只需忽略接收到的字节；反之，若主机要读取从机的一个字节，就必须发送一个空字节来引发从机的传输。
机械臂的SPI时序图如下：

图3-4NRF24L01读写操作时序

上图中 Cn 代表指令位，Sn 代表状态寄存器位，Dn 代表数据位。从图中可以看出，SCK 空闲的时候是低电平的，而数据在 SCK 的上升沿被读写。

3.3 加速度仪的设计电路及与单片机之间的通信

3.3.1 加速度ADXL345

ADXL345是一款小而薄的超低功耗3轴加速度计，分辨率高(13位)，测量范围达± 16g。数字输出数据为16位二进制补码格式，可通过SPI(3线或4线)或I 2 C数字接口访问。ADXL345非常适合移动设备应用。它可以在倾斜检测应用中测量静态重力加速度，还可以测量运动或冲击导致的动态加速度。其高分辨率(3.9mg/LSB)，能够测量不到1.0°的倾斜角度变化。加速度仪主要是提取它的加速度，将加速度融合成角度，这个是将加速度融合成角度的函数。此转换由单片机实现。

****************************************************************************
* Function Name  : MPU_GetAngle
* Description    : 将AD值转换成角度值
* Input          : xValue：x的值
*                * yValue：y的值
*                * zValue：z的值
*                * dir：0：与Z轴的角度;1：与X轴的角度;2：与Y轴的角度.
* Output         : None
* Return         : None
****************************************************************************/

int16_t MPU_GetAngle(short aacx, short aacy, short aacz, uint8_t dir)
{
	float temp;
 	float res = 0;

	switch(dir)
	{   
        /* 与自然Z轴的角度 */
		case 0:
 			temp = sqrt((aacx * aacx + aacy * aacy)) / aacz;
 			res = atan(temp);
 			break;
        
        /* 与自然X轴的角度 */
		case 1:
 			temp = aacx / sqrt((aacy * aacy + aacz * aacz));
 			res = atan(temp);
 			break;

        /* 与自然Y轴的角度 */
 		case 2:
 			temp = aacy / sqrt((aacx * aacx + aacz * aacz));
 			res = atan(temp);
 			break;

        default:
            break;
 	}

    res = res * 1800 / 3.14; 

	return res;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

3.3.2 加速度仪与单片机之间的电气连接

图3-5

3.3.3 加速度仪与单片机之间的通信-I2C

I2C(Inter－Integrated Circuit)总线是一种由 PHILIPS 公司开发的两线式串行总线，用于连接微控制器及其外围设备。它是由数据线 SDA 和时钟 SCL 构成的串行总线，可发送和接收数据。在 CPU 与被控 IC 之间、IC 与 IC 之间进行双向传送，高速 IIC 总线一般可达 400kbps 以上。
I2C 总线在传送数据过程中共有三种类型信号， 它们分别是：开始信号、结束信号和应答信号。开始信号：SCL 为高电平时，SDA 由高电平向低电平跳变，开始传送数据。结束信号：SCL 为高电平时，SDA 由低电平向高电平跳变，结束传送数据。应答信号：接收数据的 IC 在接收到 8bit 数据后，向发送数据的 IC 发出特定的低电平脉冲，表示已收到数据。CPU 向受控单元发出一个信号后，等待受控单元发出一个应答信号，CPU 接收到应答信号后，根据实际情况作出是否继续传递信号的判断。若未收到应答信号，由判断为受控单元出现故障。这些信号中，起始信号是必需的，结束信号和应答信号，都可以不要。本实验的时序图如下：

图3-6

3.4 舵机模块及PWM控制

3.4.1 舵机模块及相应程序

舵机是一种位置（角度）伺服的驱动器，适用于那些需要角度不断变化并可以保持的控制系统。目前在航模，包括飞机模型，潜艇模型，遥控机器人中已经使用的比较普遍。舵机是一种俗称，其实，是一种伺服马达。
一般来讲，舵机主要由以下几个部分组成：舵盘、减速齿轮组、位置反馈电位计、直流电机、控制电路板灯等。
工作原理：控制电路板接受来自信号线的控制信号，控制电机转动，电机带动一系列齿轮组，减速后传动至输出舵盘。舵机的输出轴和位置反馈计是相连的，舵盘转动的同时，带动位置反馈计，电位计将输出一个电压信号到控制电路板，进行反馈，然后控制电路板根据所在位置决定电机转速和方向，从而达到目标停止。
标准的舵机有三条导线,分别是：电源线、地线、控制线。电源线和地线是舵机的能源保证，主要是电机的转动消耗。
舵机的控制信号为周期是20ms的脉冲调制(PWM)信号，其中脉冲宽度从0.5ms-2.5ms,相应的多盘的位置为0-180度，呈线性变化。也就是说，给他提供一个脉宽，它的输出轴就会保持在一个相应的角度上，无论外界转矩怎么变化，直到给他提供一个另外宽度的脉冲信号，它才会改变输出角度到新对应的位置上，程序实现上可以通过定时器来实现。
舵机的转速取决于信号脉宽的变化速度，如果信号脉冲变化速度太快的话，舵机会反应不过来：将脉宽变化值线性到要求时间内，一点一点增加脉宽值，就可以控制舵机的速度了。具体来说就需要在调试的时候修改数值，以使舵机的运动更加平滑。由于舵机在每一次脉宽值改变的时候总会有一个转速由零增加再减速为零的过程，所以舵机会产生像步进电机一样运动的原因。
舵机的角度算法，为了防止大臂和小臂的抱死，
//控制部分

if(servo_ang.big>= 0 && servo_ang.big<=30)
	{
		servo_ang.small_min = 175;
		servo_ang.small_max = 180;
		if(servo_ang.small <= servo_ang.small_min)
		servo_ang.small = servo_ang.small_min;
		else if(servo_ang.small >= servo_ang.small_max)
		servo_ang.small = servo_ang.small_max;
	}
	else
	{	//min = 180 - angle + 5
		//max = 180 - angle + 35
		servo_ang.small_min = 185 - servo_ang.big;
		servo_ang.small_max = 215 - servo_ang.big;
		if(servo_ang.small <= servo_ang.small_min)
		servo_ang.small = servo_ang.small_min;
		else if(servo_ang.small >= servo_ang.small_max)
		servo_ang.small = servo_ang.small_max;
	}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

3.4.2 pwm的简介

PWM就是脉宽调制器，通过调制器给电机提供一个具有一定频率的脉冲宽度可调的脉冲电。脉冲宽度越大即占空比越大，提供给电机的平均电压越大，电机转速就高。反之脉冲宽度越小，则占空比越越小。提供给电机的平均电压越小，电机转速就低。PWM不管是高电平还是低电平时电机都是转动的，电机的转速取决于平均电压。

3.4.3 pwm与舵机的角度转换程序

void Arm::servo_control(uint16_t angle)
{
	uint16_t duty;
	duty = 50 + 1.1 * angle;
	if(duty <= 50) duty = 50;//0
	if(duty >= 250) duty = 250;//180
	if(get_port() == GPIOA)
	{
		switch(get_pin())
		{
			case GPIO_Pin_6://turntable
				set_duty(duty);
			break;
			case GPIO_Pin_7://big
				set_duty(duty);
			break;
		}
	}
	else if(get_port() == GPIOB)
	{
		switch(get_pin())
		{
			case GPIO_Pin_6://arm_small
				set_duty(duty);
			break;
			case GPIO_Pin_7://arm_wrist
				set_duty(duty);
			break;
			case GPIO_Pin_8://arm_claw
				set_duty(duty);
			break;
		}
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

4. 电路板制作

首先使用Altium Designer绘制原理图，生成PCB板并手工调制布局，之后手工布线。布线时要设置电气规则，设置安全距离，对电源线、地线加粗:同时要尽量加大焊盘:尽可能避免跳线。

图4-1发送端原理图

图4-2发送端PCB图

图4-3 接收端原理图

图4-4接收端PCB图

5. 程序下载及调试

5.1 程序流程图

           图5-1机械臂原理图
1

图5-2机械臂原理图

5.2 相应程序

发送端大臂：主要程序

#include "func.h"
uint8_t tx_Buf1[32] = { 'B', 'B' };
#define Filter_Num 100
Key key_claw(&PA5, IPU);
_filter 		filter_data;//滤波之后的数据
_filter 		raw_data;//原始数据
_servo_ang	servo_ang;
int16_t abs(int16_t p)
{
    return p>0?p:-p;
}
void setup()
{
	yg_init();

	MPU_Init();//初始化MPU6050
	while(mpu_dmp_init())
 	{
		delay_ms(10);
	}
	NRF24L01_Init();
	while(NRF24L01_Check())//检测NRF24L01是否存在
	{
		delay_ms(10);
	}
	NRF24L01_TX_Mode();
	//WWDG_Init(0X7F, 0X5F, WWDG_Prescaler_8);
}
void task()
{
	MPU_Get_Accelerometer(&raw_data.aacx, &raw_data.aacy, &raw_data.aacz);/
	servo_ang.y = abs(MPU_GetAngle(filter_data.aacx, filter_data.aacy, filter_data.aacz, 2) / 10 - 90);
	tx_Buf1[2] = (servo_ang.y >> 8) & 0xff;//arm_big
	tx_Buf1[3] = servo_ang.y & 0xff;
	NRF24L01_TxPacket(tx_Buf1);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

发送端手上：主要程序：

#include "func.h"
uint8_t tx_Buf0[32] = { 'A', 'A' };
#define Filter_Num 100
Key key_claw(&PA5, IPU);
_filter 		filter_data;_filter 		raw_data;
_servo_ang	servo_ang;
int16_t abs(int16_t p)
{
    return p>0?p:-p;
}
void setup()
{
	yg_init();

	MPU_Init();//³õʼ»¯MPU6050
	while(mpu_dmp_init())
 	{
		delay_ms(10);
	}
	NRF24L01_Init();
	while(NRF24L01_Check())
	{
		delay_ms(10);
	}
	NRF24L01_TX_Mode();
}

void task()
{
	MPU_Get_Accelerometer(&raw_data.aacx, &raw_data.aacy, &raw_data.aacz);//get aac
	Slide_Filter(&raw_data, &filter_data);//slide filter
	servo_ang.x = abs(MPU_GetAngle(filter_data.aacx, filter_data.aacy, filter_data.aacz, 1) / 10 - 90);
	servo_ang.y = abs(MPU_GetAngle(filter_data.aacx, filter_data.aacy, filter_data.aacz, 2) / 10 + 90);
	servo_ang.z = MPU_GetAngle(filter_data.aacx, filter_data.aacy, filter_data.aacz, 0) / 10 + 90;
	
	if(key_claw.key_press() == PRESS)
	{
		delay_ms(10);
		servo_ang.claw = 35;
	}
	else
	{
		delay_ms(10);
		servo_ang.claw = 80;
	}
	
	tx_Buf0[2] = (servo_ang.x >> 8) & 0xff;//arm_turntable
	tx_Buf0[3] = servo_ang.x & 0xff;
	tx_Buf0[4] = (servo_ang.y >> 8) & 0xff;//arm_small
	tx_Buf0[5] = servo_ang.y & 0xff;
	tx_Buf0[6] = (servo_ang.z >> 8) & 0xff;//no use
	tx_Buf0[7] = servo_ang.z & 0xff;
	tx_Buf0[8] = (servo_ang.claw >> 8) & 0xff;//arm_claw
	tx_Buf0[9] = servo_ang.claw & 0xff;
	NRF24L01_TxPacket(tx_Buf0);
}


接收控制端主要程序：
#include "func.h"

extern uint8_t rx_Buf0[32];
extern uint8_t rx_Buf1[32];
extern uint8_t rx_Buf2[32];
extern uint8_t rx_Buf3[32];
extern uint8_t rx_Buf4[32];
extern uint8_t rx_Buf5[32];

#define Filter_Num 100
Timer timer3(TIM3);
Key key_claw(&PE2, IPU);
_filter 		filter_data;
_filter 		raw_data;
_servo_ang	servo_ang;
static int abs_1(int p)
{
    return p > 0 ? p : -p;
}
void Slide_Filter(_filter *filter_in, _filter *filter_out)
{
	static int16_t Filter_ax[Filter_Num],Filter_ay[Filter_Num],Filter_az[Filter_Num],Filter_gx[Filter_Num],Filter_gy[Filter_Num],Filter_gz[Filter_Num];
	static uint8_t Filter_count;
	int32_t Filter_sum_ax=0,Filter_sum_ay=0,Filter_sum_az=0,Filter_sum_gx=0,Filter_sum_gy=0,Filter_sum_gz=0;
	uint8_t i=0;
	
	Filter_ax[Filter_count] = filter_in->aacx;
	Filter_ay[Filter_count] = filter_in->aacy;
	Filter_az[Filter_count] = filter_in->aacz;
	Filter_gx[Filter_count] = filter_in->gyrox;
	Filter_gy[Filter_count] = filter_in->gyroy;
	Filter_gz[Filter_count] = filter_in->gyroz;
	
	for(i=0;i<Filter_Num;i++)
	{
		Filter_sum_ax += Filter_ax[i];
		Filter_sum_ay += Filter_ay[i];
		Filter_sum_az += Filter_az[i];
		Filter_sum_gx += Filter_gx[i];
		Filter_sum_gy += Filter_gy[i];
		Filter_sum_gz += Filter_gz[i];
	}	
	
	filter_out->aacx = Filter_sum_ax / Filter_Num;
	filter_out->aacy = Filter_sum_ay / Filter_Num;
	filter_out->aacz = Filter_sum_az / Filter_Num;
	filter_out->gyrox = Filter_sum_gx / Filter_Num;
	filter_out->gyroy = Filter_sum_gy / Filter_Num;
	filter_out->gyroz = Filter_sum_gz / Filter_Num;
	
	Filter_count++;
	if(Filter_count == Filter_Num)
		Filter_count=0;
}

void setup()
{
	yg_init();
	arm_init(2000, 720);
	NRF24L01_Init();
	while(NRF24L01_Check())
	{
		delay_ms(10);
	}
	NRF24L01_RX_Mode();
}

void task()
{
	if(NRF24L01_RxPackets() == 0)
	{
		if(rx_Buf0[0] == 'A' && rx_Buf0[1] == 'A')//С±Û
		{
			servo_ang.turntable = rx_Buf0[2] << 8 | rx_Buf0[3];//x
			servo_ang.small = rx_Buf0[4] << 8 | rx_Buf0[5];//y arm_small
			//		servo_ang.z = rx_buf[4] << 8 | rx_buf[5];
			servo_ang.claw = rx_Buf0[8] << 8 | rx_Buf0[9];
		}
		if(rx_Buf1[0] == 'B' && rx_Buf1[1] == 'B')//´ó±Û
		{
			servo_ang.big = rx_Buf1[2] << 8 | rx_Buf1[3];//y  arm_big
		}
	}
	if(servo_ang.big <= 104)
	{
		servo_ang.big = 104;
		servo_ang.small = 155;
	}
	else if(servo_ang.big >= 170)
	{
		servo_ang.small = 155;
	}
	else if(servo_ang.big >= 104 && servo_ang.big <= 170)
	{
		servo_ang.small_max = 155;
		servo_ang.small_min = 240 - servo_ang.big;
		if(servo_ang.small <= servo_ang.small_min)
		servo_ang.small = servo_ang.small_min;
		else if(servo_ang.small >= servo_ang.small_max)
		servo_ang.small = servo_ang.small_max;
	}
	arm_big.servo_control(servo_ang.big);
	arm_small.servo_control(servo_ang.small);
	arm_turntable.servo_control(servo_ang.turntable);
	arm_claw.servo_control(servo_ang.claw);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165

6. 电子设计体会

机械臂的动作都比较简单，这是由于它们都是在固定的地点完成特定的重复的任务，它们的技术指标主要是力矩、可靠性、可持续工作时间（主控系统及驱动电路可以正常、稳定地工作的时间）等，而我们此次研制机械臂重点是它的灵活性和控制精度，学习它的工作原理及控制方法，所以对机械臂的功能设计的比较简单，只是重复性的干一个工作，但其扩展性很强，只要根据具体需要，对软件稍加改动，就可以使机械臂完成自己预期的工作。

我们此次只完成了整个设计的初期工作，搭建了一个机械臂的工作平台，可以实现一些工业设备的功能，完成一些简单的抓取任务，动作比较灵活，下一步还有许多工作要做：，比如测量角度时需要还需要将加速度仪和陀螺仪的姿态融合，使得角度更加的稳定，控制舵机的时候更加的稳定平滑。例如在钳子上加压力传感器，用以检测是否已抓紧物体（这个功能也可以通过检测电流的方法实现，但电路会比较复杂），如果再配上位置检测装置，不但可以抓取木块之类的东西，还可以抓取像鸡蛋等易碎物体，并且可以保证不会夹破等。经过这个项目的研究，我们不仅对机械臂的基本结构和控制原理有了初步的了解，而且掌握了许多相关专业的知识，增强了创新能力，培养了团队协作精神。

参考文献
[1]机器人制作宝典．清弘智昭．科学出版社 2000
[2]机器人基础知识．刘兴良．新时代出版社2003.7
[3]康华光.电子技术基础-模拟部分.高等教育出版社,1999.4
[4]朱定华 单片微机原理及应用[M] 北京交通大学出版社,2008
[5]孙克梅. 直流伺服电机的单片机控制及应用[J]. 沈阳航空工业学院学报2003(2)

点击下方”大饼匠人“卡片，关注并回复"27"免费下载所有源文件开发资料