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

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 2804|回復: 1
打印 上一主題 下一主題
收起左側

基于51單片機的機械臂抓取 16路舵機控制程序

[復制鏈接]
跳轉到指定樓層
樓主
ID:721819 發表于 2020-5-2 21:01 | 只看該作者 回帖獎勵 |倒序瀏覽 |閱讀模式
基于51單片機的機械臂抓取

單片機源程序如下:
  1. /***************************************************************************************************************
  2. 文 件 名：main.c  
  3. 功能描述：
  4. 備    注： 16路舵機控制
  5. ****************************************************************************************************************/
  6. #include "STC15Fxxxx.H"  //STC15系列單片機
  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};    // 定義接收數據變量數組
  33. uint8 line=0;                                        //緩存存入口與出口之間的距離，即當前緩存中有多少個沒有執行的數據
  34. uint8 point_now=0;                                //與point_aim一起標記緩存出口位置，即取數位置
  35. uint8 point_aim=1;
  36. uint8 point_in=2;                                //標記緩存入口位置，即上一行數據存放位置
  37. bit flag_connect = 0;
  38. bit flag_stop=1;                                //表示一行執行結束
  39. uint8 flag_vpwm=0;                                //表示到達了該更新pwm[]的時間
  40. bit flag_in=1;                                         //表示緩存中有空閑空間
  41. bit flag_out=0;                                        //表示緩存中有可執行數據的標志位
  42. bit flag_run_ready=0;                        //表示有要要放入緩存的EErom數據
  43. uint16 n=1000;                                        //用來計算需要建立多少個中間數據
  44. uint16 m=1;                                                //用來累計已經執行了多少中間數據
  45. double dp;
  46. double dp0[MOTOR_NUM] = {0};                                        //插補增量
  47. bit flag_download = 0;//判斷是否下載
  48. bit flag_read = 0;// 讀取flash內容，發送上位機
  49. bit flag_connect_run = 0;//連接上位機的執行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;//當前串口接收到命令狀態存儲
  57. MotorData motor_data;//舵機總信息
  58. MotorOneCmd motor_one_cmd;//單個命令
  59. CurrentItem cur_item;
  60. uint16 tuoji_count = 0;//脫機執行次數
  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();//讀取舵機控制信息
  95.         updata_file_list();
  96.         memset(&cur_item,0,sizeof(cur_item));
  97.         beep_mode = motor_data.beep_mode;
  98. }

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

  251.                                 case PSB_TRIANGLE:kind = 7;break;
  252.                                 case PSB_CROSS:kind = 8;break;
  253.                                 case PSB_PINK:kind = 9;break;
  254.                                 case PSB_CIRCLE:kind = 10;break;

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

  305.                                         case PSB_L1:pwm[5]+=10;if(pwm[5]>=2300) pwm[5]=2300;break;
  306.                                         case PSB_L2:pwm[5]-=10;if(pwm[5]<=700)  pwm[5]=700;break;
  307.                                         case PSB_R1:pwm[6]+=10;if(pwm[6]>=2300) pwm[6]=2300;break;
  308.                                         case PSB_R2:pwm[6]-=10;if(pwm[6]<=700)  pwm[6]=700;break;
  309.                                         default:break;
  310.                                 }
  311.                 }
  312.         }
  313. }
  314.         
  315. void ReadMoterInfor()
  316. {
  317.         memset(&motor_data,0,sizeof(motor_data));//清 0
  318.         SpiFlashRead((char *)&motor_data,(CMD_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_data));//讀取信息
  319.         if (motor_data.CRC1 != 0x12345678 || motor_data.sum < 0 || motor_data.duoji_count  > MOTOR_NUM)//判斷信息存儲是否有錯
  320.         {
  321.                 memset(&motor_data,0,sizeof(motor_data));
  322.                 //memset(&cur_item,0,sizeof(cur_item));
  323.         }
  324.         else//正常信息
  325.         {
  326.                 //UART1_SendOneChar(motor_data.sum + 0x30);
  327.                 //cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脫機運行次數
  328.                 //cur_item.cur_num = 0;//清 0
  329.         }
  330.                
  331. }
  332. void ReadOneCmdInfor(unsigned int addr)
  333. {
  334.         memset(&motor_one_cmd,0,sizeof(motor_one_cmd));//清 0
  335.         SpiFlashRead((char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd));//讀取信息
  336.         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)//判斷信息存儲是否有錯
  337.         {
  338.                 memset(&motor_one_cmd,0,sizeof(motor_one_cmd));
  339.         }
  340.         else//正常信息,以后留著驗證用的
  341.         {
  342.                
  343.                
  344.                 cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脫機運行次數
  345.                 cur_item.cur_num = motor_one_cmd.start;
  346.                 //UART1_SendOneChar(motor_one_cmd.tuoji_count + 0x30);
  347.                 //cur_item.cur_num = 0;//清 0
  348.         }
  349.                
  350. }

  352. void WriteMoterInfor()
  353. {
  354.         uchar temp = 0;
  355.         motor_data.CRC1 = 0x12345678;//校驗碼
  356.         motor_data.duoji_count = MOTOR_NUM-1;
  357.         temp = motor_data.filecount;
  358.         SpiFlashEraseSector(CMD_FLASH_ADDR >> 4);//擦除以前存儲的信息
  359.         SpiFlashWrite((char *)&motor_data,CMD_FLASH_ADDR<<WRITE_BIT_DEPTH,sizeof(motor_data)); //寫入flash
  360.         ReadMoterInfor();
  361.         if (temp != motor_data.filecount)
  362.         {
  363.                 error |= ERROR_FLASH_WRITE;
  364.         }
  365.         else
  366.         {
  367.                 error &= ~ERROR_FLASH_WRITE;
  368.         }
  369. }
  370. void WriteOneCmdInfor(unsigned int addr)
  371. {
  372.         uchar temp = 0;
  373.         temp = motor_one_cmd.end;
  374.         if (((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) % 16 == 0)
  375.                 SpiFlashEraseSector(((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) >> 4);//擦除以前存儲的信息
  376.         SpiFlashWrite((char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd)); //寫入flash
  377.         ReadOneCmdInfor(addr);
  378.         if (temp !=  motor_one_cmd.end)
  379.         {
  380.                 error |= ERROR_FLASH_WRITE1;
  381.         }
  382.         else
  383.         {
  384.                 error &= ~ERROR_FLASH_WRITE1;
  385.         }
  386. }
  387.         
  388. void GetOneMotorCMD()
  389. {
  390. #if DEBUG
  391.         uchar buf[20] = {0};
  392. #endif
  393.         if (flag_stop_download)//接收到了上位機的停止下載的命令
  394.         {
  395.                 flag_download = 0;//清楚下載狀態標志位
  396.                 flag_stop_download = 0;
  397.                 flag_read = 0;
  398.                 if (motor_data.filecount < MAX_SUPPORT_FILE_SAVE)
  399.                 {
  400.                         updata_file_list();
  401.                         motor_data.sum = motor_one_cmd.end;
  402.                         motor_data.file_flag[motor_data.filecount] = 1;
  403.                         motor_one_cmd.cur_file_num = file_last_num + 1;
  404.                         motor_data.filecount = motor_one_cmd.cur_file_num + 1;
  405.                         error &= ~MAX_SUPPORT_FILE_SAVE;
  406. #if DEBUG
  407.                         sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
  408.                         UART1_SendStr(buf);
  409. #endif
  410.                         WriteMoterInfor();
  411.                         WriteOneCmdInfor(motor_one_cmd.cur_file_num);
  412.                         updata_file_list();
  413. #if DEBUG
  414.                         
  415.                         sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
  416.                         UART1_SendStr(buf);
  417. #endif
  418.                 }
  419.                 else
  420.                 {
  421.                         error |= MAX_SUPPORT_FILE_SAVE;
  422.                 }
  423.                 if        (!(error &(MAX_SUPPORT_FILE_SAVE | ERROR_FLASH_FULL)))
  424.                         send_mode |= SEND_DOWN_OK;//狀態位置為
  425.         }
  426.         if (flag_connect)//如果當前跟上位機聯機狀態
  427.         {
  428.                 if (flag_read)//如果上位機讀取flash內存儲的信息
  429.                 {               
  430.                         if (cur_item.cur_num < motor_one_cmd.end)//判斷是否超過之前存儲的數
  431.                         {
  432.                                 if ((send_mode & SEND_SEND_FILE))//開始接收到讀取命令需要先發送個start
  433.                                 {
  434.                                         UART1_SendStr("#Start\r\n");
  435.                                         send_mode &= ~SEND_READ_FILE;
  436.                                 }
  437.                                 memset(redata,0,WRITE_SIZE);//清 0
  438.                                 SpiFlashRead(redata,(((long)cur_item.cur_num)<<WRITE_BIT_DEPTH),WRITE_SIZE);//讀取信息
  439. #if DEBUG
  440.                                 sprintf(buf,"%d\r\n",cur_item.cur_num);
  441.                                 UART1_SendStr(buf);
  442. #endif
  443.                                 UART1_SendStr(redata);//發送
  444.                                 cur_item.cur_num++;
  445.                         }
  446.                         else//否則
  447.                         {
  448.                                 if (cur_item.cur_num > 0)
  449.                                         UART1_SendStr("#End\r\n");//發送結束字符串
  450.                                   flag_read = 0;
  451.                         }
  452.                         send_mode = 0;//請 0
  453.                 }        
  454.                 if (flag_online_run)
  455.                 {
  456.                         if ((send_mode & SEND_CC) != 0  || cur_item.cur_num == motor_one_cmd.start)//如果當前需要更新舵機命令
  457.                         {
  458.                                 if (cur_item.tuoji_count > 0)//脫機次數沒結束
  459.                                 {
  460.                                         if (cur_item.cur_num < motor_one_cmd.end)//判斷是否讀取結束
  461.                                         {
  462.                                                 SpiFlashRead(redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//讀取命令
  463.                                                 flag_RecFul = 1;//標志位為1，
  464.                                                 cur_item.cur_num++;//
  465.                                         }
  466.                                         else//執行玩一遍
  467.                                         {
  468.                                                 cur_item.cur_num = motor_one_cmd.start;
  469.                                                 cur_item.tuoji_count--;//減一
  470.                                         }
  471.                                 }
  472.                                 else//執行完成
  473.                                 {
  474.                                         flag_online_run = 0;
  475.                                         if (flag_connect_run)//如果上位機選擇執行的功能，需要發送 AGF作為結束
  476.                                         {
  477.                                                 UART1_SendStr("#AGF\r\n");
  478.                                                 flag_connect_run = 0;
  479.                                         }
  480.                                 }
  481.                                 //讀取數據
  482.                         }
  483.                 }
  484.         }
  485.         else//脫機
  486.         {
  487.                 if (file_list_count < 0)
  488.                 {
  489.                         return;
  490.                 }
  491.                 if (cur_item.tuoji_count > 0)
  492.                 {
  493.                         if ((send_mode & SEND_CC) != 0  || cur_item.cur_num == motor_one_cmd.start)//如果當前需要更新舵機命令
  494.                         {
  495.                                 if (cur_item.cur_num < motor_one_cmd.end)//判斷是否讀取結束
  496.                                 {
  497.                                         SpiFlashRead(redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//讀取命令
  498.                                         flag_RecFul = 1;//標志位為1，
  499.                                         cur_item.cur_num++;//
  500.                                 }
  501.                                 else//執行玩一遍
  502.                                 {
  503.                                         cur_item.cur_num = motor_one_cmd.start;
  504.                                         cur_item.tuoji_count--;//減一
  505.                                 }
  506.                                 //讀取數據
  507.                         }
  508.                 }
  509.                 else
  510.                 {
  511.                         ReadOneCmdInfor(file_list[cur_item.file_num]);
  512.                         file_list_count--;
  513.                         cur_item.file_num++;
  514.                 }
  515.         }
  516.                
  517.         
  518. }
  519.         
  520. void SendUartState()
  521. {
  522.         uchar buf[40] = {0};
  523.         uchar read_motor_num = 0;
  524.         uint i = 0;
  525.         static int count = 0;
  526.         if (send_mode)//如果有狀態需要發送
  527.         {
  528.                 if (send_mode & SEND_A) //發送A
  529.                 {
  530.                         UART1_SendOneChar('A');
  531.                         send_mode &= ~SEND_A;//清狀態
  532.                 }
  533.                 if (send_mode & SEND_CC) //發送CC
  534.                 {
  535.                         UART1_SendStr("#CC\r\n");
  536.                         send_mode &= ~SEND_CC;
  537.                 }
  538.                 if (send_mode & SEND_DOWN_OK)//發送下載ok的狀態字符串
  539.                 {
  540.                         sprintf(buf,"#Down+OK+%d\r\n",(int)motor_data.filecount-1);
  541.                         UART1_SendStr(buf);
  542.                         send_mode &= ~SEND_DOWN_OK;
  543. #if DEBUG
  544.                         sprintf(buf,"%d\r\n",(uint)motor_data.filecount);
  545.                         UART1_SendStr(buf);
  546. #endif
  547.                 }
  548.                 if (send_mode & SEND_START_OK)//發送連接時候的字符串
  549.                 {
  550.                         UART1_SendStr("#Veri+UART+OK+20160906+176\r\n");
  551.                         send_mode &= ~SEND_START_OK;
  552.                 }
  553.                 if (send_mode & SEND_READ_FILE)//發送讀取文件的時候字符串
  554.                 {
  555.                         if (motor_data.filecount > 0)//如果保存的有舵機命令
  556.                         {
  557.                                 //#Name:1.txt--Size:48--Name:2.txt--Size:190--Name:desktop.ini--Size:531--
  558.                                 UART1_SendStr("#");//發送
  559.                                 for (i = 0; i < motor_data.filecount;i++)
  560.                                 {
  561.                                         if (motor_data.file_flag[i] == 1)
  562.                                         {
  563.                                                 ReadOneCmdInfor(i);
  564.                                                 if (motor_one_cmd.end - motor_one_cmd.start <= 0)
  565.                                                 {
  566.                                                         motor_data.file_flag[i] = 0;
  567.                                                         WriteMoterInfor();
  568. #if DEBUG
  569.                                                         sprintf(buf,"E=%d S=%d",motor_one_cmd.end, motor_one_cmd.start);
  570. #endif
  571.                                                 }
  572.                                                 else
  573.                                                 {
  574.                                                         sprintf(buf,"Name:%d.txt--Size:%d--",i,motor_one_cmd.end - motor_one_cmd.start);//獲取命令個數
  575.                                                         UART1_SendStr(buf);//發送
  576.                                                 }
  577.                                         }

  579.                                 }
  580.                                 UART1_SendStr("\r\n");                                
  581.                         }
  582.                         else
  583.                         {
  584.                                 sprintf(buf,"#\r\n",motor_data.sum);
  585.                                 UART1_SendStr(buf);
  586.                         }
  587.                         send_mode &= ~SEND_READ_FILE;
  588.                 }
  589.                 if (send_mode & SEND_SET_OFFLINE_OK)//設置脫機運行次數
  590.                 {
  591.                         WriteOneCmdInfor(motor_one_cmd.cur_file_num);//保存
  592.                         UART1_SendStr("#Enable+OK...\r\n");
  593.                         send_mode &= ~SEND_SET_OFFLINE_OK;
  594.                 }
  595.                 if (send_mode & SEND_SET_DISABLEOFFLINE_OK)//禁止脫機運行
  596.                 {
  597.                         for (i = 0; i < motor_data.filecount;i++)
  598.                         {
  599.                                 if (motor_data.file_flag[i] == 1)
  600.                                 {
  601.                                         ReadOneCmdInfor(i);
  602.                                         motor_one_cmd.tuoji_count = 0;
  603.                                         WriteOneCmdInfor(i);
  604.                                 }

  606.                         }
  607.                         WriteMoterInfor();
  608.                         UART1_SendStr("#Disable+OK...\r\n");
  609.                         send_mode &= ~SEND_SET_DISABLEOFFLINE_OK;
  610.                 }
  611.                 if (send_mode & SEND_SET_ONLINE_OK)//發送聯機運行狀態
  612.                 {
  613.                         UART1_SendStr("#OK\r\n");
  614.                         sprintf(buf,"#%dGC%d\r\n",cur_item.file_num,tuoji_count);
  615.                         UART1_SendStr(buf);
  616.                         UART1_SendStr("#LP=0\r\n");
  617.                         send_mode &= ~SEND_SET_ONLINE_OK;
  618.                         flag_connect_run = 1;
  619.                 }
  620.                 if (send_mode & SEND_SET_DELETE_ONE_FILE_OK)//發送刪除文件命令
  621.                 {
  622.                         //cur_item.tuoji_count = 0;
  623.                         if (cur_item.delete_num < motor_data.filecount)
  624.                         {
  625.                                 motor_data.file_flag[cur_item.delete_num] = 0;
  626.                                 WriteMoterInfor();
  627.                                 updata_file_list();
  628.                                 if (cur_item.delete_num  == motor_data.filecount-1)
  629.                                 {
  630.                                         motor_data.filecount = file_last_num + 1;
  631.                                         if (file_last_num == -1)
  632.                                         {
  633.                                                 motor_data.sum = 0;
  634.                                         }
  635.                                         else
  636.                                         {
  637.                                                 ReadOneCmdInfor(file_last_num);
  638.                                                 motor_data.sum = motor_one_cmd.end;
  639.                                                 motor_data.sum = (((long int)(motor_data.sum) >>4)<<4) + (1<<4);
  640.                                         }
  641.                                         WriteMoterInfor();
  642.                                 }
  643.                                 updata_file_list();               
  644.                                 UART1_SendStr("#FDel+OK\r\n");
  645.                         }
  646.                         send_mode &= ~SEND_SET_DELETE_ONE_FILE_OK;
  647.                         
  648.                 }
  649.                 if (send_mode & SEND_SET_DELETE_ALL_FILE_OK)//發送擦除所有文件的命令
  650.                 {
  651.                         UART1_SendStr("#Format+Start\r\n");
  652.                         SpiFlashEraseChip();
  653.                         cur_item.tuoji_count = 0;
  654.                         motor_data.sum = 0;
  655.                         motor_data.filecount = 0;
  656.                         memset(motor_data.file_flag,0,sizeof(motor_data.file_flag));
  657.                         WriteMoterInfor();
  658.                         UART1_SendStr("#Format+OK\r\n");
  659.                         send_mode &= ~SEND_SET_DELETE_ALL_FILE_OK;
  660.                         updata_file_list();
  661.                 }
  662.                 if (send_mode & SEND_SET_PS2_OK)
  663.                 {
  664.                         UART1_SendStr("#PS2+OK...\r\n");
  665.                         send_mode &= ~SEND_SET_PS2_OK;
  666.                 }
  667.                
  668. #define MATHION_HAND_NUM 20
  669.                 if (send_mode & SEND_SET_READ_UART_MOTOR_ANGLE)
  670.                 {
  671.                         if (cur_item.read_num < MATHION_HAND_NUM)
  672.                         {
  673.                                 if (flag_uart2_rev)
  674.                                 {
  675.                                         read_motor_num = atoi(uart2_buf + 1);
  676.                                         if (read_motor_num == cur_item.read_num)
  677.                                         {
  678.                                                 i = atoi(uart2_buf+5);
  679.                                                 sprintf(uart2_buf,"#%dP%d",(int)read_motor_num,i);
  680.                                                 UART1_SendStr(uart2_buf);
  681.                                                 cur_item.read_num++;
  682.                                                 buf[0] = '#';
  683.                                                 buf[1] = cur_item.read_num / 100 % 10 + 0x30;
  684.                                                 buf[2] = cur_item.read_num / 10 % 10 + 0x30;
  685.                                                 buf[3] = cur_item.read_num % 10 + 0x30;
  686.                                                 buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
  687.                                                 UART2_SendStr(buf);
  688.                                                 
  689.                                         }
  690.                                         flag_uart2_rev = 0;
  691.                                         count = 0;
  692.                                        
  693.                                 }
  694.                                 else
  695.                                 {
  696.                                         count++;
  697.                                         if (count >= 10)
  698.                                         {
  699.                                                 cur_item.read_num++;
  700.                                                 buf[0] = '#';
  701.                                                 buf[1] = cur_item.read_num / 100 % 10 + 0x30;
  702.                                                 buf[2] = cur_item.read_num / 10 % 10 + 0x30;
  703.                                                 buf[3] = cur_item.read_num % 10 + 0x30;
  704.                                                 buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
  705.                                                 UART2_SendStr(buf);
  706.                                                 flag_uart2_rev = 0;
  707.                                                 flag_uart2_rev_time_out = 0;
  708.                                                 count = 0;
  709.                                         }
  710.                                 }
  711.                         }
  712.                         else
  713.                         {
  714.                                 send_mode &= ~SEND_SET_READ_UART_MOTOR_ANGLE;
  715.                                 cur_item.read_num= 0;
  716.                                 UART1_SendStr("\r\n");
  717.                         }
  718.                 }
  719.         }
  720.         if (send_mode & SEND_SET_SET_UART_MOTOR_PULK)
  721.         {
  722.                 if (cur_item.pulk_num < MATHION_HAND_NUM)
  723.                 {
  724.                         count++;
  725.                         if (count >= 20)
  726.                         {
  727.                                 sprintf(uart2_buf,"#%dPULK\r\n",(int)cur_item.pulk_num);
  728.                                 UART2_SendStr(uart2_buf);
  729. #if DEBUG
  730.                                 UART1_SendStr(uart2_buf);
  731. #endif
  732.                                 cur_item.pulk_num++;
  733.                                 count = 0;
  734.                         }
  735.                 }
  736.                 else
  737.                 {
  738.                         send_mode &= ~SEND_SET_SET_UART_MOTOR_PULK;
  739.                 }
  740.         }
  741.         if (send_mode & SEND_SET_SET_UART_MOTOR_ANGLE)
  742.         {
  743.                 if (cur_item.pulk_num < MATHION_HAND_NUM)
  744.                 {
  745.                         count++;
  746.                         if (count >= 5)
  747.                         {
  748.                                 sprintf(uart2_buf,"#%dPMOD%d\r\n",(int)cur_item.pulk_num,(int)cur_item.angle_mode);
  749.                                 UART2_SendStr(uart2_buf);
  750. #if DEBUG
  751.                                 UART1_SendStr(uart2_buf);
  752. #endif
  753.                                 cur_item.pulk_num++;
  754.                                 count = 0;
  755.                         }
  756.                 }
  757.                 else
  758.                 {
  759.                         sprintf(buf,"#255PMOD%d+0K...\r\n",(int)cur_item.angle_mode);
  760.                         UART1_SendStr(buf);
  761.                         send_mode &= ~SEND_SET_SET_UART_MOTOR_ANGLE;
  762.                         flag_uart2_rev = 0;
  763.                 }
  764.         }
  765.         if (send_mode & SEND_SET_BEEP_ON)
  766.         {
  767.                 ReadMoterInfor();
  768.                 motor_data.beep_mode = 1;
  769.                 beep_mode = 1;
  770.                 WriteMoterInfor();
  771.                 UART1_SendStr("#FMQENABLE+OK...\r\n");
  772.                 send_mode &= ~SEND_SET_BEEP_ON;
  773.         }
  774.         if (send_mode & SEND_SET_BEEP_OFF)
  775.         {
  776.                 ReadMoterInfor();
  777.                 motor_data.beep_mode = 0;
  778.                 beep_mode = 0;
  779.                 WriteMoterInfor();
  780.                 UART1_SendStr("#FMQDISABLE+OK...\r\n");
  781.                 send_mode &= ~SEND_SET_BEEP_OFF;
  782.         }               
  783. }
  784.         
  785. void change(void)
  786. {        
  787.         uchar s;
  788.         if(line>0)                                   //緩存中有數據
  789.         {  
  790.                 line--;                                   //執行一行
  791.                 if(line<5)                        //緩存允許放入新的數據                        
  792.                         flag_in=1;
  793.                 point_now++;                //取數位置更新
  794.                 point_aim++;
  795.                
  796.                 if(point_aim==7)
  797.                            point_aim=0;
  798.                 if(point_now==7)
  799.                            point_now=0;
  800.                 n=pos[point_aim][0]*4/5;        //計算新的插補次數
  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.            }
  815.                 m=0;                                          //m清0
  816.                 flag_stop=0;                          //產生了新的目標位置，停止標志清零
  817.                
  818.         }
  819.         else                                                  //沒有緩存數據，即line==0
  820.         {
  821.                 flag_out=0;                                //緩存中沒有數據
  822.         }
  823. }
  824.         
  825. void vpwm(void)                 
  826. {
  827.         uchar j=0;
  828.         uchar how=0;
  829.   static uchar flag_how;
  830.         static uchar flag_Tover;

  832.         if(flag_stop==1)                                           //一行作業全部完成
  833.         {        
  834.                 if(flag_out==1)                                         //緩沖數組中有數據
  835.                 {
  836.                         change();                                        //更新行
  837.                 }
  838.         }
  839.         else                                                                //不是一行數據全部完成，處于中間插補階段
  840.         {
  841.                 m++;                                                        //用來累加插補過的次數
  842.                 if(m==n)                                                //n是本行作業要插補的總次數
  843.                 {
  844.                   flag_Tover=1;                                //一行數據的執行時間已經完成
  845.                         send_mode |= SEND_CC;
  846.                 }
  847.                 for(j=1;j<MOTOR_NUM;j++)
  848.                 {
  849.                         if(abs(pwm[j]-pos[point_aim][j])<5)
  850.                         {                                                           //檢測靠近終點位置
  851.                            how++;                                           //是，則累加一個
  852.                            pwm[j]=pos[point_aim][j];//并且直接過度到終點位置
  853.                         }        
  854.                         else                                                //不靠近終點，繼續插補
  855.                                 pwm[j]=pos[point_now][j]+m*dp0[j];
  856.                 }         
  857.                 //UART_Put_Inf("pwm",pwm[1]);
  858.                 if(how==MOTOR_NUM-1)
  859.                         flag_how=1;                                                  //16個舵機都到達終點
  860.                 how=0;
  861.                 if((flag_Tover==1)&&(flag_how==1))
  862.                 {                                                                //從插補次數，和脈寬寬度兩方面都到達終點，本作業行完成
  863.                          flag_Tover=0;
  864.                          flag_how=0;
  865.                          flag_stop=1;
  866.                 }
  867.                
  868.          }
  869.         return;

  871. }

  873.                            
復制代碼

以上程序51hei提供下載:
機械臂.zip (700.94 KB, 下載次數: 24)
機械臂1.zip (73.88 KB, 下載次數: 20)


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

使用道具 舉報

沙發
ID:1 發表于 2020-5-2 23:37 | 只看該作者
本帖需要重新編輯補全電路原理圖,源碼,詳細說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)
回復

使用道具 舉報

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

本版積分規則

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

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： h视频在线播放 | 九九热免费看 | 亚洲一区二区免费电影 | 日韩综合在线播放 | 拍真实国产伦偷精品 | 久久久久久久久久一区二区 | 五月婷婷丁香 | 亚洲一区久久 | 人人爽日日躁夜夜躁尤物 | 四虎影院美女 | 国产在线高清 | 99精品视频在线观看免费播放 | 精品欧美一区二区三区精品久久 | 99爱在线 | www.性色 | 久久美女视频 | 亚洲在线日韩 | 亚洲午夜久久久 | 亚洲欧美中文日韩在线v日本 | 亚洲高清在线视频 | 黄色成人av | 亚洲精品欧美 | 国产a级毛片 | 久久久久无码国产精品一区 | 久久久久中文字幕 | 国产精品久久久久久久久图文区 | 韩国精品一区 | 亚洲一区二区三区福利 | 日韩精品免费一区 | 国产精品一区二区不卡 | 精品国产综合 | 久久国产精品久久久久久 | 欧美日韩精品久久久免费观看 | 日韩字幕一区 | 国产免费va | 日韩精品一区二区三区在线观看 | 一区二区三区四区不卡视频 | 精品久久久久国产免费第一页 | 日本成人三级电影 | 欧美日韩不卡合集视频 | 久久国产三级 |