久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 18643|回復(fù): 18
打印 上一主題 下一主題
收起左側(cè)

51單片機(jī)的6自由度機(jī)械臂 16路舵機(jī)控制 源碼

  [復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:183114 發(fā)表于 2017-3-24 16:58 | 只看該作者 |只看大圖 回帖獎勵 |倒序瀏覽 |閱讀模式
這是我畢設(shè)的源碼 16路舵機(jī)控制實現(xiàn)6自由度機(jī)械臂控制，采用的是51單片機(jī)。


完整源碼下載：
6自由度手臂源代碼.rar (589.48 KB, 下載次數(shù): 344)


單片機(jī)主程序:
  1. /***************************************************************************************************************
  2. 文 件 名：main.c  
  3. 功能描述：
  4. 備    注： 16路舵機(jī)控制
  5. ****************************************************************************************************************/
  6. #include "STC15Fxxxx.H"  //STC15系列單片機(jī)
  7. #include <intrins.h>
  8. #include "UART.H"
  9. #include "timer.h"
  10. #include "util.h"
  11. #include "ps2/ps2.h"
  12. #include "flash/flash.h"//頭文件
  13. #include <string.h>
  14. #include <stdio.h>
  15. #include "LED/led.h"
  16. #include <string.h>
  17. #include <stdlib.h>
  18. #include "adc/adc.h"
  19. extern uint8 flag_p;
  20. extern bit flag_RecFul;
  21. //extern uchar KEY[9];
  22. uint16 pos[7][MOTOR_NUM]={ {1000,1500,1500,1500,1500,1500,1500,1500,1500},
  23.                                          {1000,1500,1500,1500,1500,1500,1500,1500,1500},
  24.                                          {1000,1500,1500,1500,1500,1500,1500,1500,1500},
  25.                                          {1000,500,500,500,500,500,500,500,500},
  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. };         //位置
  30. uint16 pwm[MOTOR_NUM]=    {1500,1500,1500,1500,1500,1500,1500,1500,1500};
  31. uint16 UartRec[MOTOR_NUM]={1500,1500,1500,1500,1500,1500,1500,1500,1500};
  32. uint8 redata[257] = {0};    // 定義接收數(shù)據(jù)變量數(shù)組
  33. uint8 line=0;                                        //緩存存入口與出口之間的距離，即當(dāng)前緩存中有多少個沒有執(zhí)行的數(shù)據(jù)
  34. uint8 point_now=0;                                //與point_aim一起標(biāo)記緩存出口位置，即取數(shù)位置
  35. uint8 point_aim=1;
  36. uint8 point_in=2;                                //標(biāo)記緩存入口位置，即上一行數(shù)據(jù)存放位置
  37. bit flag_connect = 0;
  38. bit flag_stop=1;                                //表示一行執(zhí)行結(jié)束
  39. uint8 flag_vpwm=0;                                //表示到達(dá)了該更新pwm[]的時間
  40. bit flag_in=1;                                         //表示緩存中有空閑空間
  41. bit flag_out=0;                                        //表示緩存中有可執(zhí)行數(shù)據(jù)的標(biāo)志位
  42. bit flag_run_ready=0;                        //表示有要要放入緩存的EErom數(shù)據(jù)
  43. uint16 n=1000;                                        //用來計算需要建立多少個中間數(shù)據(jù)
  44. uint16 m=1;                                                //用來累計已經(jīng)執(zhí)行了多少中間數(shù)據(jù)
  45. double dp;
  46. double dp0[MOTOR_NUM] = {0};                                        //插補(bǔ)增量
  47. bit flag_download = 0;//判斷是否下載
  48. bit flag_read = 0;// 讀取flash內(nèi)容，發(fā)送上位機(jī)
  49. bit flag_connect_run = 0;//連接上位機(jī)的執(zhí)行flash保存的命令
  50. bit flag_stop_download = 0;//停止下載
  51. bit flag_online_run = 0;
  52. bit flag_uart2_rev = 0;
  53. bit flag_uart2_rev_time_out = 0;
  54. bit flag_ps2_rev = 0;
  55. bit flag_read_adc = 0;
  56. unsigned long send_mode = 0;//當(dāng)前串口接收到命令狀態(tài)存儲
  57. MotorData motor_data;//舵機(jī)總信息
  58. MotorOneCmd motor_one_cmd;//單個命令
  59. CurrentItem cur_item;
  60. uint16 tuoji_count = 0;//脫機(jī)執(zhí)行次數(shù)
  61. bit flag_scan_ps2 = 0;
  62. uint8 error = 0;
  63. uchar file_list[MAX_SUPPORT_FILE_SAVE] = {0};
  64. int file_list_count = 0;
  65. int file_last_num = 0;
  66. char ps2_buf[120] = {0};
  67. char uart2_buf[50] = {0};
  68. uint cur_count = 0;
  69. uchar ad_value = 0;
  70. uchar beep_mode = 1;
  71. uchar key_bak;
  72. uchar ps2_key;
  73. uchar ps2_mode=0;

  75. void updata_file_list()
  76. {
  77.         uchar i = 0;
  78.         uchar j = 0;
  79.         file_last_num = -1;
  80.         ReadMoterInfor();
  81.         for (i = 0; i < motor_data.filecount; i++)
  82.         {
  83.                 if (motor_data.file_flag[i] == 1)
  84.                 {
  85.                         file_list[j] = i;
  86.                         j++;
  87.                         file_last_num = i;
  88.                 }
  89.         }
  90.         file_list_count = j;
  91. }
  92. void InitMotor()
  93. {
  94.         ReadMoterInfor();//讀取舵機(jī)控制信息
  95.         updata_file_list();
  96.         memset(&cur_item,0,sizeof(cur_item));
  97.         beep_mode = motor_data.beep_mode;
  98. }

  100. /***************************************************************************************************************
  101. 函 數(shù) 名：主函數(shù)  
  102. 功能描述：入口函數(shù) ，進(jìn)行各種初始化配置
  103. 輸入?yún)?shù)：無  
  104. 返 回 值：無  
  105. 備    注：
  106. ****************************************************************************************************************/       
  107. void main(void)
  108. {
  109.         uint temp = 0;        
  110.         P0M1=0x00;                                   //設(shè)置P0口為強(qiáng)推挽輸出模式
  111.         P0M0=0xFF;
  112.        
  113.         P1M1|=0x80;
  114.         P1M0|=0xc8;
  115.        
  116.         P5M1|=0x00;
  117.   P5M0|=0x2d;
  118.        
  119.         P4M0|=0x20;
  120.         P4M1|=0x00;
  121.        
  122.         Timer_init();          //定時器初始化
  123.         Timer0(31);                  //定時任意值，啟動定時器，進(jìn)入定時器循環(huán)
  124. #if MOTOR_NUM > 9
  125.         Timer1(30);
  126. #endif
  127.         SpiFlashInit();//初始化flash
  128.         while((temp = SpiFlashReadID())!=W25Q64)LED_ALL_ON();//判斷flash有沒有接錯
  129.         LED_ALL_OFF();
  130.         InitMotor();
  131.         UART1_Init();          //串口1初始化
  132.         UART2_Init();  //串口2初始化
  133. #if PS_SUPPORT
  134.         Timer3_init();
  135. #endif       
  136.         InitADC(7);
  137.         BEEP_On_Or_OFF();
  138.         while(1)
  139.         {
  140.                 if(flag_vpwm==1)                  
  141.                 {       
  142.                         vpwm();                                        //更新pwm[]數(shù)組
  143.                         flag_vpwm=0;               
  144.                 }
  145.                 if( flag_RecFul==1)                   //串口接受完一條指令
  146.                 {
  147.                         DealRec();                                 //處理串口緩存中的數(shù)據(jù)
  148.                         flag_RecFul=0;
  149.                 }
  150.                 GetOneMotorCMD();//獲取一個命令
  151.                 SendUartState();//發(fā)送狀態(tài)信息
  152. #if PS_SUPPORT
  153.                 scan_ps2();
  154. #endif
  155.                 LED_State();
  156.                 Check_Power();
  157.           cur_count++;
  158.         }
  159. }
  160. void Check_Power()
  161. {
  162.         if ((cur_count % 800) == 0)
  163.         {
  164.                 StartADC(7);
  165.                 //cur_count=0;
  166.         }
  167.         if (flag_read_adc)
  168.         {
  169.                 flag_read_adc = 0;
  170.                 //UART_Put_Inf("adc:",ad_value);
  171.                 if (ad_value > 46)//根據(jù)電壓粗略估計
  172.                 {
  173.                         //UART_Put_Inf("adc:",ad_value);
  174.                   BEEP_OFF();
  175.                 //        LED2_OFF();
  176.                 }
  177.                 else
  178.                 {
  179.                                 //UART_Put_Inf("adc1111:",ad_value);
  180.                                 BEEP=~BEEP;
  181.                                 LED1=0;
  182.                                 LED2=0;
  183.                                 LED3=0;
  184.                 }
  185.                
  186.         }
  187. }
  188. void LED_State()
  189. {
  190.         uint error_count =  0;
  191.         if (error != 0)
  192.         {
  193.                 if (error & ERROR_FLASH_FULL)
  194.                 {
  195.                         error_count = 100;
  196.                 }
  197.                 if (error & ERROR_FLASH_FILE_FULL)
  198.                 {
  199.                         error_count += 100;
  200.                 }
  201.                 if (error & ERROR_FLASH_WRITE)
  202.                 {
  203.                         error_count += 100;
  204.                 }
  205.                 if (error & ERROR_FLASH_WRITE1)
  206.                 {
  207.                         error_count += 100;
  208.                 }
  209.                 if ((cur_count % error_count) == 0)//判斷flash是否正確
  210.                 {
  211.                         LED1_ON_OR_OFF();
  212.                 }
  213.         }
  214.        
  215.         if (flag_ps2_rev)
  216.         {
  217.                 LED1_ON();
  218.                 if ((cur_count % 100) == 0)
  219.                 {
  220.                         flag_ps2_rev = 0;
  221.                         LED1_OFF();
  222.                 }
  223.         }
  224.         if (flag_connect)
  225.         {
  226.                 LED3_ON();
  227.         }
  228.         else
  229.         {
  230.                 LED3_OFF();
  231.         }
  232. }
  233. void scan_ps2()
  234. {
  235.         int  kind = 0;
  236.         char *p = NULL;
  237.         char buf[15] = {0};
  238.         char i = 0;
  239.         if (flag_scan_ps2)//
  240.         {
  241.                 flag_scan_ps2 = 0;
  242.                 ps2_key=PS2_DataKey();
  243.                 ps2_mode=PS2_RedLight();
  244.         //UART_Put_Inf("mode",ps2_mode);       
  245.                 if(ps2_mode==0)
  246.                 {
  247.                         if(key_bak == ps2_key)return;
  248.                         key_bak=ps2_key;
  249.                         BEEP=~BEEP;
  250.                         switch(ps2_key)
  251.                         {
  252.                                 case PSB_PAD_UP:kind = 1;break;
  253.                                 case PSB_PAD_DOWN:kind = 2;break;
  254.                                 case PSB_PAD_LEFT:kind = 3;break;
  255.                                 case PSB_PAD_RIGHT:kind = 4;break;

  257.                                 case PSB_TRIANGLE:kind = 7;break;
  258.                                 case PSB_CROSS:kind = 8;break;
  259.                                 case PSB_PINK:kind = 9;break;
  260.                                 case PSB_CIRCLE:kind = 10;break;

  262.                                 case PSB_L1:kind = 6;break;
  263.                                 case PSB_L2:kind = 5;break;
  264.                                 case PSB_R1:kind = 12;break;
  265.                                 case PSB_R2:kind = 11;break;
  266.                                 default:break;
  267.                         }
  268.                                 if (kind != 0)
  269.                                 {               
  270.                                         flag_ps2_rev = 1;
  271.                                         flag_connect = 1;
  272.                                         SpiFlashRead(ps2_buf,(PS2_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(ps2_buf));
  273.                                         sprintf(buf,"%dK",kind);
  274.                                         //UART1_SendStr(buf);
  275.                                         p = strstr(ps2_buf,buf);
  276.                                         if (p != NULL)
  277.                                         {
  278.                                                 p = p + strlen(buf);
  279.                                                 while(i < 14 && *p != 0)
  280.                                                 {
  281.                                                         buf[i] = *p++;
  282.                                                         i++;
  283.                                                         if (*p == '#')
  284.                                                                 break;
  285.                                                 }
  286.                                                 if (i < 12)
  287.                                                 {
  288.                                                         buf[i] = '\r';
  289.                                                         buf[i+1] = '\n';
  290.                                                         memcpy(redata,buf,sizeof(buf));
  291.                                                         flag_RecFul = 1;
  292.                                                 }
  293.                                                 UART1_SendStr(redata);
  294.                                         }
  295.                                 }
  296.                         }
  297.                 else if(ps2_mode==1)//綠燈模式
  298.                 {
  299.                         switch(ps2_key)
  300.                                 {
  301.                                         case PSB_PAD_UP:pwm[1]+=10;if(pwm[1]>=2300) pwm[1]=2300;break;
  302.                                         case PSB_PAD_DOWN:pwm[1]-=10;if(pwm[1]<=700) pwm[1]=700;break;
  303.                                         case PSB_PAD_LEFT:pwm[2]+=10;if(pwm[2]>=2300) pwm[2]=2300;break;
  304.                                         case PSB_PAD_RIGHT:pwm[2]-=10;if(pwm[2]<=700) pwm[2]=700;break;
  305.                
  306.                                         case PSB_TRIANGLE:pwm[3]+=10;if(pwm[3]>=2300) pwm[3]=2300;break;
  307.                                         case PSB_CROSS:pwm[3]-=10;if(pwm[3]<=700) pwm[3]=700;break;
  308.                                         case PSB_PINK:pwm[4]+=10;if(pwm[4]>=2300) pwm[4]=2300;break;
  309.                                         case PSB_CIRCLE:pwm[4]-=10;if(pwm[4]<=700) pwm[4]=700;break;

  311.                                         case PSB_L1:pwm[5]+=10;if(pwm[5]>=2300) pwm[5]=2300;break;
  312.                                         case PSB_L2:pwm[5]-=10;if(pwm[5]<=700)  pwm[5]=700;break;
  313.                                         case PSB_R1:pwm[6]+=10;if(pwm[6]>=2300) pwm[6]=2300;break;
  314.                                         case PSB_R2:pwm[6]-=10;if(pwm[6]<=700)  pwm[6]=700;break;
  315.                                         default:break;
  316.                                 }
  317.                 }
  318.         }
  319. }
  320. /***************************************************************************************************************
  321. 函 數(shù) 名：從flash讀取舵機(jī)總的信息
  322. 功能描述：初始化舵機(jī)控制信息
  323. 輸入?yún)?shù)：無  
  324. 返 回 值：無  
  325. 備    注：
  326. ****************************************************************************************************************/       
  327. void ReadMoterInfor()
  328. {
  329.         memset(&motor_data,0,sizeof(motor_data));//清 0
  330.         SpiFlashRead((char *)&motor_data,(CMD_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_data));//讀取信息
  331.         if (motor_data.CRC1 != 0x12345678 || motor_data.sum < 0 || motor_data.duoji_count  > MOTOR_NUM)//判斷信息存儲是否有錯
  332.         {
  333.                 memset(&motor_data,0,sizeof(motor_data));
  334.                 //memset(&cur_item,0,sizeof(cur_item));
  335.         }
  336.         else//正常信息
  337.         {
  338.                 //UART1_SendOneChar(motor_data.sum + 0x30);
  339.                 //cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脫機(jī)運(yùn)行次數(shù)
  340.                 //cur_item.cur_num = 0;//清 0
  341.         }
  342.                
  343. }
  344. void ReadOneCmdInfor(unsigned int addr)
  345. {
  346.         memset(&motor_one_cmd,0,sizeof(motor_one_cmd));//清 0
  347.         SpiFlashRead((char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd));//讀取信息
  348.         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)//判斷信息存儲是否有錯
  349.         {
  350.                 memset(&motor_one_cmd,0,sizeof(motor_one_cmd));
  351.         }
  352.         else//正常信息,以后留著驗證用的
  353.         {
  354.                
  355.                
  356.                 cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脫機(jī)運(yùn)行次數(shù)
  357.                 cur_item.cur_num = motor_one_cmd.start;
  358.                 //UART1_SendOneChar(motor_one_cmd.tuoji_count + 0x30);
  359.                 //cur_item.cur_num = 0;//清 0
  360.         }
  361.                
  362. }
  363. /***************************************************************************************************************
  364. 函 數(shù) 名：把舵機(jī)信息寫到flash中
  365. 功能描述：把舵機(jī)信息寫到flahs中
  366. 輸入?yún)?shù)：無  
  367. 返 回 值：無  
  368. 備    注：
  369. ****************************************************************************************************************/       
  370. void WriteMoterInfor()
  371. {
  372.         uchar temp = 0;
  373.         motor_data.CRC1 = 0x12345678;//校驗碼
  374.         motor_data.duoji_count = MOTOR_NUM-1;
  375.         temp = motor_data.filecount;
  376.         SpiFlashEraseSector(CMD_FLASH_ADDR >> 4);//擦除以前存儲的信息
  377.         SpiFlashWrite((char *)&motor_data,CMD_FLASH_ADDR<<WRITE_BIT_DEPTH,sizeof(motor_data)); //寫入flash
  378.         ReadMoterInfor();
  379.         if (temp != motor_data.filecount)
  380.         {
  381.                 error |= ERROR_FLASH_WRITE;
  382.         }
  383.         else
  384.         {
  385.                 error &= ~ERROR_FLASH_WRITE;
  386.         }
  387. }
  388. void WriteOneCmdInfor(unsigned int addr)
  389. {
  390.         uchar temp = 0;
  391.         temp = motor_one_cmd.end;
  392.         if (((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) % 16 == 0)
  393.                 SpiFlashEraseSector(((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) >> 4);//擦除以前存儲的信息
  394.         SpiFlashWrite((char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd)); //寫入flash
  395.         ReadOneCmdInfor(addr);
  396.         if (temp !=  motor_one_cmd.end)
  397.         {
  398.                 error |= ERROR_FLASH_WRITE1;
  399.         }
  400.         else
  401.         {
  402.                 error &= ~ERROR_FLASH_WRITE1;
  403.         }
  404. }
  405. /***************************************************************************************************************
  406. 函 數(shù) 名：獲取一串舵機(jī)的控制字符串
  407. 功能描述：處理從flash讀取的舵機(jī)控制字符串處理
  408. 輸入?yún)?shù)：無  
  409. 返 回 值：無  
  410. 備    注：
  411. ****************************************************************************************************************/       
  412. void GetOneMotorCMD()
  413. {
  414. #if DEBUG
  415.         uchar buf[20] = {0};
  416. #endif
  417.         if (flag_stop_download)//接收到了上位機(jī)的停止下載的命令
  418.         {
  419.                 flag_download = 0;//清楚下載狀態(tài)標(biāo)志位
  420.                 flag_stop_download = 0;
  421.                 flag_read = 0;
  422.                 if (motor_data.filecount < MAX_SUPPORT_FILE_SAVE)
  423.                 {
  424.                         updata_file_list();
  425.                         motor_data.sum = motor_one_cmd.end;
  426.                         motor_data.file_flag[motor_data.filecount] = 1;
  427.                         motor_one_cmd.cur_file_num = file_last_num + 1;
  428.                         motor_data.filecount = motor_one_cmd.cur_file_num + 1;
  429.                         error &= ~MAX_SUPPORT_FILE_SAVE;
  430. #if DEBUG
  431.                         sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
  432.                         UART1_SendStr(buf);
  433. #endif
  434.                         WriteMoterInfor();
  435.                         WriteOneCmdInfor(motor_one_cmd.cur_file_num);
  436.                         updata_file_list();
  437. #if DEBUG
  438.                        
  439.                         sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
  440.                         UART1_SendStr(buf);
  441. #endif
  442.                 }
  443.                 else
  444.                 {
  445.                         error |= MAX_SUPPORT_FILE_SAVE;
  446.                 }
  447.                 if        (!(error &(MAX_SUPPORT_FILE_SAVE | ERROR_FLASH_FULL)))
  448.                         send_mode |= SEND_DOWN_OK;//狀態(tài)位置為
  449.         }
  450.         if (flag_connect)//如果當(dāng)前跟上位機(jī)聯(lián)機(jī)狀態(tài)
  451.         {
  452.                 if (flag_read)//如果上位機(jī)讀取flash內(nèi)存儲的信息
  453.                 {               
  454.                         if (cur_item.cur_num < motor_one_cmd.end)//判斷是否超過之前存儲的數(shù)
  455.                         {
  456.                                 if ((send_mode & SEND_SEND_FILE))//開始接收到讀取命令需要先發(fā)送個start
  457.                                 {
  458.                                         UART1_SendStr("#Start\r\n");
  459.                                         send_mode &= ~SEND_READ_FILE;
  460.                                 }
  461.                                 memset(redata,0,WRITE_SIZE);//清 0
  462.                                 SpiFlashRead(redata,(((long)cur_item.cur_num)<<WRITE_BIT_DEPTH),WRITE_SIZE);//讀取信息
  463. #if DEBUG
  464.                                 sprintf(buf,"%d\r\n",cur_item.cur_num);
  465.                                 UART1_SendStr(buf);
  466. #endif
  467.                                 UART1_SendStr(redata);//發(fā)送
  468.                                 cur_item.cur_num++;
  469.                         }
  470.                         else//否則
  471.                         {
  472.                                 if (cur_item.cur_num > 0)
  473.                                         UART1_SendStr("#End\r\n");//發(fā)送結(jié)束字符串
  474.                                   flag_read = 0;
  475.                         }
  476.                         send_mode = 0;//請 0
  477.                 }       
  478.                 if (flag_online_run)
  479.                 {
  480.                         if ((send_mode & SEND_CC) != 0  || cur_item.cur_num == motor_one_cmd.start)//如果當(dāng)前需要更新舵機(jī)命令
  481.                         {
  482.                                 if (cur_item.tuoji_count > 0)//脫機(jī)次數(shù)沒結(jié)束
  483.                                 {
  484.                                         if (cur_item.cur_num < motor_one_cmd.end)//判斷是否讀取結(jié)束
  485.                                         {
  486.                                                 SpiFlashRead(redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//讀取命令
  487.                                                 flag_RecFul = 1;//標(biāo)志位為1，
  488.                                                 cur_item.cur_num++;//
  489.                                         }
  490.                                         else//執(zhí)行玩一遍
  491.                                         {
  492.                                                 cur_item.cur_num = motor_one_cmd.start;
  493.                                                 cur_item.tuoji_count--;//減一
  494.                                         }
  495.                                 }
  496.                                 else//執(zhí)行完成
  497.                                 {
  498.                                         flag_online_run = 0;
  499.                                         if (flag_connect_run)//如果上位機(jī)選擇執(zhí)行的功能，需要發(fā)送 AGF作為結(jié)束
  500.                                         {
  501.                                                 UART1_SendStr("#AGF\r\n");
  502.                                                 flag_connect_run = 0;
  503.                                         }
  504.                                 }
  505.                                 //讀取數(shù)據(jù)
  506.                         }
  507.                 }
  508.         }
  509.         else//脫機(jī)
  510.         {
  511.                 if (file_list_count < 0)
  512.                 {
  513.                         return;
  514.                 }
  515.                 if (cur_item.tuoji_count > 0)
  516.                 {
  517.                         if ((send_mode & SEND_CC) != 0  || cur_item.cur_num == motor_one_cmd.start)//如果當(dāng)前需要更新舵機(jī)命令
  518.                         {
  519.                                 if (cur_item.cur_num < motor_one_cmd.end)//判斷是否讀取結(jié)束
  520.                                 {
  521.                                         SpiFlashRead(redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//讀取命令
  522.                                         flag_RecFul = 1;//標(biāo)志位為1，
  523.                                         cur_item.cur_num++;//
  524.                                 }
  525.                                 else//執(zhí)行玩一遍
  526.                                 {
  527.                                         cur_item.cur_num = motor_one_cmd.start;
  528.                                         cur_item.tuoji_count--;//減一
  529.                                 }
  530.                                 //讀取數(shù)據(jù)
  531.                         }
  532.                 }
  533.                 else
  534.                 {
  535.                         ReadOneCmdInfor(file_list[cur_item.file_num]);
  536.                         file_list_count--;
  537.                         cur_item.file_num++;
  538.                 }
  539.         }
  540.                
  541.        
  542. }
  543. /***************************************************************************************************************
  544. 函 數(shù) 名：發(fā)送串口狀態(tài)信息
  545. 功能描述：根據(jù)狀態(tài)標(biāo)志位發(fā)送相應(yīng)的信息
  546. 輸入?yún)?shù)：無  
  547. 返 回 值：無  
  548. 備    注：
  549. ****************************************************************************************************************/       
  550. void SendUartState()
  551. {
  552.         uchar buf[40] = {0};
  553.         uchar read_motor_num = 0;
  554.         uint i = 0;
  555.         static int count = 0;
  556.         if (send_mode)//如果有狀態(tài)需要發(fā)送
  557.         {
  558.                 if (send_mode & SEND_A) //發(fā)送A
  559.                 {
  560.                         UART1_SendOneChar('A');
  561.                         send_mode &= ~SEND_A;//清狀態(tài)
  562.                 }
  563.                 if (send_mode & SEND_CC) //發(fā)送CC
  564.                 {
  565.                         UART1_SendStr("#CC\r\n");
  566.                         send_mode &= ~SEND_CC;
  567.                 }
  568.                 if (send_mode & SEND_DOWN_OK)//發(fā)送下載ok的狀態(tài)字符串
  569.                 {
  570.                         sprintf(buf,"#Down+OK+%d\r\n",(int)motor_data.filecount-1);
  571.                         UART1_SendStr(buf);
  572.                         send_mode &= ~SEND_DOWN_OK;
  573. #if DEBUG
  574.                         sprintf(buf,"%d\r\n",(uint)motor_data.filecount);
  575.                         UART1_SendStr(buf);
  576. #endif
  577.                 }
  578.                 if (send_mode & SEND_START_OK)//發(fā)送連接時候的字符串
  579.                 {
  580.                         UART1_SendStr("#Veri+UART+OK+20160906+176\r\n");
  581.                         send_mode &= ~SEND_START_OK;
  582.                 }
  583.                 if (send_mode & SEND_READ_FILE)//發(fā)送讀取文件的時候字符串
  584.                 {
  585.                         if (motor_data.filecount > 0)//如果保存的有舵機(jī)命令
  586.                         {
  587.                                 //#Name:1.txt--Size:48--Name:2.txt--Size:190--Name:desktop.ini--Size:531--
  588.                                 UART1_SendStr("#");//發(fā)送
  589.                                 for (i = 0; i < motor_data.filecount;i++)
  590.                                 {
  591.                                         if (motor_data.file_flag[i] == 1)
  592.                                         {
  593.                                                 ReadOneCmdInfor(i);
  594.                                                 if (motor_one_cmd.end - motor_one_cmd.start <= 0)
  595.                                                 {
  596.                                                         motor_data.file_flag[i] = 0;
  597.                                                         WriteMoterInfor();
  598. #if DEBUG
  599.                                                         sprintf(buf,"E=%d S=%d",motor_one_cmd.end, motor_one_cmd.start);
  600. #endif
  601.                                                 }
  602.                                                 else
  603.                                                 {
  604.                                                         sprintf(buf,"Name:%d.txt--Size:%d--",i,motor_one_cmd.end - motor_one_cmd.start);//獲取命令個數(shù)
  605.                                                         UART1_SendStr(buf);//發(fā)送
  606.                                                 }
  607.                                         }

  609.                                 }
  610.                                 UART1_SendStr("\r\n");                               
  611.                         }
  612.                         else
  613.                         {
  614.                                 sprintf(buf,"#\r\n",motor_data.sum);
  615.                                 UART1_SendStr(buf);
  616.                         }
  617.                         send_mode &= ~SEND_READ_FILE;
  618.                 }
  619.                 if (send_mode & SEND_SET_OFFLINE_OK)//設(shè)置脫機(jī)運(yùn)行次數(shù)
  620.                 {
  621.                         WriteOneCmdInfor(motor_one_cmd.cur_file_num);//保存
  622.                         UART1_SendStr("#Enable+OK...\r\n");
  623.                         send_mode &= ~SEND_SET_OFFLINE_OK;
  624.                 }
  625.                 if (send_mode & SEND_SET_DISABLEOFFLINE_OK)//禁止脫機(jī)運(yùn)行
  626.                 {
  627.                         for (i = 0; i < motor_data.filecount;i++)
  628.                         {
  629.                                 if (motor_data.file_flag[i] == 1)
  630.                                 {
  631.                                         ReadOneCmdInfor(i);
  632.                                         motor_one_cmd.tuoji_count = 0;
  633.                                         WriteOneCmdInfor(i);
  634.                                 }

  636.                         }
  637.                         WriteMoterInfor();
  638.                         UART1_SendStr("#Disable+OK...\r\n");
  639.                         send_mode &= ~SEND_SET_DISABLEOFFLINE_OK;
  640.                 }
  641.                 if (send_mode & SEND_SET_ONLINE_OK)//發(fā)送聯(lián)機(jī)運(yùn)行狀態(tài)
  642.                 {
  643.                         UART1_SendStr("#OK\r\n");
  644.                         sprintf(buf,"#%dGC%d\r\n",cur_item.file_num,tuoji_count);
  645.                         UART1_SendStr(buf);
  646.                         UART1_SendStr("#LP=0\r\n");
  647.                         send_mode &= ~SEND_SET_ONLINE_OK;
  648.                         flag_connect_run = 1;
  649.                 }
  650.                 if (send_mode & SEND_SET_DELETE_ONE_FILE_OK)//發(fā)送刪除文件命令
  651.                 {
  652.                         //cur_item.tuoji_count = 0;
  653.                         if (cur_item.delete_num < motor_data.filecount)
  654.                         {
  655.                                 motor_data.file_flag[cur_item.delete_num] = 0;
  656.                                 WriteMoterInfor();
  657.                                 updata_file_list();
  658.                                 if (cur_item.delete_num  == motor_data.filecount-1)
  659.                                 {
  660.                                         motor_data.filecount = file_last_num + 1;
  661.                                         if (file_last_num == -1)
  662.                                         {
  663.                                                 motor_data.sum = 0;
  664.                                         }
  665.                                         else
  666.                                         {
  667.                                                 ReadOneCmdInfor(file_last_num);
  668.                                                 motor_data.sum = motor_one_cmd.end;
  669.                                                 motor_data.sum = (((long int)(motor_data.sum) >>4)<<4) + (1<<4);
  670.                                         }
  671.                                         WriteMoterInfor();
  672.                                 }
  673.                                 updata_file_list();               
  674.                                 UART1_SendStr("#FDel+OK\r\n");
  675.                         }
  676.                         send_mode &= ~SEND_SET_DELETE_ONE_FILE_OK;
  677.                        
  678.                 }
  679.                 if (send_mode & SEND_SET_DELETE_ALL_FILE_OK)//發(fā)送擦除所有文件的命令
  680.                 {
  681.                         UART1_SendStr("#Format+Start\r\n");
  682.                         SpiFlashEraseChip();
  683.                         cur_item.tuoji_count = 0;
  684.                         motor_data.sum = 0;
  685.                         motor_data.filecount = 0;
  686.                         memset(motor_data.file_flag,0,sizeof(motor_data.file_flag));
  687.                         WriteMoterInfor();
  688.                         UART1_SendStr("#Format+OK\r\n");
  689.                         send_mode &= ~SEND_SET_DELETE_ALL_FILE_OK;
  690.                         updata_file_list();
  691.                 }
  692.                 if (send_mode & SEND_SET_PS2_OK)
  693.                 {
  694.                         UART1_SendStr("#PS2+OK...\r\n");
  695.                         send_mode &= ~SEND_SET_PS2_OK;
  696.                 }
  697.                
  698. #define MATHION_HAND_NUM 20
  699.                 if (send_mode & SEND_SET_READ_UART_MOTOR_ANGLE)
  700.                 {
  701.                         if (cur_item.read_num < MATHION_HAND_NUM)
  702.                         {
  703.                                 if (flag_uart2_rev)
  704.                                 {
  705.                                         read_motor_num = atoi(uart2_buf + 1);
  706.                                         if (read_motor_num == cur_item.read_num)
  707.                                         {
  708.                                                 i = atoi(uart2_buf+5);
  709.                                                 sprintf(uart2_buf,"#%dP%d",(int)read_motor_num,i);
  710.                                                 UART1_SendStr(uart2_buf);
  711.                                                 cur_item.read_num++;
  712.                                                 buf[0] = '#';
  713.                                                 buf[1] = cur_item.read_num / 100 % 10 + 0x30;
  714.                                                 buf[2] = cur_item.read_num / 10 % 10 + 0x30;
  715.                                                 buf[3] = cur_item.read_num % 10 + 0x30;
  716.                                                 buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
  717.                                                 UART2_SendStr(buf);
  718.                                                
  719.                                         }
  720.                                         flag_uart2_rev = 0;
  721.                                         count = 0;
  722.                                        
  723.                                 }
  724.                                 else
  725.                                 {
  726.                                         count++;
  727.                                         if (count >= 10)
  728.                                         {
  729.                                                 cur_item.read_num++;
  730.                                                 buf[0] = '#';
  731.                                                 buf[1] = cur_item.read_num / 100 % 10 + 0x30;
  732.                                                 buf[2] = cur_item.read_num / 10 % 10 + 0x30;
  733.                                                 buf[3] = cur_item.read_num % 10 + 0x30;
  734.                                                 buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
  735.                                                 UART2_SendStr(buf);
  736.                                                 flag_uart2_rev = 0;
  737.                                                 flag_uart2_rev_time_out = 0;
  738.                                                 count = 0;
  739.                                         }
  740.                                 }
  741.                         }
  742.                         else
  743.                         {
  744.                                 send_mode &= ~SEND_SET_READ_UART_MOTOR_ANGLE;
  745.                                 cur_item.read_num= 0;
  746.                                 UART1_SendStr("\r\n");
  747.                         }
  748.                 }
  749.         }
  750.         if (send_mode & SEND_SET_SET_UART_MOTOR_PULK)
  751.         {
  752.                 if (cur_item.pulk_num < MATHION_HAND_NUM)
  753.                 {
  754.                         count++;
  755.                         if (count >= 20)
  756.                         {
  757.                                 sprintf(uart2_buf,"#%dPULK\r\n",(int)cur_item.pulk_num);
  758.                                 UART2_SendStr(uart2_buf);
  759. #if DEBUG
  760.                                 UART1_SendStr(uart2_buf);
  761. #endif
  762.                                 cur_item.pulk_num++;
  763.                                 count = 0;
  764.                         }
  765.                 }
  766.                 else
  767.                 {
  768.                         send_mode &= ~SEND_SET_SET_UART_MOTOR_PULK;
  769.                 }
  770.         }
  771.         if (send_mode & SEND_SET_SET_UART_MOTOR_ANGLE)
  772.         {
  773.                 if (cur_item.pulk_num < MATHION_HAND_NUM)
  774.                 {
  775.                         count++;
  776.                         if (count >= 5)
  777.                         {
  778.                                 sprintf(uart2_buf,"#%dPMOD%d\r\n",(int)cur_item.pulk_num,(int)cur_item.angle_mode);
  779.                                 UART2_SendStr(uart2_buf);
  780. #if DEBUG
  781.                                 UART1_SendStr(uart2_buf);
  782. #endif
  783.                                 cur_item.pulk_num++;
  784.                                 count = 0;
  785.                         }
  786.                 }
  787.                 else
  788.                 {
  789.                         sprintf(buf,"#255PMOD%d+0K...\r\n",(int)cur_item.angle_mode);
  790.                         UART1_SendStr(buf);
  791.                         send_mode &= ~SEND_SET_SET_UART_MOTOR_ANGLE;
  792.                         flag_uart2_rev = 0;
  793.                 }
  794.         }
  795.         if (send_mode & SEND_SET_BEEP_ON)
  796.         {
  797.                 ReadMoterInfor();
  798.                 motor_data.beep_mode = 1;
  799.                 beep_mode = 1;
  800.                 WriteMoterInfor();
  801.                 UART1_SendStr("#FMQENABLE+OK...\r\n");
  802.                 send_mode &= ~SEND_SET_BEEP_ON;
  803.         }
  804.         if (send_mode & SEND_SET_BEEP_OFF)
  805.         {
  806.                 ReadMoterInfor();
  807.                 motor_data.beep_mode = 0;
  808.                 beep_mode = 0;
  809.                 WriteMoterInfor();
  810.                 UART1_SendStr("#FMQDISABLE+OK...\r\n");
  811.                 send_mode &= ~SEND_SET_BEEP_OFF;
  812.         }               
  813. }
  814. /***************************************************************************************************************
  815. 函 數(shù) 名：作業(yè)初位置，末尾置更新函數(shù)  
  816. 功能描述：從緩存中取一個新的目標(biāo)位置替換原來的目標(biāo)位置，原來的目標(biāo)位置變?yōu)樾碌某跷恢茫淮胃?br />
  817.                 ：有效的數(shù)據(jù)是插補(bǔ)增量，和插補(bǔ)次數(shù)，知道這兩個量，和當(dāng)前初位置即可
  818. 備    注： 先進(jìn)先出，循環(huán)訪問       
  819. ****************************************************************************************************************/       
  820. void change(void)
  821. {       
  822.         uchar s;
  823.         if(line>0)                                   //緩存中有數(shù)據(jù)
  824.         {  
  825.                 line--;                                   //執(zhí)行一行
  826.                 if(line<5)                        //緩存允許放入新的數(shù)據(jù)                       
  827.                         flag_in=1;
  828.                 point_now++;                //取數(shù)位置更新
  829.                 point_aim++;
  830.                
  831.                 if(point_aim==7)
  832.                            point_aim=0;
  833.                 if(point_now==7)
  834.                            point_now=0;
  835.                 n=pos[point_aim][0]*4/5;        //計算新的插補(bǔ)次數(shù)
  836.                 for(s=1;s<MOTOR_NUM;s++)        //計算新的插補(bǔ)增量
  837.                 {
  838.                  if(pos[point_aim][s]>pos[point_now][s])
  839.                         {
  840.                                    dp=pos[point_aim][s]-pos[point_now][s];
  841.                                    dp0[s]=dp/n;
  842.                         }
  843.                     if(pos[point_aim][s]<=pos[point_now][s])
  844.                         {
  845.                                 dp=pos[point_now][s]-pos[point_aim][s];
  846.                                 dp0[s]=dp/n;
  847.                                 dp0[s]=-dp0[s];
  848.                         }
  849.            }
  850.                 m=0;                                          //m清0
  851.                 flag_stop=0;                          //產(chǎn)生了新的目標(biāo)位置，停止標(biāo)志清零
  852.                
  853.         }
  854.         else                                                  //沒有緩存數(shù)據(jù)，即line==0
  855.         {
  856.                 flag_out=0;                                //緩存中沒有數(shù)據(jù)
  857.         }
  858. }
  859. /***************************************************************************************************************
  860. 函 數(shù) 名：vpwm()  
  861. 功能描述：數(shù)據(jù)插補(bǔ)，插補(bǔ)時間間隔為20/12ms，由timer0控制，使舵機(jī)平滑實現(xiàn)速度控制
  862.                 ：另一個功能是執(zhí)行完一行后去更新下一行數(shù)據(jù)，即調(diào)用change()
  863. 備    注：
  864. ****************************************************************************************************************/       
  865. void vpwm(void)                 
  866. {
  867.         uchar j=0;
  868.         uchar how=0;
  869.   static uchar flag_how;
  870.         static uchar flag_Tover;

  872.         if(flag_stop==1)                                           //一行作業(yè)全部完成
  873.         {       
  874.                 if(flag_out==1)                                         //緩沖數(shù)組中有數(shù)據(jù)
  875.                 {
  876.                         change();                                        //更新行
  877.                 }
  878.         }
  879.         else                                                                //不是一行數(shù)據(jù)全部完成，處于中間插補(bǔ)階段
  880.         {
  881.                 m++;                                                        //用來累加插補(bǔ)過的次數(shù)
  882.                 if(m==n)                                                //n是本行作業(yè)要插補(bǔ)的總次數(shù)
  883.                 {
  884.                   flag_Tover=1;                                //一行數(shù)據(jù)的執(zhí)行時間已經(jīng)完成
  885.                         send_mode |= SEND_CC;
  886.                 }
  887.                 for(j=1;j<MOTOR_NUM;j++)
  888.                 {
  889.                         if(abs(pwm[j]-pos[point_aim][j])<5)
  890.                         {                                                           //檢測靠近終點(diǎn)位置
  891.                            how++;                                           //是，則累加一個
  892.                            pwm[j]=pos[point_aim][j];//并且直接過度到終點(diǎn)位置
  893.                         }       
  894.                         else                                                //不靠近終點(diǎn)，繼續(xù)插補(bǔ)
  895.                                 pwm[j]=pos[point_now][j]+m*dp0[j];
  896.                 }        
  897.                 //UART_Put_Inf("pwm",pwm[1]);
  898.                 if(how==MOTOR_NUM-1)
  899.                         flag_how=1;                                                  //16個舵機(jī)都到達(dá)終點(diǎn)
  900.                 how=0;
  901.                 if((flag_Tover==1)&&(flag_how==1))
  902.                 {                                                                //從插補(bǔ)次數(shù)，和脈寬寬度兩方面都到達(dá)終點(diǎn)，本作業(yè)行完成
  903.                          flag_Tover=0;
  904.                          flag_how=0;
  905.                          flag_stop=1;
  906.                 }
  907.                
  908.          }
  909.         return;

  911. }

  913.                           
復(fù)制代碼


分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏13 分享淘帖 頂頂1 踩踩

相關(guān)帖子
回復(fù)

使用道具 舉報

沙發(fā)
ID:196638 發(fā)表于 2017-5-5 20:20 | 只看該作者
點(diǎn)個贊！
回復(fù)

使用道具 舉報

板凳
ID:183825 發(fā)表于 2017-8-15 14:03 | 只看該作者
kankan 看看看 學(xué)習(xí)學(xué)習(xí)！！謝謝！！
回復(fù)

使用道具 舉報

地板
ID:242730 發(fā)表于 2017-10-25 13:01 來自手機(jī) | 只看該作者
請問樓主用的是單片機(jī)控制舵機(jī)控制板嗎
回復(fù)

使用道具 舉報

5#
ID:253033 發(fā)表于 2018-3-9 22:09 | 只看該作者
學(xué)習(xí)學(xué)習(xí)
回復(fù)

使用道具 舉報

6#
ID:253033 發(fā)表于 2018-3-9 23:04 | 只看該作者
單用單片機(jī)可以嗎不用其他的東西？
回復(fù)

使用道具 舉報

7#
ID:384210 發(fā)表于 2018-8-8 15:12 | 只看該作者
新來初到，能下載嗎
回復(fù)

使用道具 舉報

8#
ID:362243 發(fā)表于 2018-8-27 10:37 | 只看該作者
你好，
只有程序嗎？
回復(fù)

使用道具 舉報

9#
ID:396582 發(fā)表于 2018-9-15 20:35 | 只看該作者
你好，請問這是用中斷輸出pwm控制的嗎，怎么實現(xiàn)同時控制幾個舵機(jī)啊。聽說程序模擬pwm波誤差大
回復(fù)

使用道具 舉報

10#
ID:312557 發(fā)表于 2018-11-11 14:45 | 只看該作者
學(xué)習(xí)下  看看看 學(xué)習(xí)學(xué)習(xí)！！謝謝！！
回復(fù)

使用道具 舉報

11#
ID:615962 發(fā)表于 2019-9-24 23:32 來自手機(jī) | 只看該作者
只用單片機(jī)么？
回復(fù)

使用道具 舉報

12#
ID:645882 發(fā)表于 2019-11-20 16:51 | 只看該作者
請問 16路舵機(jī)是什么意思？是16個舵機(jī)嗎
回復(fù)

使用道具 舉報

13#
ID:683198 發(fā)表于 2020-1-7 20:45 來自手機(jī) | 只看該作者
感謝分享
回復(fù)

使用道具 舉報

14#
ID:686631 發(fā)表于 2020-1-17 12:25 | 只看該作者
舵機(jī)用單片機(jī)直接驅(qū)動？
回復(fù)

使用道具 舉報

15#
ID:138918 發(fā)表于 2020-5-14 12:53 | 只看該作者
不錯 果斷收藏學(xué)習(xí)
回復(fù)

使用道具 舉報

16#
ID:582255 發(fā)表于 2021-1-21 22:07 | 只看該作者
作者好像都沒有回帖啊
回復(fù)

使用道具 舉報

17#
ID:964559 發(fā)表于 2021-9-7 10:58 | 只看該作者
你好，有沒有電路圖
回復(fù)

使用道具 舉報

18#
ID:808223 發(fā)表于 2022-1-9 17:29 | 只看該作者
這是實現(xiàn)什么功能呢？
回復(fù)

使用道具 舉報

19#
ID:984082 發(fā)表于 2022-3-29 22:54 | 只看該作者
您好樓主  您的機(jī)械臂抓取物體是靠什么識別的
回復(fù)

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表
主站蜘蛛池模板： 综合色播| 免费看a | 久久精品亚洲精品国产欧美kt∨ | 国产精品久久久久久福利一牛影视 | 国产91久久久久蜜臀青青天草二 | 欧美久久影院 | 欧美久久影院 | 国产无套一区二区三区久久 | 久久精品手机视频 | 国产www成人| 日韩中文字幕在线视频 | 久久精品国产久精国产 | 色香蕉在线| 午夜影院视频 | 免费视频二区 | 中文字幕 亚洲一区 | 综合国产第二页 | 一级黄色片网站 | 懂色中文一区二区三区在线视频 | 欧美三区在线观看 | 美女一区二区在线观看 | 日韩欧美大片在线观看 | 久久久久久国产精品免费 | 九九热最新视频 | 中文字幕在线观看成人 | 欧美自拍第一页 | 国产精品一区2区 | 精品一区av| 99精品免费| 中文字幕一区二区三区在线观看 | 天天操夜夜操免费视频 | 日韩超碰在线 | 欧美日韩中文在线观看 | 成人免费视频在线观看 | 精品综合久久久 | 免费在线观看成人av | 国产欧美日韩在线播放 | 精品一区二区久久久久久久网站 | 蜜臀久久99精品久久久久久宅男 | 日本精品一区二区三区视频 | 最新日韩精品 |