一款机械臂程序分享
- /***************************************************************
- @笔者:tacbo
- 功能列表:
- 1、单个舵机控制(支持PWM舵机和总线舵机)
- 2、多个舵机控制(支持PWM舵机和总线舵机)
- 3、手柄控制舵机
- 4、串口控制舵机
- 5、OLED显示舵机的执行情况
- 6、USB一键下载
- 7、可支持总线MP3 总线WIFI等设备
- 8、上位机图形化编程
- 9、控制6自由度机械臂
- ***************************************************************/
- #include "tb_rcc.h" //配置时钟文件
- #include "tb_gpio.h" //配置IO口文件
- #include "tb_global.h" //存放全局变量
- #include "tb_delay.h" //存放延时函数
- #include "tb_type.h" //存放类型定义
- #include "tb_usart.h" //存放串口功能文件
- #include "tb_timer.h" //存放定时器功能文件
-
- #include "ADC.h" //存放ADC的
- #include "PS2_SONY.h" //存放索尼手柄
- #include "w25q64.h" //存储芯片的操作
- #include "oled_i2c.h" //OLED文件
- #include <stdio.h> //标准库文件
- #include <string.h> //标准库文件
- #include <math.h> //标准库文件
- #define VERSION 20170919 //版本定义
- #define CYCLE 1000 //PWM模块周期
- #define PS2_LED_RED 0x73 //PS2手柄红灯模式
- #define PS2_LED_GRN 0x41 //PS2手柄绿灯模式
- #define PSX_BUTTON_NUM 16 //手柄按键数目
- #define PS2_MAX_LEN 64 //手柄命令最大字节数
- #define FLAG_VERIFY 0x25 //校验标志
- #define ACTION_SIZE 0x80 //一个动作的存储大小
- #define W25Q64_INFO_ADDR_SAVE_STR (((8<<10)-2)<<10)//(8*1024-1)*1024 //eeprom_info结构体存储的位置
- void system_init(void); //系统初始化
- void beep_led_dis_init(void); //开机提示
- void handle_nled(void); //LED工作指示灯提示
- void soft_reset(void); //软件复位
- void car_pwm_set(int car_left, int car_right); //电机控制函数
- void handle_ps2(void); //手柄数据解析
- void handle_button(void); //手柄按键解析
- void parse_psx_buf(unsigned char *buf, unsigned char mode); //手柄按键解析子函数
- void handle_car(void); //摇杆数据解析控制车
- void handle_uart(void); //串口解析
- void parse_cmd(u8 *cmd); //命令解析
- void action_save(u8 *str); //动作保存函数
- int get_action_index(u8 *str); //获取动作组序号
- void print_group(int start, int end); //打印动作组
- void int_exchange(int *int1, int *int2); //两个int互换
- void do_group_once(int group_num); //执行动作组1次
- void handle_action(void); //处理动作组执行
- u8 check_dj_state(void); //获取舵机的状态
- void do_action(u8 *uart_receive_buf); //执行动作
- void replace_char(u8*str, u8 ch1, u8 ch2); //字符串字母替换
- void rewrite_eeprom(void); //写入eeprom_info结构体
- void handle_adc(void); //处理ADC数据
- void handle_oled(void); //处理OLED数据
- u8 psx_buf[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //存储手柄的数据
- const char *pre_cmd_set_red[PSX_BUTTON_NUM] = { //红灯模式下按键的配置
- "<PS2_RED01:#005P0600T2000!^$DST:5!>", //L2
- "<PS2_RED02:#005P2400T2000!^$DST:5!>", //R2
- "<PS2_RED03:#004P0600T2000!^$DST:4!>", //L1
- "<PS2_RED04:#004P2400T2000!^$DST:4!>", //R1
- "<PS2_RED05:#002P2400T2000!^$DST:2!>", //RU
- "<PS2_RED06:#003P0600T2000!^$DST:3!>", //RR
- "<PS2_RED07:#002P0600T2000!^$DST:2!>", //RD
- "<PS2_RED08:#003P2400T2000!^$DST:3!>", //RL
- "<PS2_RED09:$DJ_RECORD_DO:1!>", //SE 执行1次
- "<PS2_RED10:$DJ_RECORD_CLEAR!>", //AL 清除
- "<PS2_RED11:$DJ_RECORD!>", //AR 学习
- "<PS2_RED12:$DJR!>", //ST
- "<PS2_RED13:#001P0600T2000!^$DST:1!>", //LU
- "<PS2_RED14:#000P0600T2000!^$DST:0!>", //LR
- "<PS2_RED15:#001P2400T2000!^$DST:1!>", //LD
- "<PS2_RED16:#000P2400T2000!^$DST:0!>", //LL
- };
- const char *pre_cmd_set_grn[PSX_BUTTON_NUM] = { //绿灯模式下按键的配置
- "<PS2_GRN01:$!>", //L2
- "<PS2_GRN02:$!>", //R2
- "<PS2_GRN03:$!>", //L1
- "<PS2_GRN04:$!>", //R1
- "<PS2_GRN05:$!>", //RU
- "<PS2_GRN06:$!>", //RR
- "<PS2_GRN07:$!>", //RD
- "<PS2_GRN08:$!>", //RL
- "<PS2_GRN09:$!>", //SE
- "<PS2_GRN10:$!>", //AL-NO
- "<PS2_GRN11:$!>", //AR-NO
- "<PS2_GRN12:$!>", //ST
- "<PS2_GRN13:$!>", //LU
- "<PS2_GRN14:$!>", //LR
- "<PS2_GRN15:$!>", //LD
- "<PS2_GRN16:$!>", //LL
- };
- /*"D:\DreamSpark\OLED\MP3_UI.bmp",0 图片取模数据
- unsigned char MY_PIC[] = {
- 0x00,0x03,0x05,0x09,0x11,0xFF,0x11,0x89,0x05,0xC3,0x00,0xE0,0x00,0xF0,0x00,0xF8,
- *************************
- 0x80,0xFF,0x80,0xEE,0xEE,0xEE,0xF5,0xFB,0xFF,0x9C,0xBE,0xB6,0xB6,0x88,0xFF,0x00,
- };
- */
- int i; //常用的一个临时变量
- u8 car_dw = 1; //摇杆档位控制
- u8 group_do_ok = 1; //动作执行完成标志
- int do_start_index; //动作组执行 起始序号
- int do_time; //动作组执行 执行次数
- int group_num_start; //动作组执行 起始序号
- int group_num_end; //动作组执行 终止序号
- int group_num_times; //动作组执行 起始变量
- u32 dj_record_time = 1000; //学习时间默认1000
- /*-------------------------------------------------------------------------------------------------------
- * 程序从这里执行
- * 这个启动代码 完成时钟配置 使用外部晶振作为STM32的运行时钟 并倍频到72M最快的执行速率
- -------------------------------------------------------------------------------------------------------*/
- int main(void)
- {
- tb_rcc_init(); ///时钟初始化 已经备注
-
- tb_gpio_init(); ///IO初始化 打开时钟
-
- tb_global_init(); //全局变量初始化
-
- nled_init(); ///工作指示灯初始化
-
- // beep_init(); ///蜂鸣器初始化
-
- dj_io_init(); ///舵机IO口初始化
-
- //w25q64 init
- W25Q_Init(); //动作组存储芯片初始化
-
- if(W25Q_TYPE != W25Q64){ //判断是否是W25Q64芯片
- while(1)beep_on(); //如果不是则长鸣,说明芯片有问题,无法通信
- }
- W25Q_Read((u8 *)(&eeprom_info), W25Q64_INFO_ADDR_SAVE_STR, sizeof(eeprom_info)); //读取全局变量
- if(eeprom_info.version != VERSION) { //判断版本是否是当前版本
- eeprom_info.version = VERSION; //复制当前版本
- eeprom_info.dj_record_num = 0; //学习动作组变量赋值0
- rewrite_eeprom(); //写入到存储器 ///把结构体里面的内容读取处理 然后再把内容写进去
- }
-
- //ADC_init(); ///ADC初始化
-
- //PSX_init(); //手柄初始化 GPIO
-
-
- TIM2_Int_Init(20000, 71); ///舵机 定时器初始化 定时器中断输出高电平
-
- ///小车 pwm 初始化 利用了高级定时器 PWM引脚输出
- TIM3_Pwm_Init(1000, 239);
- TIM4_Pwm_Init(1000, 239);
- car_pwm_set(0,0); //设置小车的左右轮速度为0
-
- //串口1初始化
- tb_usart1_init(115200); //配置结构体,不包括使能串口
- uart1_open(); //打开串口中断
-
- //串口2初始化
- tb_usart2_init(115200);
- uart2_open();
-
- //串口3初始化
- tb_usart3_init(115200);
- uart3_open();
-
- //总中断打开
- tb_interrupt_open();
-
-
- //三个串口发送测试字符
- uart1_send_str((u8 *)"uart1 check ok!");
- uart2_send_str((u8 *)"uart2 check ok!");
- uart3_send_str((u8 *)"uart3 check ok!");
-
- //总线输出 复位总线舵机
- zx_uart_send_str((u8 *)"#255P1500T2000!");
-
- //系统滴答时钟初始化
- SysTick_Int_Init();
-
- //蜂鸣器LED 名叫闪烁 示意系统启动
- beep_led_dis_init();
-
-
- //执行预存命令
- if(eeprom_info.pre_cmd[PRE_CMD_SIZE] == FLAG_VERIFY) {
- if(eeprom_info.pre_cmd[0] == '
- ) {
- memset(uart_receive_buf, 0, sizeof(uart_receive_buf));
- strcpy((char *)uart_receive_buf, (char *)eeprom_info.pre_cmd);///把储存的数据读取出来
- uart1_mode = 1;
- uart1_get_ok = 1;
- uart1_send_str(uart_receive_buf);
- }
- }
-
- //OLED初始化
- //OLED_Init();
-
- while(1) //parse_cmd(uart_receive_buf); //解析命令模式 串口改变变量和按键 公用这个函数 解析动作,解析动作只是改变舵机的变量,舵机控制在中断函数
- {
- handle_nled(); //处理信号灯 ///利用系统内部定时器SysTick
- handle_ps2(); //处理手柄 初始化 把手柄的数据读取出来
-
-
- handle_button(); //处理手柄按钮 //do_action(uart_receive_buf); 获取按键状态 控制舵机
- handle_car(); //处理摇杆控制小车
-
- handle_uart(); //处理串口接收数据 //定义一个指针,可以被多个函数调用 中断接收串口数据 do_action(uart_receive_buf);
- handle_action(); //处理动作组 main 865
-
- handle_adc(); //处理ADC
- handle_oled(); //处理OLED显示
- }
- }
- //开机指示函数,蜂鸣器和工作指示灯鸣3声作为开机指示
- void beep_led_dis_init(void) {
- beep_on();nled_on();tb_delay_ms(100);beep_off();nled_off();tb_delay_ms(100);
- beep_on();nled_on();tb_delay_ms(100);beep_off();nled_off();tb_delay_ms(100);
- beep_on();nled_on();tb_delay_ms(100);beep_off();nled_off();tb_delay_ms(100);
- }
- ///工作指示灯处理,间隔1000MS闪烁一次
- void handle_nled(void) {
- static u32 time_count=0;
- static u8 flag = 0;
- if(systick_ms-time_count > 1000) {
- time_count = systick_ms;
- if(flag) {
- nled_on();
- } else {
- nled_off();
- }
- flag= ~flag;
- }
- }
- //软件复位函数,调用后单片机自动复位
- void soft_reset(void) {
- __set_FAULTMASK(1);
- NVIC_SystemReset();
- }
- //小车控制函数
- //参数 左轮速度和右轮速度 范围 -1000 到 1000
- void car_pwm_set(int car_left, int car_right) {
-
- if(car_left >= CYCLE)car_left = CYCLE-1;
- else if(car_left <= -CYCLE)car_left = -CYCLE+1;
- else if(car_left == 0)car_left = 1;
-
- if(car_right >= CYCLE)car_right = CYCLE-1;
- else if(car_right <= -CYCLE)car_right = -CYCLE+1;
- else if(car_right == 0)car_right = 1;
-
- //car_left = car_left/car_dw;
- //car_right = car_right/car_dw;
-
- if(car_right>0) {
- TIM_SetCompare4(TIM4,1);
- TIM_SetCompare3(TIM4,car_right); ///设置CCR,初始化已经设置的CCR没影响 占空比
- }
- else
- {
- TIM_SetCompare4(TIM4,-car_right);
- TIM_SetCompare3(TIM4,1);
-
- }
- if(car_left>0)
- {
- TIM_SetCompare4(TIM3,1);
- TIM_SetCompare3(TIM3,car_left); ///设置占空比 CCR PWM比较寄存器
- }
- else
- {
- TIM_SetCompare4(TIM3,-car_left);
- TIM_SetCompare3(TIM3,1);
-
- }
- // //总线马达设置
- // sprintf((char *)cmd_return, "#0233P%dT0!#034P%dT0!",
- // (int)(1500+car_left), (int)(1500+car_right));
- // zx_uart_send_str(cmd_return);
-
- return;
- }
- //处理手柄
- void handle_ps2(void)
- {
- static u32 systick_ms_bak = 0;
- //每20ms处理1次
- if(systick_ms - systick_ms_bak < 20)
- {
- return;
- }
-
- systick_ms_bak = systick_ms;
-
- //读写手柄数据
- psx_write_read(psx_buf); //把psx buf数据处理
-
- #if 0
- //测试手柄数据,1为打开 0为关闭
- sprintf((char *)cmd_return, "0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x\r\n",
- (int)psx_buf[0], (int)psx_buf[1], (int)psx_buf[2], (int)psx_buf[3],
- (int)psx_buf[4], (int)psx_buf[5], (int)psx_buf[6], (int)psx_buf[7], (int)psx_buf[8]);
- uart1_send_str(cmd_return);
- #endif
-
- return;
- }
- //处理手柄按键
- void handle_button(void)
- {
- static unsigned char psx_button_bak[2] = {0};///首先对这个数组清空
- //对比两次获取的按键值是否相同 ,相同就不处理,不相同则处理
-
- if((psx_button_bak[0] == psx_buf[3]) //psx buf 是已经获取的值
- && (psx_button_bak[1] == psx_buf[4]))
- {
-
- }
-
-
- else {
- //处理buf3和buf4两个字节,这两个字节存储这手柄16个按键的状态
- parse_psx_buf(psx_buf+3, psx_buf[1]); ///获取状态 把按键获取的数值 输入到函数处理
- psx_button_bak[0] = psx_buf[3];//把这两个值 放入到数组
- psx_button_bak[1] = psx_buf[4];
- }
- return;
- }
- //处理手柄按键字符,buf为字符数组,mode是指模式 主要是红灯和绿灯模式 获取按钮状态
- void parse_psx_buf(unsigned char *buf, unsigned char mode) ///mode就是红灯或者绿灯
- {
- u8 i, pos = 0;
- static u16 bak=0xffff, temp, temp2;
- temp = (buf[0]<<8) + buf[1];
-
- if(bak != temp)
- {
- temp2 = temp;
- temp &= bak;
- for(i=0;i<16;i++) {//16个按键一次轮询
- if((1<<i) & temp)
- {
- }
-
-
- else
- {
- if((1<<i) & bak)
- { //press 表示按键按下了
-
- memset(uart_receive_buf, 0, sizeof(uart_receive_buf)); ///把数组清空
- if(mode == PS2_LED_RED)
- {
- memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_red[i], strlen(pre_cmd_set_red[i]));
- }
- else if(mode == PS2_LED_GRN)
- {
- memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_grn[i], strlen(pre_cmd_set_grn[i]));
- } //把按下的按键对应的数值传递给接收数组 用来解析
-
- else continue;
-
- pos = str_contain_str(uart_receive_buf, (u8 *)"^"); ///通过按键 解析模式
- if(pos)
- uart_receive_buf[pos-1] = '\0';
-
- if(str_contain_str(uart_receive_buf, (u8 *)"[ DISCUZ_CODE_0 ]quot;)) //命令模式
- {
- uart1_close();
- uart1_get_ok = 0;
- strcpy((char *)cmd_return, (char *)uart_receive_buf+11);
- strcpy((char *)uart_receive_buf, (char *)cmd_return);
- uart1_get_ok = 1;
- uart1_open();
- uart1_mode = 1;
- }
-
-
-
- else if(str_contain_str(uart_receive_buf, (u8 *)"#"))
- {
- uart1_close();
- uart1_get_ok = 0;
- strcpy((char *)cmd_return, (char *)uart_receive_buf+11);
- strcpy((char *)uart_receive_buf,(char *) cmd_return);
- uart1_get_ok = 1;
- uart1_open();
- uart1_mode = 2;
- }
-
- //uart1_send_str(uart_receive_buf);
- bak = 0xffff;
- } else {//release 表示按键松开了
-
- memset(uart_receive_buf, 0, sizeof(uart_receive_buf));
- if(mode == PS2_LED_RED) {
- memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_red[i], strlen(pre_cmd_set_red[i]));
- } else if(mode == PS2_LED_GRN) {
- memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_grn[i], strlen(pre_cmd_set_grn[i]));
- } else continue;
-
- pos = str_contain_str(uart_receive_buf, (u8 *)"^");
- if(pos) {
- if(str_contain_str(uart_receive_buf+pos, (u8 *)"[ DISCUZ_CODE_0 ]quot;)) {
- //uart1_close();
- //uart1_get_ok = 0;
- strcpy((char *)cmd_return, (char *)uart_receive_buf+pos);
- cmd_return[strlen((char *)cmd_return) - 1] = '\0';
- strcpy((char *)uart_receive_buf, (char *)cmd_return);
- parse_cmd(uart_receive_buf);
- //uart1_get_ok = 1;
- //uart1_mode = 1;
- } else if(str_contain_str(uart_receive_buf+pos, (u8 *)"#")) {
- //uart1_close();
- //uart1_get_ok = 0;
- strcpy((char *)cmd_return, (char *)uart_receive_buf+pos);
- cmd_return[strlen((char *)cmd_return) - 1] = '\0';
- strcpy((char *)uart_receive_buf, (char *)cmd_return);
- do_action(uart_receive_buf);
- //uart1_get_ok = 1;
- //uart1_mode = 2;
- }
- //uart1_send_str(uart_receive_buf);
- }
- }
- }
- }
- bak = temp2;
- beep_on();mdelay(10);beep_off();
- }
- return;
- }
- //int型 取绝对值函数
- int abs_int(int int1) {
- if(int1 > 0)return int1;
- return (-int1);
- }
- //处理小车函数 主要处理摇杆的数据 这里 8为左摇杆 6为右摇杆
- void handle_car(void)
- {
- static int car_left, car_right, car_left_bak, car_right_bak;
-
- if(psx_buf[1] != PS2_LED_RED)return;
-
- if(abs_int(127 - psx_buf[8]) < 5 )
- psx_buf[8] = 127;
-
- if(abs_int(127 - psx_buf[6]) < 5 )
- psx_buf[6] = 127;
-
- car_left = (127 - psx_buf[8]) * 8;
- car_right = (127 - psx_buf[6]) * 8;
-
- // if(abs_int(car_left_bak-car_left) < 20 && abs_int(car_right_bak-car_right) < 20)return;
- // if(abs_int(car_left_bak-car_left) > 40)car_left_bak = car_left;
- // if(abs_int(car_right_bak-car_right) > 40)car_right_bak = car_right;
-
- if(car_left != car_left_bak || car_right != car_right_bak)
- {
- car_pwm_set(car_left, car_right); ///把速度传送到定时器PWM寄存器
- car_left_bak = car_left;
- car_right_bak = car_right;
- }
- }
- //处理串口接收到的数据 ///首先通过中断获取串口的数据
- void handle_uart(void)
-
- ///串口接收中断,判断中断标志位,进入中断函数,把数据保存在数组,判断数组内容,返回uart1_get_ok
- ///当接收到数据之后,马上执行串口解析动作,判断数据,执行动作
- {
- if(uart1_get_ok)
-
- {
- if(uart1_mode == 1) //命令模式
- {
- //uart1_send_str("cmd:");
- //uart1_send_str(uart_receive_buf);
- parse_cmd(uart_receive_buf); //解析命令模式
- }
-
-
- else if(uart1_mode == 2) ///单个命令 只有一种模式
- { //单个舵机调试
- //uart1_send_str("sig:");
- //uart1_send_str(uart_receive_buf);
- do_action(uart_receive_buf); ///处理接收数据内容
- }
-
- else if(uart1_mode == 3)
- { //多路舵机调试
- //uart1_send_str("group:");
- //uart1_send_str(uart_receive_buf);
- do_action(uart_receive_buf);
- }
-
- else if(uart1_mode == 4)
- { //存储模式
- //uart1_send_str("save:");
- //uart1_send_str(uart_receive_buf);
- action_save(uart_receive_buf);
- }
-
- uart1_mode = 0;
- uart1_get_ok = 0;
- }
- return;
- }
- /*
- 所有舵机停止命令:$DST!
- 第x个舵机停止命令:$DST:x!
- 单片机重启命令:$RST!
- 检查动作组x到y组命令:$CGP:x-y!
- 执行第x个动作:$DGS:x!
- 执行第x到y组动作z次:$DGT:x-y,z!
- 小车左轮x速度,右轮y速度:$DCR:x,y!
- 所有舵机复位命令:$DJR!
- 获取应答信号:$GETA!
- */
- //命令解析函数
- void parse_cmd(u8 *cmd)
- {
- int pos, i, index, int1, int2;
- u8 temp_buf[160];
- u32 long1;
-
- //uart1_send_str(cmd);
-
- if(pos = str_contain_str(uart_receive_buf, (u8 *)"$DST!"), pos) ///接收信息比较 所有舵机停止命令:$DST!
- {
- group_do_ok = 1; //动作执行完成标志
-
- for(i=0;i<DJ_NUM;i++) ///停止舵机
- ……………………
- …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码
代码下载:
注释stm32单片机之串口1解析控制舵机.7z
(1.18 MB, 下载次数: 10)
|