设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › 舵机控制器STM32源程序
返回列表 发新帖
查看: 3421|回复: 1
打印 上一主题 下一主题
收起左侧

舵机控制器STM32源程序

[复制链接]
跳转到指定楼层
楼主
ID:339076 发表于 2018-5-27 14:45 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
分享一个舵机驱动程序

单片机源程序如下:
  1. #include <stdlib.h>
  2. #include <string.h>
  3. #include "stm32f10x.h"                                            //包含stm32库系统头文件
  4. #include "servor.h"                                                        //包含GPIO库头文件
  5. #include "usart.h"                                                          //包含串口通信设置头文件
  6. #include "delay.h"                                                //包含系统定时器库头文件
  7. #include "timer.h"                                        //包含定时器设置头文件
  8. #include "PS2.h"
  9. #include "key.h"
  10. #include "led.h"
  11. #include "beep.h"
  12. #include "flash.h"
  13. #include "common.h"
  14. #include "adc.h"
  15. const u8 TEXT_Buffer[]={"Flash SPI TEST"};
  16. #define SIZE sizeof(TEXT_Buffer)       
  17.         u8 datatemp[SIZE];
  18.         u32 FLASH_SIZE;
  19. u8 date[3];                                                                        //该变量用来存储安卓终端发来的数据
  20. u32 value;                                                                       
  21. u32 key, key_bak;
  22. extern u32 a;                                                                  //a用来计数，配合系统滴答定时器可检测代码运行时间
  23. extern uint8 flag_RecFul;
  24. uint16 pos[7][MOTOR_NUM]={ {1000,1500,1500,1500,1500,1500,1500,1500,1500},
  25.                                          {1000,1500,1500,1500,1500,1500,1500,1500,1500},
  26.                                          {1000,1500,1500,1500,1500,1500,1500,1500,1500},
  27.                                          {1000,500,500,500,500,500,500,500,500},
  28.                                          {1000,1500,1500,1500,1500,1500,1500,1500,1500},
  29.                                          {1000,500,500,500,500,500,500,500,500},
  30.                                          {1000,1500,1500,1500,1500,1500,1500,1500,1500}
  31. };         //位置
  32. uint16 CPWM[MOTOR_NUM]=   {1500,1500,1500,1500,1500,1500,1500,1500,1500};
  33. uint16 UartRec[MOTOR_NUM]={1500,1500,1500,1500,1500,1500,1500,1500,1500};

  35. unsigned int dj1=1500;
  36. unsigned int dj2=1500;
  37. unsigned int dj3=1500;
  38. unsigned int dj4=1500;
  39. unsigned int dj5=1500;
  40. unsigned int dj6=1600;
  41. unsigned char buf[30];

  43. uint8 line=0;                                        //缓存存入口与出口之间的距离，即当前缓存中有多少个没有执行的数据
  44. char point_now=0;                                //与point_aim一起标记缓存出口位置，即取数位置
  45. char point_aim=1;
  46. char point_in=1;                                //标记缓存入口位置，即上一行数据存放位置
  47. uint8 flag_connect = 0;
  48. uint8 flag_stop=1;                                //表示一行执行结束
  49. uint8 flag_vpwm=0;                                //表示到达了该更新pwm[]的时间
  50. uint8 flag_in=1;                                         //表示缓存中有空闲空间
  51. uint8 flag_out=0;                                        //表示缓存中有可执行数据的标志位
  52. uint8 flag_run_ready=0;                        //表示有要要放入缓存的EErom数据

  54. uint16 n=1000;                                        //用来计算需要建立多少个中间数据
  55. uint16 m=1;                                                //用来累计已经执行了多少中间数据
  56. double dp;
  57. double dp0[MOTOR_NUM] = {0};                                        //插补增量
  58. uint8 flag_download = 0;//判断是否下载
  59. uint8 flag_read = 0;// 读取flash内容，发送上位机
  60. uint8 flag_connect_run = 0;//连接上位机的执行flash保存的命令
  61. uint8 flag_stop_download = 0;//停止下载
  62. uint8 flag_online_run = 0;
  63. uint8 flag_uart2_rev = 0;
  64. uint8 flag_uart2_rev_time_out = 0;
  65. uint8 flag_ps2_rev = 0;
  66. char redata[257] = {0};    // 定义接收数据变量数组
  67. //uint8 flag_read_adc = 0;

  69. unsigned long send_mode = 0;//当前串口接收到命令状态存储
  70. MotorData motor_data;//舵机总信息
  71. MotorOneCmd motor_one_cmd;//单个命令
  72. CurrentItem cur_item;
  73. uint16 tuoji_count = 0;//脱机执行次数
  74. unsigned char flag_scan_ps2 = 0;
  75. uint8 flag_adc=0;
  76. uint8 error = 0;
  77. uint8 file_list[MAX_SUPPORT_FILE_SAVE] = {0};
  78. int file_list_count = 0;
  79. int file_last_num = 0;
  80. unsigned char ps2_buf[120] = {0};
  81. char uart2_buf[50] = {0};
  82. uint16 cur_count = 0;
  83. uint16 adc_value=0;
  84. //uchar ad_value = 0;
  85. //uchar beep_mode = 0;
  86. u8 ps2_mode=0;
  87. void SerVor_Test(void);
  88. void Ps2_Test(void);
  89. void Flash_Test(void);
  90. void updata_file_list(void);
  91. void InitMotor(void);
  92. void vpwm(void);
  93. void scan_ps2(void);
  94. void Check_Power(void);
  95. /**********************************************************************************/
  96. /*****************************主函数***********************************************/
  97. int main(void)
  98. {   
  99.         SysTick_Init();                                                                           //系统滴答定时器初始化
  100.         Servor_GPIO_Config();                                                                           //GPIO初始化
  101.   Uart_Init(1);
  102.   Uart_Init(2);       
  103.         Timer_Init();                                                                           //定时器初始化
  104.         Timer_ON();                                                                                   //开启定时器                        
  105.         LED_Init();       
  106.         Beep_Init();
  107.         PS2_Init();
  108.         Adc_Init();
  109.         USART_Config(USART1,115200);
  110.         USART_Config(USART2,115200);
  111.         SpiFlashInit();                  //初始化flash         
  112.         Led_Test();
  113.         Beep_Test();
  114.        
  115.         while(SpiFlashReadID()!=W25Q64)                                                        //检测不到W25Q64
  116.         {
  117.                 UART_PutStr (USART1,"Not Found W25Q64\r\n");
  118.         }

  120.         while (1)
  121.         {       
  122.         //        Flash_Test();
  123.         //UART_PutStr (USART2,"uart2 test\r\n");
  124.                 if(flag_vpwm==1)                  
  125.                 {       
  126.                         vpwm();                                        //更新pwm[]数组
  127.                         flag_vpwm=0;
  128.                 }
  129.                 if( flag_RecFul==1)                   //串口接受完一条指令
  130.                 {
  131.                        
  132.                         //LED3_ON();
  133.                          DealRec();                                 //处理串口缓存中的数据
  134.                         flag_RecFul=0;
  135.                         //LED3_OFF();
  136.                 }
  137.                 GetOneMotorCMD();//获取一个命令
  138.                 SendUartState();//发送状态信息
  139.   #if PS_SUPPORT
  140.                  scan_ps2();
  141.   #endif
  142.         //        LED_State();
  143.                 Check_Power();
  144.                 //cur_count++;
  145.         }
  146. }


  149. void updata_file_list(void)
  150. {
  151.         unsigned char i = 0;
  152.         unsigned char j = 0;
  153.         file_last_num = -1;
  154.         ReadMoterInfor();
  155.         for (i = 0; i < motor_data.filecount; i++)
  156.         {
  157.                 if (motor_data.file_flag[i] == 1)
  158.                 {
  159.                         file_list[j] = i;
  160.                         j++;
  161.                         file_last_num = i;
  162.                 }
  163.         }
  164.         file_list_count = j;
  165. }
  166. void InitMotor(void)
  167. {
  168.         ReadMoterInfor();//读取舵机控制信息
  169.         updata_file_list();
  170.         memset(&cur_item,0,sizeof(cur_item));
  171.         //beep_mode = motor_data.beep_mode;
  172. }

  174. void Check_Power(void)
  175. {
  176.                 if(flag_adc==1)
  177.                 {
  178.                         flag_adc=0;
  179.                         adc_value=Get_Adc(0);
  180.                         if(adc_value>=2100 && adc_value<=2481) //电池电量低  7.4V锂电池这里当电池电压低于6V就算没电  　>2100是为了防止没接电池误报
  181.                         {
  182.                                 Beep=~Beep;
  183.                         }
  184.                         else
  185.                         {
  186.                                 BEEP(OFF);
  187.                         }
  188.                         //UART_Put_Inf("adc:",adc_value); //将读取到ADC1 通道0的ADC值打印出来
  189.                 }
  190. }

  192. void scan_ps2(void)
  193. {
  194.         int  kind = 0;
  195.         char *p = NULL;
  196.         char buf[15] = {0};
  197.         char i = 0;
  198.         if (flag_scan_ps2)//
  199.         {
  200.                 flag_scan_ps2 = 0;
  201.                 key = PS2_DataKey();
  202.                 ps2_mode=PS2_RedLight();
  203.                 if(ps2_mode==0)
  204.                 {
  205.                         if(key_bak == key)return;
  206.                         key_bak=key;
  207.                         Beep=~Beep;
  208.                         switch(key)
  209.                         {
  210.                                 case PSB_PAD_UP:kind = 1;break;
  211.                                 case PSB_PAD_DOWN:kind = 2;break;
  212.                                 case PSB_PAD_LEFT:kind = 3;break;
  213.                                 case PSB_PAD_RIGHT:kind = 4;break;

  215.                                 case PSB_TRIANGLE:kind = 7;break;
  216.                                 case PSB_CROSS:kind = 8;break;
  217.                                 case PSB_PINK:kind = 9;break;
  218.                                 case PSB_CIRCLE:kind = 10;break;

  220.                                 case PSB_L1:kind = 6;break;
  221.                                 case PSB_L2:kind = 5;break;
  222.                                 case PSB_R1:kind = 12;break;
  223.                                 case PSB_R2:kind = 11;break;
  224.                                 default:break;
  225.                         }
  226.                         if (kind != 0)
  227.                         {               
  228.                                 flag_ps2_rev = 1;
  229.                                 flag_connect = 1;
  230.                                 SpiFlashRead(ps2_buf,(PS2_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(ps2_buf));
  231.                                 sprintf(buf,"%dK",kind);
  232.                                 UART_PutStr(USART1,buf);
  233.                                 p = strstr(ps2_buf,buf);
  234.                                 if (p != NULL)
  235.                                 {
  236.                                         p = p + strlen(buf);
  237.                                         while(i < 14 && *p != 0)
  238.                                         {
  239.                                                 buf[i] = *p++;
  240.                                                 i++;
  241.                                                 if (*p == '#')
  242.                                                         break;
  243.                                         }
  244.                                         if (i < 12)
  245.                                         {
  246.                                                 buf[i] = '\r';
  247.                                                 buf[i+1] = '\n';
  248.                                                 memcpy(redata,buf,sizeof(buf));
  249.                                                 flag_RecFul = 1;
  250.                                         }
  251.                                         UART_PutStr(USART1,redata);
  252.                         }
  253.                 }
  254.                 }
  255.                 else if(ps2_mode==1)
  256.                 {
  257.                                 switch(key)
  258.                                 {
  259.                                         case PSB_PAD_UP:CPWM[1]+=10;if(CPWM[1]>=2300)   CPWM[1]=2300;dj1+=10;if(dj1>=2200)dj1=2200;sprintf(buf,"#1P%dT1\r\n",dj1);UART_PutStr(USART2,buf);break;
  260.                                         case PSB_PAD_DOWN:CPWM[1]-=10;if(CPWM[1]<=700)  CPWM[1]=700;dj1-=10;if(dj1<=700)dj1=700;sprintf(buf,"#1P%dT1\r\n",dj1);UART_PutStr(USART2,buf);break;
  261.                                         case PSB_PAD_LEFT:CPWM[2]+=10;if(CPWM[2]>=2300) CPWM[2]=2300;dj2+=10;if(dj2>=2200)dj2=2200;sprintf(buf,"#2P%dT1\r\n",dj2);UART_PutStr(USART2,buf);break;
  262.                                         case PSB_PAD_RIGHT:CPWM[2]-=10;if(CPWM[2]<=700) CPWM[2]=700;dj2-=10;if(dj2<=700)dj2=700;sprintf(buf,"#2P%dT1\r\n",dj2);UART_PutStr(USART2,buf);break;
  263.                
  264.                                         case PSB_TRIANGLE:CPWM[3]+=10;if(CPWM[3]>=2300) CPWM[3]=2300;dj3+=10;if(dj3>=2200)dj3=2200;sprintf(buf,"#3P%dT1\r\n",dj3);UART_PutStr(USART2,buf);break;
  265.                                         case PSB_CROSS:CPWM[3]-=10;if(CPWM[3]<=700)  CPWM[3]=700;dj3-=10;if(dj3<=700)dj3=700;sprintf(buf,"#3P%dT1\r\n",dj3);UART_PutStr(USART2,buf);break;
  266.                                         case PSB_PINK:CPWM[4]+=10;if(CPWM[4]>=2300)  CPWM[4]=2300;dj4+=10;if(dj4>=2300)dj4=2300;sprintf(buf,"#4P%dT1\r\n",dj4);UART_PutStr(USART2,buf);break;
  267.                                         case PSB_CIRCLE:CPWM[4]-=10;if(CPWM[4]<=700) CPWM[4]=700;dj4-=10;if(dj4<=700)dj4=700;sprintf(buf,"#4P%dT1\r\n",dj4);UART_PutStr(USART2,buf);break;

  269.                                         case PSB_L1:CPWM[5]+=10;if(CPWM[5]>=2300) CPWM[5]=2300;dj5+=10;if(dj5>=2200)dj5=2200;sprintf(buf,"#5P%dT1\r\n",dj5);UART_PutStr(USART2,buf);break;
  270.                                         case PSB_L2:CPWM[5]-=10;if(CPWM[5]<=700)  CPWM[5]=700;dj5-=10;if(dj5<=700)dj5=700;sprintf(buf,"#5P%dT1\r\n",dj5);UART_PutStr(USART2,buf);break;
  271.                                         case PSB_R1:CPWM[6]+=10;if(CPWM[6]>=2300) CPWM[6]=2300;dj6+=10;if(dj6>=2300)dj6=2300;sprintf(buf,"#6P%dT1\r\n",dj6);UART_PutStr(USART2,buf);break;
  272.                                         case PSB_R2:CPWM[6]-=10;if(CPWM[6]<=700)  CPWM[6]=700;dj6-=10;if(dj6<=700)dj6=700;sprintf(buf,"#6P%dT1\r\n",dj6);UART_PutStr(USART2,buf);break;
  273.                                         default:break;
  274.                                 }
  275.                 }
  276.         }
  277. }


  280. /***************************************************************************************************************
  281. 函 数 名：从flash读取舵机总的信息
  282. 功能描述：初始化舵机控制信息
  283. 输入参数：无  
  284. 返 回 值：无  
  285. 备    注：
  286. ****************************************************************************************************************/       
  287. void ReadMoterInfor(void)
  288. {
  289.         memset(&motor_data,0,sizeof(motor_data));//清 0
  290.         SpiFlashRead((unsigned char *)&motor_data,(CMD_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_data));//读取信息
  291.         if (motor_data.CRC1 != 0x12345678 || motor_data.sum < 0 || motor_data.duoji_count  > MOTOR_NUM)//判断信息存储是否有错
  292.         {
  293.                 memset(&motor_data,0,sizeof(motor_data));
  294.                 //memset(&cur_item,0,sizeof(cur_item));
  295.         }
  296.         else//正常信息
  297.         {
  298.         //        UART1_SendOneChar(motor_data.sum + 0x30);
  299.                 //cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脱机运行次数
  300.                 //cur_item.cur_num = 0;//清 0
  301.         }
  302.                
  303. }
  304. void ReadOneCmdInfor(unsigned int addr)
  305. {
  306.         memset(&motor_one_cmd,0,sizeof(motor_one_cmd));//清 0
  307.         SpiFlashRead((unsigned char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd));//读取信息
  308.         if (motor_one_cmd.start >= motor_one_cmd.end || motor_one_cmd.cur_file_num != (addr) || motor_data.file_flag[motor_one_cmd.cur_file_num] == 0)//判断信息存储是否有错
  309.         {
  310.                 memset(&motor_one_cmd,0,sizeof(motor_one_cmd));
  311.         }
  312.         else//正常信息,以后留着验证用的
  313.         {
  314.                
  315.                
  316.                 cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脱机运行次数
  317.                 cur_item.cur_num = motor_one_cmd.start;
  318.         //        UART1_SendOneChar(motor_one_cmd.tuoji_count + 0x30);
  319.         //        cur_item.cur_num = 0;//清 0
  320.         }
  321.                
  322. }
  323. /***************************************************************************************************************
  324. 函 数 名：把舵机信息写到flash中
  325. 功能描述：把舵机信息写到flahs中
  326. 输入参数：无  
  327. 返 回 值：无  
  328. 备    注：
  329. ****************************************************************************************************************/       
  330. void WriteMoterInfor(void)
  331. {
  332.         unsigned char temp =0;
  333.         motor_data.CRC1 = 0x12345678;//校验码
  334.         motor_data.duoji_count = MOTOR_NUM-1;
  335.         temp = motor_data.filecount;
  336.         SpiFlashEraseSector(CMD_FLASH_ADDR >> 4);//擦除以前存储的信息
  337.         SpiFlashWrite((unsigned char *)&motor_data,CMD_FLASH_ADDR<<WRITE_BIT_DEPTH,sizeof(motor_data)); //写入flash
  338.         ReadMoterInfor();
  339.         if (temp != motor_data.filecount)
  340.         {
  341.                 error |= ERROR_FLASH_WRITE;
  342.         }
  343.         else
  344.         {
  345.                 error &= ~ERROR_FLASH_WRITE;
  346.         }
  347. }
  348. void WriteOneCmdInfor(unsigned int addr)
  349. {
  350.         unsigned char temp = 0;
  351.         temp = motor_one_cmd.end;
  352.         if (((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) % 16 == 0)
  353.                 SpiFlashEraseSector(((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) >> 4);//擦除以前存储的信息
  354.         SpiFlashWrite((unsigned char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd)); //写入flash
  355.         ReadOneCmdInfor(addr);
  356.         if (temp !=  motor_one_cmd.end)
  357.         {
  358.                 error |= ERROR_FLASH_WRITE1;
  359.         }
  360.         else
  361.         {
  362.                 error &= ~ERROR_FLASH_WRITE1;
  363.         }
  364. }
  365. /***************************************************************************************************************
  366. 函 数 名：获取一串舵机的控制字符串
  367. 功能描述：处理从flash读取的舵机控制字符串处理
  368. 输入参数：无  
  369. 返 回 值：无  
  370. 备    注：
  371. ****************************************************************************************************************/       
  372. void GetOneMotorCMD(void)
  373. {
  374. #if DEBUG
  375.         uchar buf[20] = {0};
  376. #endif
  377.         if (flag_stop_download)//接收到了上位机的停止下载的命令
  378.         {
  379.                 flag_download = 0;//清楚下载状态标志位
  380.                 flag_stop_download = 0;
  381.                 flag_read = 0;
  382.                 if (motor_data.filecount < MAX_SUPPORT_FILE_SAVE)
  383.                 {
  384.                         updata_file_list();
  385.                         motor_data.sum = motor_one_cmd.end;
  386.                         motor_data.file_flag[motor_data.filecount] = 1;
  387.                         motor_one_cmd.cur_file_num = file_last_num + 1;
  388.                         motor_data.filecount = motor_one_cmd.cur_file_num + 1;
  389.                         error &= ~MAX_SUPPORT_FILE_SAVE;
  390. #if DEBUG
  391.                         sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
  392.                         UART_PutStr(USART1,buf);
  393. #endif
  394.                         WriteMoterInfor();
  395.                         WriteOneCmdInfor(motor_one_cmd.cur_file_num);
  396.                         updata_file_list();
  397. #if DEBUG
  398.                        
  399.                         sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
  400.                         UART_PutStr(USART1,buf);
  401. #endif
  402.                 }
  403.                 else
  404.                 {
  405.                         error |= MAX_SUPPORT_FILE_SAVE;
  406.                 }
  407.                 if        (!(error &(MAX_SUPPORT_FILE_SAVE | ERROR_FLASH_FULL)))
  408.                         send_mode |= SEND_DOWN_OK;//状态位置为
  409.         }
  410.         if (flag_connect)//如果当前跟上位机联机状态
  411.         {
  412.                 if (flag_read)//如果上位机读取flash内存储的信息
  413.                 {               
  414.                         if (cur_item.cur_num < motor_one_cmd.end)//判断是否超过之前存储的数
  415.                         {
  416.                                 if ((send_mode & SEND_SEND_FILE))//开始接收到读取命令需要先发送个start
  417.                                 {
  418.                                         UART_PutStr(USART1,"#Start\r\n");
  419.                                         send_mode &= ~SEND_READ_FILE;
  420.                                 }
  421.                                 memset(redata,0,WRITE_SIZE);//清 0
  422.                                 SpiFlashRead((unsigned char *)redata,(((long)cur_item.cur_num)<<WRITE_BIT_DEPTH),WRITE_SIZE);//读取信息
  423. #if DEBUG
  424.                                 sprintf(buf,"%d\r\n",cur_item.cur_num);
  425.                                 UART_PutStr(USART1,buf);
  426. #endif
  427.                                 UART_PutStr(USART1,redata);//发送
  428.                                 cur_item.cur_num++;
  429.                         }
  430.                         else//否则
  431.                         {
  432.                                 if (cur_item.cur_num > 0)
  433.                                         UART_PutStr(USART1,"#End\r\n");//发送结束字符串
  434.                                 flag_read = 0;
  435.                         }
  436.                         send_mode = 0;//请 0
  437.                 }       
  438.                 if (flag_online_run)
  439.                 {
  440.                         if ((send_mode & SEND_CC) != 0  || cur_item.cur_num == motor_one_cmd.start)//如果当前需要更新舵机命令
  441.                         {
  442.                                 if (cur_item.tuoji_count > 0)//脱机次数没结束
  443.                                 {
  444.                                         if (cur_item.cur_num < motor_one_cmd.end)//判断是否读取结束
  445.                                         {
  446.                                                 SpiFlashRead((unsigned char *)redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//读取命令
  447.                                                 flag_RecFul = 1;//标志位为1，
  448.                                                 cur_item.cur_num++;//
  449.                                         }
  450.                                         else//执行玩一遍
  451.                                         {
  452.                                                 cur_item.cur_num = motor_one_cmd.start;
  453.                                                 cur_item.tuoji_count--;//减一
  454.                                         }
  455.                                 }
  456.                                 else//执行完成
  457.                                 {
  458.                                         flag_online_run = 0;
  459.                                         if (flag_connect_run)//如果上位机选择执行的功能，需要发送 AGF作为结束
  460.                                         {
  461.                                                 UART_PutStr(USART1,"#AGF\r\n");
  462.                                                 flag_connect_run = 0;
  463.                                         }
  464.                                 }
  465.                                 //读取数据
  466.                         }
  467.                 }
  468.         }
  469.         else//脱机
  470.         {
  471.                 if (file_list_count < 0)
  472.                 {
  473.                         return;
  474.                 }
  475.                 if (cur_item.tuoji_count > 0)
  476.                 {
  477.                         if ((send_mode & SEND_CC) != 0  || cur_item.cur_num == motor_one_cmd.start)//如果当前需要更新舵机命令
  478.                         {
  479.                                 if (cur_item.cur_num < motor_one_cmd.end)//判断是否读取结束
  480.                                 {
  481.                                         SpiFlashRead((unsigned char *)redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//读取命令
  482.                                         flag_RecFul = 1;//标志位为1，
  483.                                         cur_item.cur_num++;//
  484.                                 }
  485.                                 else//执行玩一遍
  486.                                 {
  487.                                         cur_item.cur_num = motor_one_cmd.start;
  488.                                         cur_item.tuoji_count--;//减一
  489.                                 }
  490.                                 //读取数据
  491.                         }
  492.                 }
  493.                 else
  494.                 {
  495.                         ReadOneCmdInfor(file_list[cur_item.file_num]);
  496.                         file_list_count--;
  497.                         cur_item.file_num++;
  498.                 }
  499.         }
  500.                
  501.        
  502. }
  503. /***************************************************************************************************************
  504. 函 数 名：发送串口状态信息
  505. 功能描述：根据状态标志位发送相应的信息
  506. 输入参数：无  
  507. 返 回 值：无  
  508. 备    注：
  509. ****************************************************************************************************************/       
  510. void SendUartState(void)
  511. {
  512.   char buf[40] = {0};
  513.         unsigned char read_motor_num = 0;
  514.         unsigned int i = 0;
  515.         static int count = 0;
  516.         if (send_mode)//如果有状态需要发送
  517.         {
  518.                 if (send_mode & SEND_A) //发送A
  519.                 {
  520.                          UART_PutChar(USART1,'A');
  521.                         send_mode &= ~SEND_A;//清状态
  522.                 }
  523.                 if (send_mode & SEND_CC) //发送CC
  524.                 {
  525.                         UART_PutStr(USART1,"#CC\r\n");
  526.                         send_mode &= ~SEND_CC;
  527.                 }
  528.                 if (send_mode & SEND_DOWN_OK)//发送下载ok的状态字符串
  529.                 {
  530.                         sprintf(buf,"#Down+OK+%d\r\n",(int)motor_data.filecount-1);
  531.                         UART_PutStr(USART1,buf);
  532.                         send_mode &= ~SEND_DOWN_OK;
  533. #if DEBUG
  534.                         sprintf(buf,"%d\r\n",(uint)motor_data.filecount);
  535.                         UART_PutStr(USART1,buf);
  536. #endif
  537.                 }
  538.                 if (send_mode & SEND_START_OK)//发送连接时候的字符串
  539.                 {
  540.                         UART_PutStr(USART1,"#Veri+UART+OK+20160906+176\r\n");
  541.                         send_mode &= ~SEND_START_OK;
  542.                 }
  543.                 if (send_mode & SEND_READ_FILE)//发送读取文件的时候字符串
  544.                 {
  545.                         if (motor_data.filecount > 0)//如果保存的有舵机命令
  546.                         {
  547.                                 //#Name:1.txt--Size:48--Name:2.txt--Size:190--Name:desktop.ini--Size:531--
  548.                                 UART_PutStr(USART1,"#");//发送
  549.                                 for (i = 0; i < motor_data.filecount;i++)
  550.                                 {
  551.                                         if (motor_data.file_flag[i] == 1)
  552.                                         {
  553.                                                 ReadOneCmdInfor(i);
  554.                                                 if (motor_one_cmd.end - motor_one_cmd.start <= 0)
  555.                                                 {
  556.                                                         motor_data.file_flag[i] = 0;
  557.                                                         WriteMoterInfor();
  558. #if DEBUG
  559.                                                         sprintf(buf,"E=%d S=%d",motor_one_cmd.end, motor_one_cmd.start);
  560. #endif
  561.                                                 }
  562.                                                 else
  563.                                                 {
  564.                                                         sprintf(buf,"Name:%d.txt--Size:%d--",i,motor_one_cmd.end - motor_one_cmd.start);//获取命令个数
  565.                                                         UART_PutStr(USART1,buf);//发送
  566.                                                 }
  567.                                         }

  569.                                 }
  570.                                 UART_PutStr(USART1,"\r\n");                               
  571.                         }
  572.                         else
  573.                         {
  574.                                 sprintf(buf,"#\r\n",motor_data.sum);
  575.                                 UART_PutStr(USART1,buf);
  576.                         }
  577.                         send_mode &= ~SEND_READ_FILE;
  578.                 }
  579.                 if (send_mode & SEND_SET_OFFLINE_OK)//设置脱机运行次数
  580.                 {
  581.                         WriteOneCmdInfor(motor_one_cmd.cur_file_num);//保存
  582.                         UART_PutStr(USART1,"#Enable+OK...\r\n");
  583.                         send_mode &= ~SEND_SET_OFFLINE_OK;
  584.                 }
  585.                 if (send_mode & SEND_SET_DISABLEOFFLINE_OK)//禁止脱机运行
  586.                 {
  587.                         for (i = 0; i < motor_data.filecount;i++)
  588.                         {
  589.                                 if (motor_data.file_flag[i] == 1)
  590.                                 {
  591.                                         ReadOneCmdInfor(i);
  592.                                         motor_one_cmd.tuoji_count = 0;
  593.                                         WriteOneCmdInfor(i);
  594.                                 }

  596.                         }
  597.                         WriteMoterInfor();
  598.                         UART_PutStr(USART1,"#Disable+OK...\r\n");
  599.                         send_mode &= ~SEND_SET_DISABLEOFFLINE_OK;
  600.                 }
  601.                 if (send_mode & SEND_SET_ONLINE_OK)//发送联机运行状态
  602.                 {
  603.                         UART_PutStr(USART1,"#OK\r\n");
  604.                         sprintf(buf,"#1GC%d\r\n",tuoji_count);
  605.                         UART_PutStr(USART1,buf);
  606.                         UART_PutStr(USART1,"#LP=0\r\n");
  607.                         send_mode &= ~SEND_SET_ONLINE_OK;
  608.                         flag_connect_run = 1;
  609.                 }
  610.                 if (send_mode & SEND_SET_DELETE_ONE_FILE_OK)//发送删除文件命令
  611.                 {
  612.                         //cur_item.tuoji_count = 0;
  613.                         if (cur_item.delete_num < motor_data.filecount)
  614.                         {
  615.                                 motor_data.file_flag[cur_item.delete_num] = 0;
  616.                                 WriteMoterInfor();
  617.                                 updata_file_list();
  618.                                 if (cur_item.delete_num  == motor_data.filecount-1)
  619.                                 {
  620.                                         motor_data.filecount = file_last_num + 1;
  621.                                         if (file_last_num == -1)
  622.                                         {
  623.                                                 motor_data.sum = 0;
  624.                                         }
  625.                                         else
  626.                                         {
  627.                                                 ReadOneCmdInfor(file_last_num);
  628.                                                 motor_data.sum = motor_one_cmd.end;
  629.                                                 motor_data.sum = (((long int)(motor_data.sum) >>4)<<4) + (1<<4);
  630.                                         }
  631.                                         WriteMoterInfor();
  632.                                 }
  633.                                 updata_file_list();               
  634.                                 UART_PutStr(USART1,"#FDel+OK\r\n");
  635.                         }
  636.                         send_mode &= ~SEND_SET_DELETE_ONE_FILE_OK;
  637.                        
  638.                 }
  639.                 if (send_mode & SEND_SET_DELETE_ALL_FILE_OK)//发送擦除所有文件的命令
  640.                 {
  641.                         UART_PutStr(USART1,"#Format+Start\r\n");
  642.                         SpiFlashEraseChip();
  643.                         cur_item.tuoji_count = 0;
  644.                         motor_data.sum = 0;
  645.                         motor_data.filecount = 0;
  646.                         memset(motor_data.file_flag,0,sizeof(motor_data.file_flag));
  647.                         WriteMoterInfor();
  648.                         UART_PutStr(USART1,"#Format+OK\r\n");
  649.                         send_mode &= ~SEND_SET_DELETE_ALL_FILE_OK;
  650.                         updata_file_list();
  651.                 }
  652.                 if (send_mode & SEND_SET_PS2_OK)
  653.                 {
  654.                         UART_PutStr(USART1,"#PS2+OK...\r\n");
  655.                         send_mode &= ~SEND_SET_PS2_OK;
  656.                 }
  657.                
  658. #define MATHION_HAND_NUM 20
  659.                 if (send_mode & SEND_SET_READ_UART_MOTOR_ANGLE)
  660.                 {
  661.                         UART_PutStr(USART1,"\r\n");
  662.                         if (cur_item.read_num < MATHION_HAND_NUM)
  663.                         {
  664.                                 if (flag_uart2_rev)
  665.                                 {
  666.                                         read_motor_num = atoi(uart2_buf + 1);
  667.                                         if (read_motor_num == cur_item.read_num)
  668.                                         {
  669.                                                
  670.                                                 i = atoi(uart2_buf+5);
  671.                                                 sprintf(uart2_buf,"#%dP%d",(int)read_motor_num,i);
  672.                                                 UART_PutStr(USART1,uart2_buf);
  673.                                                 cur_item.read_num++;
  674.                                                 buf[0] = '#';
  675.                                                 buf[1] = cur_item.read_num / 100 % 10 + 0x30;
  676.                                                 buf[2] = cur_item.read_num / 10 % 10 + 0x30;
  677.                                                 buf[3] = cur_item.read_num % 10 + 0x30;
  678.                                                 buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
  679.                                                 UART_PutStr(USART2,buf);
  680.                                                
  681.                                         }
  682.                                         flag_uart2_rev = 0;
  683.                                         count = 0;
  684.                                        
  685.                                 }
  686.                                 else
  687.                                 {
  688.                                         count++;
  689.                                         if (count >= 10)
  690.                                         {
  691.                                                 cur_item.read_num++;
  692.                                                 buf[0] = '#';
  693.                                                 buf[1] = cur_item.read_num / 100 % 10 + 0x30;
  694.                                                 buf[2] = cur_item.read_num / 10 % 10 + 0x30;
  695.                                                 buf[3] = cur_item.read_num % 10 + 0x30;
  696.                                                 buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
  697.                                                 UART_PutStr(USART2,buf);
  698.                                                 flag_uart2_rev = 0;
  699.                                                 flag_uart2_rev_time_out = 0;
  700.                                                 count = 0;
  701.                                         }
  702.                                 }
  703.                         }
  704.                         else
  705.                         {
  706.                                 send_mode &= ~SEND_SET_READ_UART_MOTOR_ANGLE;
  707.                                 cur_item.read_num= 0;
  708.                                 UART_PutStr(USART1,"\r\n");
  709.                         }
  710.                 }
  711.         }
  712.         if (send_mode & SEND_SET_SET_UART_MOTOR_PULK)
  713.         {
  714.                 if (cur_item.pulk_num < MATHION_HAND_NUM)
  715.                 {
  716.                         count++;
  717.                         if (count >= 20)
  718.                         {
  719.                                 sprintf(uart2_buf,"#%dPULK\r\n",(int)cur_item.pulk_num);
  720.                                 UART_PutStr(USART2,uart2_buf);
  721. #if DEBUG
  722.                                 UART_PutStr(USART1,uart2_buf);
  723. #endif
  724.                                 cur_item.pulk_num++;
  725.                                 count = 0;
  726.                         }
  727.                 }
  728.                 else
  729.                 {
  730.                         send_mode &= ~SEND_SET_SET_UART_MOTOR_PULK;
  731.                 }
  732.         }
  733.         if (send_mode & SEND_SET_SET_UART_MOTOR_ANGLE)
  734.         {
  735.                 if (cur_item.pulk_num < MATHION_HAND_NUM)
  736.                 {
  737.                         count++;
  738.                         if (count >= 5)
  739.                         {
  740.                                 sprintf(uart2_buf,"#%dPMOD%d\r\n",(int)cur_item.pulk_num,(int)cur_item.angle_mode);
  741.                                 UART_PutStr(USART2,uart2_buf);
  742. #if DEBUG
  743.                                 UART_PutStr(USART1,uart2_buf);
  744. #endif
  745.                                 cur_item.pulk_num++;
  746.                                 count = 0;
  747.                         }
  748.                 }
  749.                 else
  750.                 {
  751.                         sprintf(buf,"#255PMOD%d+0K...\r\n",(int)cur_item.angle_mode);
  752.                         UART_PutStr(USART1,buf);
  753.                         send_mode &= ~SEND_SET_SET_UART_MOTOR_ANGLE;
  754.                         flag_uart2_rev = 0;
  755.                 }
  756.         }
  757.         if (send_mode & SEND_SET_BEEP_ON)
  758.         {
  759.                 ReadMoterInfor();
  760.                 motor_data.beep_mode = 1;
  761.                 //beep_mode = 1;
  762.                 WriteMoterInfor();
  763.                 UART_PutStr(USART1,"#FMQENABLE+OK...\r\n");
  764.                 send_mode &= ~SEND_SET_BEEP_ON;
  765.         }
  766.         if (send_mode & SEND_SET_BEEP_OFF)
  767.         {
  768.                 ReadMoterInfor();
  769.                 motor_data.beep_mode = 0;
  770.                 //beep_mode = 0;
  771.                 WriteMoterInfor();
  772.                 UART_PutStr(USART1,"#FMQDISABLE+OK...\r\n");
  773.                 send_mode &= ~SEND_SET_BEEP_OFF;
  774.         }               
  775. }


  778. /***************************************************************************************************************
  779. 函 数 名：作业初位置，末尾置更新函数  
  780. 功能描述：从缓存中取一个新的目标位置替换原来的目标位置，原来的目标位置变为新的初位置，一次更替
  781.                 ：有效的数据是插补增量，和插补次数，知道这两个量，和当前初位置即可
  782. 备    注： 先进先出，循环访问       
  783. ****************************************************************************************************************/       
  784. void change(void)
  785. {       
  786.         unsigned char s;
  787.         if(line>0)                                   //缓存中有数据
  788.         {  
  789.                 line--;                                   //执行一行
  790.                 if(line<5)                        //缓存允许放入新的数据                       
  791.                         flag_in=1;
  792.                 point_now++;                //取数位置更新
  793.                 point_aim++;
  794.                
  795.                 if(point_aim==7)
  796.                            point_aim=0;
  797.                 if(point_now==7)
  798.                            point_now=0;
  799.                 n=pos[point_aim][0]*4/8;        //计算新的插补次数       

  801.                 for(s=1;s<MOTOR_NUM;s++)        //计算新的插补增量
  802.                 {
  803.                  if(pos[point_aim][s]>pos[point_now][s])
  804.                         {
  805.                                    dp=pos[point_aim][s]-pos[point_now][s];
  806.                                    dp0[s]=dp/n;
  807.                         }
  808.                     if(pos[point_aim][s]<=pos[point_now][s])
  809.                         {
  810.                                 dp=pos[point_now][s]-pos[point_aim][s];
  811.                                 dp0[s]=dp/n;
  812.                                 dp0[s]=-dp0[s];
  813.                         }
  814.                                 //UART_Put_Inf("dp:",dp);
  815.                         // UART_Put_Inf("n",n);
  816.                        
  817.            }
  818.                 m=0;                                          //m清0
  819.                 flag_stop=0;                          //产生了新的目标位置，停止标志清零

  821.         }
  822.         else                                                  //没有缓存数据，即line==0
  823.         {
  824.                 flag_out=0;                                //缓存中没有数据
  825.         }
  826. }
  827. /***************************************************************************************************************
  828. 函 数 名：vpwm()  
  829. 功能描述：数据插补，插补时间间隔为20/12ms，由timer0控制，使舵机平滑实现速度控制
  830.                 ：另一个功能是执行完一行后去更新下一行数据，即调用change()
  831. 备    注：
  832. ****************************************************************************************************************/       
  833. void vpwm(void)                 
  834. {
  835.         unsigned char j=0;
  836.         unsigned char how=0;
  837.   static unsigned char flag_how;
  838.         static unsigned char flag_Tover;

  840.         if(flag_stop==1)                                           //一行作业全部完成
  841.         {       
  842.                 if(flag_out==1)                                         //缓冲数组中有数据
  843.                 {
  844.                         change();                                        //更新行
  845.                 }
  846.         }
  847.         else                                                                //不是一行数据全部完成，处于中间插补阶段
  848.         {
  849.                 m++;                                                        //用来累加插补过的次数
  850.                 if(m==n)                                                //n是本行作业要插补的总次数
  851.                 {
  852.                   flag_Tover=1;                                //一行数据的执行时间已经完成
  853.                         send_mode |= SEND_CC;
  854.                 }
  855.                 for(j=1;j<MOTOR_NUM;j++)
  856.                 {
  857.                         if(abs(CPWM[j]-pos[point_aim][j])<5)
  858.                         {                                                           //检测靠近终点位置
  859.                            how++;                                           //是，则累加一个
  860.                            CPWM[j]=pos[point_aim][j];//并且直接过度到终点位置
  861.                         }       
  862.                         else                                                //不靠近终点，继续插补
  863.                         {
  864.                                 CPWM[j]=pos[point_now][j]+m*dp0[j];
  865.                         }
  866.                 }
  867.                                                                                                                           
  868.                 if(how==MOTOR_NUM-1)
  869.                         flag_how=1;                                                  //16个舵机都到达终点
  870.                 how=0;
  871.                 if((flag_Tover==1)&&(flag_how==1))
  872.                 {                                                                //从插补次数，和脉宽宽度两方面都到达终点，本作业行完成
  873.                          flag_Tover=0;
  874.                          flag_how=0;
  875.                          flag_stop=1;
  876.                 }
  877.                
  878.          }
  879.         return;

  881. }



  885. /************************************************************************/

  887. #if 0
  888. void SerVor_Test(void)
  889. {
  890.                 CPWM[0]=700;CPWM[1]=700;CPWM[2]=700;CPWM[3]=700;CPWM[4]=700;CPWM[5]=700;
  891.                 Delay_ms(2000);
  892.                 CPWM[0]=2300;CPWM[1]=2300;CPWM[2]=2300;CPWM[3]=2300;CPWM[4]=2300;CPWM[5]=2300;
  893.                 Delay_ms(2000);
  894. }
  895. #endif
  896. #if 0
  897. void Ps2_Test(void)
  898. {
  899.                 key = PS2_DataKey();
  900.                 if(key!=0)
  901.             {
  902.                                 printf("  \r\n   %d  is  pressed  \r\n",key);
  903.             }
  904.      printf(" %5d %5d %5d %5d\r\n",PS2_AnologData(PSS_LX),PS2_AnologData(PSS_LY),
  905.                                               PS2_AnologData(PSS_RX),PS2_AnologData(PSS_RY) );
  906.     Delay_ms(1000);   
  907. }
  908. #endif
  909. #if 1
  910. void Flash_Test(void)
  911. {
  912.                 while(SpiFlashReadID()!=W25Q64)                                                        //检测不到W25Q64
  913.         {
  914.                 UART_PutStr (USART1,"Not Found W25Q64\r\n");
  915.         }
  916.         UART_PutStr (USART1," Found W25Q64\r\n");
  917.         SpiFlashWrite((u8*)TEXT_Buffer,FLASH_SIZE-100,SIZE);                //从倒数第100个地址写入数据
  918.         UART_PutStr (USART1,"Flash Write Success\r\n");
  919.         SpiFlashRead(datatemp,FLASH_SIZE-100,SIZE);          //读取写入flash中的数据
  920.         UART_PutStr (USART1,datatemp);
  921.         UART_PutStr (USART1,"\r\n Flash Read Success\r\n");
  922.         while(1);
  923. }
  924. #endif
复制代码

所有资料51hei提供下载:
舵机控制器STM32源程序V1.rar (340.32 KB, 下载次数: 43)


评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励！

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:366809 发表于 2019-3-6 09:20 | 只看该作者
楼主有QQ吗，下载了你的程序有一些疑问想请教
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表