前言

本项目实现了一套完整的五自由度舵机机械臂控制系统。系统采用分布式架构设计：

- 下位机：STM32F103单片机负责机械臂运动控制

- 上位机：Ubuntu系统运行视觉opencv

- 通讯协议：USART1串口，波特率9600，自定义数据包格式

原理非常简单，手把手教学，抓取误差极低

目录

一、定时器配置

二、舵机

三、机械臂控制

3.1逆运动学解算

代码

二维坐标定位

三维坐标定位

3.2步态平滑

代码

原理

3.3机械臂抓取

代码

三、串口通信

说明:

串口初始化

定义存储数组

串口收发数据

代码如下

数据包的收发

串口中断接收函数

main函数

定义

main函数

视觉

一、定时器配置

由于我们有4个自由度和一个机械爪，所以要配置5路pwm，我们先配置5个gpio引脚，模式选择复用推挽输出

/******************************************************************************* * 功 能 ：初始化定时器输出比较通道，输出PWM（20ms) * 定 时 器 ：TIM3 TIM4 * 使用引脚：GPIOB的 P0 P6 P7 P8 P9 * *******************************************************************************/ #include "PWM.h" #define PWM_Timx TIM4 #define PWM_Timy TIM3 #define PWM_TimxGpio GPIOB #define PWM_TimyGpio GPIOB void PWM_Init(void) { RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4 | RCC_APB1Periph_TIM3,ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB ,ENABLE); /*选择时钟*/ TIM_InternalClockConfig(PWM_Timx); //设置内部时钟为时钟源 TIM_InternalClockConfig(PWM_Timy); //设置内部时钟为时钟源 /*GPIO配置*/ GPIO_InitTypeDef GPIO_InitStruct; //配置GPIO初始结构体值 GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(PWM_TimxGpio, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(PWM_TimyGpio, &GPIO_InitStruct);

舵机周期要20ms，所以我们配置psc为72-1，arr配置为20000-1，

72Mhz ÷（72×20000）= 50hz； 1/50hz = 0.02ms

/*时基单元配置*/ TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct; //配置时钟初始结构体值 TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInitStruct.TIM_Period = 20000 - 1; //ARR TIM_TimeBaseInitStruct.TIM_Prescaler = 72 - 1; //PSC TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0; TIM_TimeBaseInit(PWM_Timx, &TIM_TimeBaseInitStruct); TIM_TimeBaseInit(PWM_Timy, &TIM_TimeBaseInitStruct); /*输出比较配置*/ TIM_OCInitTypeDef TIM_OCInitStruct; //配置输出捕获初始结构体值 TIM_OCStructInit(&TIM_OCInitStruct); TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;//高电平有效，极性不反转 TIM_OCInitStruct.TIM_Pulse = 0; TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable; TIM_OC1Init(PWM_Timx, &TIM_OCInitStruct); TIM_OC2Init(PWM_Timx, &TIM_OCInitStruct); TIM_OC3Init(PWM_Timx, &TIM_OCInitStruct); TIM_OC4Init(PWM_Timx, &TIM_OCInitStruct); TIM_OC3Init(PWM_Timy, &TIM_OCInitStruct);//TIM3 /*使能预装载寄存器*/ TIM_OC1PreloadConfig(PWM_Timx, TIM_OCPreload_Enable); TIM_OC2PreloadConfig(PWM_Timx, TIM_OCPreload_Enable); TIM_OC3PreloadConfig(PWM_Timx, TIM_OCPreload_Enable); TIM_OC4PreloadConfig(PWM_Timx, TIM_OCPreload_Enable); TIM_OC3PreloadConfig(PWM_Timy, TIM_OCPreload_Enable); TIM_ARRPreloadConfig(PWM_Timx, ENABLE); TIM_ARRPreloadConfig(PWM_Timy, ENABLE); /*开启定时器*/ TIM_Cmd(PWM_Timx,ENABLE); TIM_Cmd(PWM_Timy,ENABLE); }

设置输出比较占空比函数

/*设置TIM4的4个通道*/ void PWM_SetCompare1(uint16_t Compare){TIM_SetCompare1(PWM_Timx, Compare);} void PWM_SetCompare2(uint16_t Compare){TIM_SetCompare2(PWM_Timx, Compare);} void PWM_SetCompare3(uint16_t Compare){TIM_SetCompare3(PWM_Timx, Compare);} void PWM_SetCompare4(uint16_t Compare){TIM_SetCompare4(PWM_Timx, Compare);} /*实际设置的TIM3的通道3*/ void PWM_SetCompare5(uint16_t Compare){TIM_SetCompare3(PWM_Timy, Compare);}

二、舵机

定义一个结构体，负责存储舵机角度

typedef struct { float Servo_1; float Servo_2; float Servo_3; float Servo_4; float Servo_5; }Servo_InitTypedef;

为每个舵机分别写对应的设置角度函数（下面代码只了写一个舵机）。

#include "Servo.h" /** *@brief 舵机初始化，需要配置结构体 Servo_InitTypedef 来给各个舵机指定角度 *@param Servo_struct 指向 Servo_InitTypedef 结构体 *@retval 无 */ void Servo_Init(Servo_InitTypedef* Servo_struct) { PWM_Init(); Servo_SetAngle(Servo_struct); } /** *@brief 设置舵机1角度 *@param angle角度 *@retval 无 */ void Servo_SetServo1(float angle) { PWM_SetCompare1((u16)(angle/ 180 * 2000 + 500)); }

用结构体统一设置每个舵机的角度，内部定义了angle_1等，用来判断角度是否发生改变，如果未发生改变，则不做处理

/** *@brief 设置机械臂舵机角度 *@param Servo_struct ：Servo_InitTypedef结构体的地址 *@retval 无 */ void Servo_SetAngle(Servo_InitTypedef* Servo_struct) { static float angle_1, angle_2, angle_3, angle_4, angle_5; /*检测舵机角度是否发生变化，如果没变则不处理*/ if(Servo_struct->Servo_1 != angle_1) { Servo_SetServo1(Servo_struct->Servo_1); angle_1 = Servo_struct->Servo_1; } if(Servo_struct->Servo_2 != angle_2) { Servo_SetServo2(Servo_struct->Servo_2); angle_2 = Servo_struct->Servo_2; } if(Servo_struct->Servo_3 != angle_3) { Servo_SetServo3(Servo_struct->Servo_3); angle_3 = Servo_struct->Servo_3; } if(Servo_struct->Servo_4 != angle_4) { Servo_SetServo4(Servo_struct->Servo_4); angle_4 = Servo_struct->Servo_4; } if(Servo_struct->Servo_5 != angle_5) { Servo_SetServo5(Servo_struct->Servo_5); angle_5 = Servo_struct->Servo_5; } }

三、机械臂控制

我们写一个结构体定义RobArm_Typedef，负责存储每个舵机的数据；再定义一个结构体数组，分别代表五个舵机，加extern关键字，方便外部调用

typedef struct { u8 RobArm_NO; //舵机编号 float RobArm_Angle; //角度 }RobArm_Typedef; extern RobArm_Typedef RobArm[5];

我们要改机械臂各个舵机角度的时候只需要改RobArm[]这个数组就可以了，例如

/** *@brief 设置机械臂舵机角度（所有舵机） *@param RobArm 结构体数组首地址 *@param Angle_Arr 角度数组首地址(数组方便后期角度读取) *@retval 无 */ void RobArm_SetAllArm(RobArm_Typedef *RobArm, float *Angle_Arr) { if(RobArm == NULL || Angle_Arr == NULL)//检测空指针 return; u8 i; for(i = 0; i < 5; i ++) { RobArm[i].RobArm_Angle = (Angle_Arr[i] < 0) ? 0 : (Angle_Arr[i] > 180) ? 180 : Angle_Arr[i]; //将舵机角度依次输入 } Servo_InitTypedef Servo_struct; //设置舵机角度 Servo_struct.Servo_1 = RobArm[0].RobArm_Angle; Servo_struct.Servo_2 = RobArm[1].RobArm_Angle; Servo_struct.Servo_3 = RobArm[2].RobArm_Angle; Servo_struct.Servo_4 = RobArm[3].RobArm_Angle; Servo_struct.Servo_5 = RobArm[4].RobArm_Angle; Servo_SetAngle(&Servo_struct); }

3.1逆运动学解算

代码

/** *@brief 机械臂逆运动学姿态解算 *@param x，y：端执行器位置 *@param *Angle_struct 角度接收结构体指针 RobArm_AngleTypedef *@retval */ u8 RobArm_KinematicAnalysis(float x, float y, float z, float Rob_Arm_EndEffector_angle, RobArm_AngleTypedef *Angle_struct) { /*空指针保护*/ if(Angle_struct == NULL) return 255; /*超出范围保护*/ if(sqrtf(x*x + y*y + z*z) <= ROB_ARM_LENTH_3 || sqrtf(x*x + y*y + z*z) >= ROB_ARM_LENTH_1 + ROB_ARM_LENTH_2 + ROB_ARM_LENTH_3) return 255; /*机械臂长度赋值*/ static float L1 = ROB_ARM_LENTH_1; static float L2 = ROB_ARM_LENTH_2; static float L3 = ROB_ARM_LENTH_3; float x0 = sqrtf(x*x + y*y); float y0 = z; /*末端执行器角度*/ float Alpha_rad = Rob_Arm_EndEffector_angle * my_PI/180.0f; /*末端执行器到原点的角度*/ float x2 = x0 - L3 * cosf(Alpha_rad); float y2 = y0 - L3 * sinf(Alpha_rad); float Target_rad = atan2f(y2, x2); float rr = x2*x2 + y2*y2; /********************计算角0********************/ float Bata = atan2f(y, x) * 180.0f/my_PI; Angle_struct->Theta0 = Bata; /********************计算角1********************/ /*余弦定理*/ float Theta_cos = (rr + L1*L1 - L2*L2) / (2.0f * L1 * sqrtf(rr) ); Theta_cos = (Theta_cos > 1.0f) ? 1.0f : (Theta_cos < -1.0f) ? -1.0f : Theta_cos;//限定sin在[-1,1] float Theta_rad = acosf(Theta_cos); Angle_struct->Theta1 = (Target_rad + Theta_rad) * 180.0f/my_PI;//转化成角度 /********************计算角2********************/ /*余弦定理*/ float Theta2_cos = (L1*L1 + L2*L2 - rr) / (2.0f * L1 * L2); Theta2_cos = (Theta2_cos > 1.0f) ? 1.0f : (Theta2_cos < -1.0f) ? -1.0f : Theta2_cos;//限定sin2在[-1,1] float Theta2_rad = acosf(Theta2_cos); Angle_struct->Theta2 = Theta2_rad * 180.0f/my_PI;//转化成角度 /********************计算角3********************/ float Phi = 180.0f - (Angle_struct->Theta1 + Angle_struct->Theta2); Angle_struct->Theta3 = Rob_Arm_EndEffector_angle + Phi;//计算角3 /********************角度偏移********************/ Angle_struct->Theta3 += 90.0f; //末端执行器偏移 Angle_struct->Theta1 += -3.0f; Angle_struct->Theta2 += 9.0f; Angle_struct->Theta3 += 0.0f; return 0; }

二维坐标定位

大臂小臂和末端执行器分别是L1，L2，L3，然后要抓取的点是P，末端执行器的角度是我们自己给的Rob_Arm_EndEffector_angle。

我们首先根据三角函数计算出N点的坐标，就知道了Xn、Yn和角度，同时能计算出r = x^2 + y^2，L1和L2又是我们已经知道的，就可以求出三角形的内角1，2，3。第一个关节的角度就是

1+，第二个关节角度就是2；

接下来我们要计算第三个关节的角度，根据平行，能知道=+5，就能求出∠5，∠3和∠4互为余角，∠4 = π/2 - ∠3，第三个关节就是∠4 + ∠5；

那么所有的关节角度都确定了，就可以确定机械臂的姿态了。

由此写出来的函数就只需要知道二维坐标(x,y)和末端执行器的角度（自己设定的）就可以实现坐标定位了

三维坐标定位

有了二维坐标的定位之后，我们看三维中的目标点P，会发现三维中的R就是二维中的x，z就是二维坐标中的y。三维坐标给我们的目标点P坐标已知，所以我们只需要知道R就可以

三维坐标中R = x^2 + y^2，然后底座角度= arctan(y, x)

所以原来写的二维坐标函数内部把x换成R，把y换成z就行

这样我们所有的轴的角度就知道了，就可以实现三维坐标的定点抓取了

3.2步态平滑

代码

/** *@brief ease-in-out平滑步长（首尾速度趋近于0） *@param *Target_Angle_arr 目标角度数组 *@param *Now_Angle_arr 当前目标数组 *@retval */ ErrorStatus RobArm_ActionGrap_Move(float *Target_Angle_arr, float *Now_Angle_arr, u16 Steps) { /*空指针保护*/ if(Target_Angle_arr == NULL || Now_Angle_arr == NULL) return ERROR; float Start_Angle[5];//存储开始角度 float Delta[5];//存储角度差 /*记录开始位置*/ for(u8 i = 0; i < 5; i++) { Start_Angle[i] = Now_Angle_arr[i];//记录开始角度 Delta[i] = Target_Angle_arr[i] - Now_Angle_arr[i];//记录角度差 } /*平滑步长*/ float Angle[5]; for(u16 i = 1; i <= Steps; i++) { float t = (float)i / (float)Steps;//时间系数t=（0，1] float ratio = 0.5f - 0.5f * cosf(t * my_PI);//0.5 - 0.5*cos(π*t) /*将计算的角度存入数组*/ for(u8 j = 0; j < 5; j++) { Angle[j] = Start_Angle[j] + Delta[j] * ratio; } /*将计算出的当前步角度设置给机械臂*/ RobArm_SetAllArm(RobArm, Angle); } return SUCCESS; }

原理

我们的机械臂已经可以实现定位了，但是要给他加一个平滑步态，不然舵机会转动太快

这个用的是余弦缓动函数：ratio = 0.5 - 0.5*cos(π*t)，速度在中间是最快的，在刚开始和结束的时候会减速变慢

Start_Angle[5]负责存储刚开始的角度，Delta[5]存储5个关节需要转过的角度差；

3.第一个for循环是负责记录当前的位置，然后计算需要转动的角度

第二个for循环是负责根据缓动函数，依据你输入的Steps（越大，运动越平滑，同时会更慢），将数值计算出来存到Angle数组里面，然后最后RobArm_SetAllArm(RobArm, Angle);传给舵机角度

3.3机械臂抓取

代码

/** *@brief 定点抓放（左负右正） *@param x,y,z坐标位置 *@param Grap_Angle 爪子闭合角度（0~55)；0为爪子张开最大时，55为闭合时 *@param End_Effector_angle 末端执行器角度 *@retval */ u8 RobArm_ActionGrap(float x, float y, float z, float Grap_Angle, float End_Effector_angle) { /*超出范围保护*/ // if(sqrtf(x*x + y*y) <= ROB_ARM_LENTH_3 || sqrtf(x*x + y*y) >= ROB_ARM_LENTH_1 + ROB_ARM_LENTH_2 + ROB_ARM_LENTH_3) // return 255; float Target_Angle[5]; //存储目标角度 float Now_Angle[5]; //存储当前角度 RobArm_AngleTypedef Angle_struct; /**************** 逆运动学解算出机械臂角度 **************/ if(RobArm_KinematicAnalysis(x, y, z, End_Effector_angle, &Angle_struct)) return 254; /**************** 记录当前机械臂角度 *******************/ for(u8 i = 0; i < 5; i ++) { Now_Angle[i] = RobArm[i].RobArm_Angle; } /**************** 机械臂移动到目标位置 *****************/ /*先移动底座*/ for(u8 i = 0; i < 4; i ++) { Target_Angle[i + 1] = Now_Angle[i + 1]; } Target_Angle[0] = Angle_struct.Theta0; if(!RobArm_ActionGrap_Move(Target_Angle, Now_Angle, 1000)) return 252; delay_ms(300); Now_Angle[0] = Angle_struct.Theta0; //更新底座当前角度 /*再移动机械臂*/ /*记录目标机械臂角度*/ Target_Angle[0] = Angle_struct.Theta0; Target_Angle[1] = Angle_struct.Theta1; Target_Angle[2] = Angle_struct.Theta2; Target_Angle[3] = Angle_struct.Theta3; Target_Angle[4] = RobArm[4].RobArm_Angle;//爪子暂不变 if(!RobArm_ActionGrap_Move(Target_Angle, Now_Angle, ROB_ARM_STEP)) return 253; delay_ms(200);//抓取前停顿 /**************** 爪子执行到指定角度 ***************/ RobArm_SetArm(&RobArm[4], Grap_Angle); delay_ms(300); /**************** 回到默认位置(爪角度不变) *****************/ if(RobArm_GoDefault(RobArm)) return SUCCESS; else return 251; }

考虑到如果机械臂和底座同时运动，可能会发生一些碰撞，比如撞墙

所以我们决定每次抓的时候，底座先转到位，机械臂再开始抓取定位，抓到目标后，机械臂先回位，底座再回位，如果想抓到目标后放在另一个目标位置，要把下面这一段代码删掉(位置在最后面)

/**************** 回到默认位置(爪角度不变) *****************/ if(RobArm_GoDefault(RobArm)) return SUCCESS;

三、串口通信

说明:

由于我们的视觉是跑在ubuntu上，我们要用串口跟上位机连接，所以我自己定义了一个数据包

协议格式是：帧头(0xAA) + X(4byte float) + Y(4byte float) + Z(4byte float) + 校验和(1byte)

有需要也可以改成机械臂串口json 协议

串口初始化

我们设置串口的发送引脚TX为复用推挽输出，接收引脚RX为上拉输入

波特率为9600，无校验位，中断优先级根据需要设置

uint8_t frame_heade; /** *@brief 串口初始化 *@param 无 *@retval 无 */ void Serial_Init(uint32_t BaudRate) { RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Pin=GPIO_Pin_9; GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin=GPIO_Pin_10; //设置RXD引脚 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU; GPIO_Init(GPIOA, &GPIO_InitStructure); USART_InitTypeDef USART_InitStruct; USART_InitStruct.USART_BaudRate = BaudRate; //设置9600波特率 USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //流控关闭 USART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; //开启输出输入通道 USART_InitStruct.USART_Parity = USART_Parity_No; //关闭校验位 USART_InitStruct.USART_StopBits = USART_StopBits_1; //设置停止位为1 USART_InitStruct.USART_WordLength = USART_WordLength_8b; USART_Init(USART1,&USART_InitStruct); USART_ITConfig(USART1,USART_IT_RXNE,ENABLE); //开启串口中断 NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); NVIC_InitTypeDef NVIC_InitStruct; NVIC_InitStruct.NVIC_IRQChannel=USART1_IRQn; NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE; NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=1; NVIC_InitStruct.NVIC_IRQChannelSubPriority=1; NVIC_Init(&NVIC_InitStruct); USART_Cmd(USART1,ENABLE); //开启串口开关 }

定义存储数组

我们定义一个接受数组一个存储数组，类型是uint8_t，定义容量14，符合数据包格式

uint8_t i,Serial_RxFlag; uint8_t Serial_RxPacket[14]; uint8_t Serial_TxPacket[14];

串口收发数据

代码如下

/** *@brief 串口发送一个字节 *@param Byte 要发送的字节 *@retval 无 */ void Serial_SendByte(uint8_t Byte) { USART_SendData(USART1,Byte); while(!USART_GetFlagStatus(USART1, USART_FLAG_TXE)); } /** *@brief 串口发送数组 *@param Array 要发送的数组 *@param 数组长度 *@retval 无 */ void Serial_SendArray(const uint8_t *Array, uint8_t Length) { for(i=0;i<Length;i++) { Serial_SendByte(Array[i]); } } /** *@brief 串口发送字符串 *@param String要发送的字符串 *@retval */ void Serial_SendString(char *String) { for(i=0;String[i];i++) { Serial_SendByte(String[i]); } } /** *@brief 串口发送数字 *@param Num 要发送的数字 *@param Length 数字长度 *@retval 无 */ void Serial_SendNum(uint32_t Num, uint8_t Length) { uint16_t j; for(j=0;j<Length;j++) { Serial_SendByte(((Num/Serial_Square(Length-j-1))%10+'0')); } } /** *@brief 获取串口接收标志位（标志位自动置0） *@param 无 *@retval 标志位状态，1为接收 0为未接收 */ uint8_t Serial_GetRxFlag(void) { if(Serial_RxFlag) { Serial_RxFlag=0; return 1; } return 0; }

数据包的收发

我们发送数据包格式是帧头(0xAA) + cmd(1byte) + 校验和(1byte)

接受数据包，当接受标志位置1时，代表收到了正确的数据包，然后Serial_RxPacket中存储的只有坐标，将坐标存储到action结构体里面

/** *@brief 发送一个数据包 *@param 无 *@retval 无 */ void Serial_SendPacket(const uint8_t* TxPacket, u8 Frame_Heade,uint8_t len) { u8 check = Frame_Heade;//校验 Serial_SendByte(Frame_Heade); Serial_SendArray(TxPacket, len); for(u8 i = 0; i < len; i ++) check += TxPacket[i]; Serial_SendByte(check); } /** *@brief 获取接收到的数据(解数据包) *@param Rx_arr uint8类型的接收数组 *@retval 无 */ bool Serial_GetPacket(ActionTypedef* action, u8 FrameHeade) { frame_heade = FrameHeade; if(Serial_GetRxFlag())//检测接收标志位置1（标志位自动置0） { /*解包： 协议格式：帧头(0xAA) + X(4byte float) + Y(4byte float) + Z(4byte float) + 校验和(1byte)*/ memcpy(&action->x, &Serial_RxPacket[0], 4); // X memcpy(&action->y, &Serial_RxPacket[4], 4); // Y memcpy(&action->z, &Serial_RxPacket[8], 4); // Z return true; } else { return false; } }

串口中断接收函数

我们这里用状态机的方法，当串口接收到数据时，先检验第一位是不是帧头，检测到帧头后进入下一个状态

下一个状态接收13字节的数据(3个坐标，每个坐标是float类型占4字节；一个字节校验和)进入最后一个状态

最后将坐标加起来，等不等于最后的校验和，如果等于，置接接收标志位为1，在上面那个函数中进一步处理数据。

/** *@brief USART1串口中断 *@param 无 *@retval 无 */ void USART1_IRQHandler(void) { static uint8_t RxState = 0; static uint8_t pRxPacket = 0; if(USART_GetITStatus(USART1, USART_IT_RXNE) == SET) { uint8_t RxData = USART_ReceiveData(USART1); if(RxState == 0) { //等待帧头 0xAA if(RxData == frame_heade) { pRxPacket = 0; RxState = 1; } } else if(RxState == 1) { //接收 12 字节数据：X+Y+Z Serial_RxPacket[pRxPacket++] = RxData; if(pRxPacket >= 12) { RxState = 2; } } else if(RxState == 2) { //校验和 uint8_t checksum = 0; for(int i=0; i<12; i++) { checksum += Serial_RxPacket[i]; } if(RxData == checksum) { Serial_RxFlag = 1; //正确接收！ OLED_ShowString(100, 40,"Ok", OLED_8X16); } else { OLED_ShowString(100, 40,"Fa", OLED_8X16); } RxState = 0; } USART_ClearITPendingBit(USART1, USART_IT_RXNE); } }

main函数

定义

我们把前面的头文件都加进来，然后定义OPT_action负责记录抓取位置，offset_开头的三个参数是位置偏移，后面视觉会讲到

定义StateEnumTypedef枚举负责状态机的状态

#include "OLED.h" #include "Delay.h" #include "Rob_arm.h" #include "key.h" #include "serial.h" #include "key.h" ActionTypedef OPT_action; //记录抓取位置 u8 Tx_arr[] = {0x55}; //命令 u8 Frame_heade = 0xAA; //数据帧帧头 u8 KeyNum; //按键 float offset_x = 0; //位置偏移 float offset_y = 200; float offset_z = -130; //状态机枚举 typedef enum { Ready = 0, WaitData, Run, }StateEnumTypedef; StateEnumTypedef ArmState = Ready;

main函数

我们mian函数采用状态机的方法，刚开始进入Ready状态，当你按下按键的时候发送命令0x55，然后进入WaitData状态，当接收到数据时，加上位置偏移，然后进入Run运行状态驱动机械臂；

int main() { Key_Init(); //初始化按键 Serial_Init(9600); //初始化串口 OLED_Init(); //初始化OLED显示屏 RobArm_DeInit(RobArm); //初始化机械臂 delay_s(3);//延迟保护 while(1) { switch(ArmState) { case Ready://准备状态：按下按键发松命令 { OLED_ShowString(100, 0,"Rd", OLED_8X16); /*检测按键*/ KeyNum = Key_GetNum(); /*检测到按键发送命令*/ if(KeyNum == 1) { ArmState = WaitData;//进入等待状态 Serial_SendPacket(Tx_arr, Frame_heade, 1);//发送获取位置信息命令 } break; } case WaitData://等待状态：等待接收数据包并解包数据 { OLED_ShowString(100, 0,"Wt", OLED_8X16); /*接收到正确数据包*/ if(Serial_GetPacket(&OPT_action, Frame_heade))//获取位置信息 { /*OLED显示数据*/ OLED_ShowFloatNum(1,1,OPT_action.x, 3, 0, OLED_8X16); OLED_ShowFloatNum(1,21,OPT_action.y, 3, 0, OLED_8X16); OLED_ShowFloatNum(1,41,OPT_action.z, 3, 0, OLED_8X16); OPT_action.x += offset_x; OPT_action.y += offset_y; OPT_action.z += offset_z; OLED_ShowFloatNum(40,1,OPT_action.x, 3, 0, OLED_8X16); OLED_ShowFloatNum(40,21,OPT_action.y, 3, 0, OLED_8X16); OLED_ShowFloatNum(40,41,OPT_action.z, 3, 0, OLED_8X16); ArmState = Run;//进入运行状态 } break; } case Run://运行状态：运行到指定坐标 { OLED_ShowString(100, 0,"Rn", OLED_8X16); if(RobArm_ActionGrap(OPT_action.x, OPT_action.y, OPT_action.z, 50, -45)) { RobArm_ActionGrap(180, 0, offset_z, 0, -45); RobArm_ActionGrap(180, 0, offset_z, 50, -45); RobArm_ActionGrap(0, 200, offset_z, 0, -45); ArmState = Ready; } break; } default: { break; } } OLED_Update(); } }

视觉

视觉部分在下一篇文章