前言本项目实现了一套完整的五自由度舵机机械臂控制系统。系统采用分布式架构设计- 下位机STM32F103单片机负责机械臂运动控制- 上位机Ubuntu系统运行视觉opencv- 通讯协议USART1串口波特率9600自定义数据包格式原理非常简单手把手教学抓取误差极低目录一、定时器配置二、舵机三、机械臂控制3.1逆运动学解算代码二维坐标定位三维坐标定位3.2步态平滑代码原理3.3机械臂抓取代码三、串口通信说明:串口初始化定义存储数组串口收发数据代码如下数据包的收发串口中断接收函数main函数定义main函数视觉一、定时器配置由于我们有4个自由度和一个机械爪所以要配置5路pwm我们先配置5个gpio引脚模式选择复用推挽输出/******************************************************************************* * 功 能 初始化定时器输出比较通道输出PWM20ms) * 定 时 器 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-1arr配置为20000-172Mhz ÷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 xy端执行器位置 *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; }二维坐标定位大臂小臂和末端执行器分别是L1L2L3然后要抓取的点是P末端执行器的角度是我们自己给的Rob_Arm_EndEffector_angle。我们首先根据三角函数计算出N点的坐标就知道了Xn、Yn和角度同时能计算出r x^2 y^2L1和L2又是我们已经知道的就可以求出三角形的内角123。第一个关节的角度就是1第二个关节角度就是2接下来我们要计算第三个关节的角度根据平行能知道5就能求出∠5∠3和∠4互为余角∠4 π/2 - ∠3第三个关节就是∠4 ∠5那么所有的关节角度都确定了就可以确定机械臂的姿态了。由此写出来的函数就只需要知道二维坐标(x,y)和末端执行器的角度自己设定的就可以实现坐标定位了三维坐标定位有了二维坐标的定位之后我们看三维中的目标点P会发现三维中的R就是二维中的xz就是二维坐标中的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;//时间系数t01] 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_ModeGPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_PinGPIO_Pin_9; GPIO_InitStructure.GPIO_SpeedGPIO_Speed_50MHz; GPIO_Init(GPIOA, GPIO_InitStructure); GPIO_InitStructure.GPIO_PinGPIO_Pin_10; //设置RXD引脚 GPIO_InitStructure.GPIO_ModeGPIO_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_IRQChannelUSART1_IRQn; NVIC_InitStruct.NVIC_IRQChannelCmdENABLE; NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority1; NVIC_InitStruct.NVIC_IRQChannelSubPriority1; 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(i0;iLength;i) { Serial_SendByte(Array[i]); } } /** *brief 串口发送字符串 *param String要发送的字符串 *retval */ void Serial_SendString(char *String) { for(i0;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(j0;jLength;j) { Serial_SendByte(((Num/Serial_Square(Length-j-1))%100)); } } /** *brief 获取串口接收标志位标志位自动置0 *param 无 *retval 标志位状态1为接收 0为未接收 */ uint8_t Serial_GetRxFlag(void) { if(Serial_RxFlag) { Serial_RxFlag0; 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 字节数据XYZ Serial_RxPacket[pRxPacket] RxData; if(pRxPacket 12) { RxState 2; } } else if(RxState 2) { //校验和 uint8_t checksum 0; for(int i0; i12; 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(); } }视觉视觉部分在下一篇文章