舵機控制器STM32源程序

ID:339076 · 發表于 2018-5-27 14:45

分享一個舵機驅動程序

單片機源程序如下:

#include <stdlib.h>
#include <string.h>
#include "stm32f10x.h" //包含stm32庫系統頭文件
#include "servor.h" //包含GPIO庫頭文件
#include "usart.h" //包含串口通信設置頭文件
#include "delay.h" //包含系統定時器庫頭文件
#include "timer.h" //包含定時器設置頭文件
#include "PS2.h"
#include "key.h"
#include "led.h"
#include "beep.h"
#include "flash.h"
#include "common.h"
#include "adc.h"
const u8 TEXT_Buffer[]={"Flash SPI TEST"};
#define SIZE sizeof(TEXT_Buffer)
u8 datatemp[SIZE];
u32 FLASH_SIZE;
u8 date[3]; //該變量用來存儲安卓終端發來的數據
u32 value;
u32 key, key_bak;
extern u32 a; //a用來計數，配合系統滴答定時器可檢測代碼運行時間
extern uint8 flag_RecFul;
uint16 pos[7][MOTOR_NUM]={ {1000,1500,1500,1500,1500,1500,1500,1500,1500},
{1000,1500,1500,1500,1500,1500,1500,1500,1500},
{1000,1500,1500,1500,1500,1500,1500,1500,1500},
{1000,500,500,500,500,500,500,500,500},
{1000,1500,1500,1500,1500,1500,1500,1500,1500},
{1000,500,500,500,500,500,500,500,500},
{1000,1500,1500,1500,1500,1500,1500,1500,1500}
}; //位置
uint16 CPWM[MOTOR_NUM]= {1500,1500,1500,1500,1500,1500,1500,1500,1500};
uint16 UartRec[MOTOR_NUM]={1500,1500,1500,1500,1500,1500,1500,1500,1500};
unsigned int dj1=1500;
unsigned int dj2=1500;
unsigned int dj3=1500;
unsigned int dj4=1500;
unsigned int dj5=1500;
unsigned int dj6=1600;
unsigned char buf[30];
uint8 line=0; //緩存存入口與出口之間的距離，即當前緩存中有多少個沒有執行的數據
char point_now=0; //與point_aim一起標記緩存出口位置，即取數位置
char point_aim=1;
char point_in=1; //標記緩存入口位置，即上一行數據存放位置
uint8 flag_connect = 0;
uint8 flag_stop=1; //表示一行執行結束
uint8 flag_vpwm=0; //表示到達了該更新pwm[]的時間
uint8 flag_in=1; //表示緩存中有空閑空間
uint8 flag_out=0; //表示緩存中有可執行數據的標志位
uint8 flag_run_ready=0; //表示有要要放入緩存的EErom數據
uint16 n=1000; //用來計算需要建立多少個中間數據
uint16 m=1; //用來累計已經執行了多少中間數據
double dp;
double dp0[MOTOR_NUM] = {0}; //插補增量
uint8 flag_download = 0;//判斷是否下載
uint8 flag_read = 0;// 讀取flash內容，發送上位機
uint8 flag_connect_run = 0;//連接上位機的執行flash保存的命令
uint8 flag_stop_download = 0;//停止下載
uint8 flag_online_run = 0;
uint8 flag_uart2_rev = 0;
uint8 flag_uart2_rev_time_out = 0;
uint8 flag_ps2_rev = 0;
char redata[257] = {0}; // 定義接收數據變量數組
//uint8 flag_read_adc = 0;
unsigned long send_mode = 0;//當前串口接收到命令狀態存儲
MotorData motor_data;//舵機總信息
MotorOneCmd motor_one_cmd;//單個命令
CurrentItem cur_item;
uint16 tuoji_count = 0;//脫機執行次數
unsigned char flag_scan_ps2 = 0;
uint8 flag_adc=0;
uint8 error = 0;
uint8 file_list[MAX_SUPPORT_FILE_SAVE] = {0};
int file_list_count = 0;
int file_last_num = 0;
unsigned char ps2_buf[120] = {0};
char uart2_buf[50] = {0};
uint16 cur_count = 0;
uint16 adc_value=0;
//uchar ad_value = 0;
//uchar beep_mode = 0;
u8 ps2_mode=0;
void SerVor_Test(void);
void Ps2_Test(void);
void Flash_Test(void);
void updata_file_list(void);
void InitMotor(void);
void vpwm(void);
void scan_ps2(void);
void Check_Power(void);
/**********************************************************************************/
/*****************************主函數***********************************************/
int main(void)
{
SysTick_Init(); //系統滴答定時器初始化
Servor_GPIO_Config(); //GPIO初始化
Uart_Init(1);
Uart_Init(2);
Timer_Init(); //定時器初始化
Timer_ON(); //開啟定時器
LED_Init();
Beep_Init();
PS2_Init();
Adc_Init();
USART_Config(USART1,115200);
USART_Config(USART2,115200);
SpiFlashInit(); //初始化flash
Led_Test();
Beep_Test();
while(SpiFlashReadID()!=W25Q64) //檢測不到W25Q64
{
UART_PutStr (USART1,"Not Found W25Q64\r\n");
}
while (1)
{
// Flash_Test();
//UART_PutStr (USART2,"uart2 test\r\n");
if(flag_vpwm==1)
{
vpwm(); //更新pwm[]數組
flag_vpwm=0;
}
if( flag_RecFul==1) //串口接受完一條指令
{
//LED3_ON();
DealRec(); //處理串口緩存中的數據
flag_RecFul=0;
//LED3_OFF();
}
GetOneMotorCMD();//獲取一個命令
SendUartState();//發送狀態信息
#if PS_SUPPORT
scan_ps2();
#endif
// LED_State();
Check_Power();
//cur_count++;
}
}
void updata_file_list(void)
{
unsigned char i = 0;
unsigned char j = 0;
file_last_num = -1;
ReadMoterInfor();
for (i = 0; i < motor_data.filecount; i++)
{
if (motor_data.file_flag[i] == 1)
{
file_list[j] = i;
j++;
file_last_num = i;
}
}
file_list_count = j;
}
void InitMotor(void)
{
ReadMoterInfor();//讀取舵機控制信息
updata_file_list();
memset(&cur_item,0,sizeof(cur_item));
//beep_mode = motor_data.beep_mode;
}
void Check_Power(void)
{
if(flag_adc==1)
{
flag_adc=0;
adc_value=Get_Adc(0);
if(adc_value>=2100 && adc_value<=2481) //電池電量低 7.4V鋰電池這里當電池電壓低于6V就算沒電　>2100是為了防止沒接電池誤報
{
Beep=~Beep;
}
else
{
BEEP(OFF);
}
//UART_Put_Inf("adc:",adc_value); //將讀取到ADC1 通道0的ADC值打印出來
}
}
void scan_ps2(void)
{
int kind = 0;
char *p = NULL;
char buf[15] = {0};
char i = 0;
if (flag_scan_ps2)//
{
flag_scan_ps2 = 0;
key = PS2_DataKey();
ps2_mode=PS2_RedLight();
if(ps2_mode==0)
{
if(key_bak == key)return;
key_bak=key;
Beep=~Beep;
switch(key)
{
case PSB_PAD_UP:kind = 1;break;
case PSB_PAD_DOWN:kind = 2;break;
case PSB_PAD_LEFT:kind = 3;break;
case PSB_PAD_RIGHT:kind = 4;break;
case PSB_TRIANGLE:kind = 7;break;
case PSB_CROSS:kind = 8;break;
case PSB_PINK:kind = 9;break;
case PSB_CIRCLE:kind = 10;break;
case PSB_L1:kind = 6;break;
case PSB_L2:kind = 5;break;
case PSB_R1:kind = 12;break;
case PSB_R2:kind = 11;break;
default:break;
}
if (kind != 0)
{
flag_ps2_rev = 1;
flag_connect = 1;
SpiFlashRead(ps2_buf,(PS2_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(ps2_buf));
sprintf(buf,"%dK",kind);
UART_PutStr(USART1,buf);
p = strstr(ps2_buf,buf);
if (p != NULL)
{
p = p + strlen(buf);
while(i < 14 && *p != 0)
{
buf[i] = *p++;
i++;
if (*p == '#')
break;
}
if (i < 12)
{
buf[i] = '\r';
buf[i+1] = '\n';
memcpy(redata,buf,sizeof(buf));
flag_RecFul = 1;
}
UART_PutStr(USART1,redata);
}
}
}
else if(ps2_mode==1)
{
switch(key)
{
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;
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;
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;
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;
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;
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;
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;
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;
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;
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;
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;
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;
default:break;
}
}
}
}
/***************************************************************************************************************
函數名：從flash讀取舵機總的信息
功能描述：初始化舵機控制信息
輸入參數：無
返回值：無
備注：
****************************************************************************************************************/
void ReadMoterInfor(void)
{
memset(&motor_data,0,sizeof(motor_data));//清 0
SpiFlashRead((unsigned char *)&motor_data,(CMD_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_data));//讀取信息
if (motor_data.CRC1 != 0x12345678 || motor_data.sum < 0 || motor_data.duoji_count > MOTOR_NUM)//判斷信息存儲是否有錯
{
memset(&motor_data,0,sizeof(motor_data));
//memset(&cur_item,0,sizeof(cur_item));
}
else//正常信息
{
// UART1_SendOneChar(motor_data.sum + 0x30);
//cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脫機運行次數
//cur_item.cur_num = 0;//清 0
}
}
void ReadOneCmdInfor(unsigned int addr)
{
memset(&motor_one_cmd,0,sizeof(motor_one_cmd));//清 0
SpiFlashRead((unsigned char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd));//讀取信息
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)//判斷信息存儲是否有錯
{
memset(&motor_one_cmd,0,sizeof(motor_one_cmd));
}
else//正常信息,以后留著驗證用的
{
cur_item.tuoji_count = motor_one_cmd.tuoji_count;//脫機運行次數
cur_item.cur_num = motor_one_cmd.start;
// UART1_SendOneChar(motor_one_cmd.tuoji_count + 0x30);
// cur_item.cur_num = 0;//清 0
}
}
/***************************************************************************************************************
函數名：把舵機信息寫到flash中
功能描述：把舵機信息寫到flahs中
輸入參數：無
返回值：無
備注：
****************************************************************************************************************/
void WriteMoterInfor(void)
{
unsigned char temp =0;
motor_data.CRC1 = 0x12345678;//校驗碼
motor_data.duoji_count = MOTOR_NUM-1;
temp = motor_data.filecount;
SpiFlashEraseSector(CMD_FLASH_ADDR >> 4);//擦除以前存儲的信息
SpiFlashWrite((unsigned char *)&motor_data,CMD_FLASH_ADDR<<WRITE_BIT_DEPTH,sizeof(motor_data)); //寫入flash
ReadMoterInfor();
if (temp != motor_data.filecount)
{
error |= ERROR_FLASH_WRITE;
}
else
{
error &= ~ERROR_FLASH_WRITE;
}
}
void WriteOneCmdInfor(unsigned int addr)
{
unsigned char temp = 0;
temp = motor_one_cmd.end;
if (((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) % 16 == 0)
SpiFlashEraseSector(((((unsigned long)addr)<<4) + FILE_FLASH_ADDR) >> 4);//擦除以前存儲的信息
SpiFlashWrite((unsigned char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<<WRITE_BIT_DEPTH,sizeof(motor_one_cmd)); //寫入flash
ReadOneCmdInfor(addr);
if (temp != motor_one_cmd.end)
{
error |= ERROR_FLASH_WRITE1;
}
else
{
error &= ~ERROR_FLASH_WRITE1;
}
}
/***************************************************************************************************************
函數名：獲取一串舵機的控制字符串
功能描述：處理從flash讀取的舵機控制字符串處理
輸入參數：無
返回值：無
備注：
****************************************************************************************************************/
void GetOneMotorCMD(void)
{
#if DEBUG
uchar buf[20] = {0};
#endif
if (flag_stop_download)//接收到了上位機的停止下載的命令
{
flag_download = 0;//清楚下載狀態標志位
flag_stop_download = 0;
flag_read = 0;
if (motor_data.filecount < MAX_SUPPORT_FILE_SAVE)
{
updata_file_list();
motor_data.sum = motor_one_cmd.end;
motor_data.file_flag[motor_data.filecount] = 1;
motor_one_cmd.cur_file_num = file_last_num + 1;
motor_data.filecount = motor_one_cmd.cur_file_num + 1;
error &= ~MAX_SUPPORT_FILE_SAVE;
#if DEBUG
sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
UART_PutStr(USART1,buf);
#endif
WriteMoterInfor();
WriteOneCmdInfor(motor_one_cmd.cur_file_num);
updata_file_list();
#if DEBUG
sprintf(buf,"%d %d\r\n",(uint)motor_data.filecount,(uint)motor_data.file_flag[motor_data.filecount-1]);
UART_PutStr(USART1,buf);
#endif
}
else
{
error |= MAX_SUPPORT_FILE_SAVE;
}
if (!(error &(MAX_SUPPORT_FILE_SAVE | ERROR_FLASH_FULL)))
send_mode |= SEND_DOWN_OK;//狀態位置為
}
if (flag_connect)//如果當前跟上位機聯機狀態
{
if (flag_read)//如果上位機讀取flash內存儲的信息
{
if (cur_item.cur_num < motor_one_cmd.end)//判斷是否超過之前存儲的數
{
if ((send_mode & SEND_SEND_FILE))//開始接收到讀取命令需要先發送個start
{
UART_PutStr(USART1,"#Start\r\n");
send_mode &= ~SEND_READ_FILE;
}
memset(redata,0,WRITE_SIZE);//清 0
SpiFlashRead((unsigned char *)redata,(((long)cur_item.cur_num)<<WRITE_BIT_DEPTH),WRITE_SIZE);//讀取信息
#if DEBUG
sprintf(buf,"%d\r\n",cur_item.cur_num);
UART_PutStr(USART1,buf);
#endif
UART_PutStr(USART1,redata);//發送
cur_item.cur_num++;
}
else//否則
{
if (cur_item.cur_num > 0)
UART_PutStr(USART1,"#End\r\n");//發送結束字符串
flag_read = 0;
}
send_mode = 0;//請 0
}
if (flag_online_run)
{
if ((send_mode & SEND_CC) != 0 || cur_item.cur_num == motor_one_cmd.start)//如果當前需要更新舵機命令
{
if (cur_item.tuoji_count > 0)//脫機次數沒結束
{
if (cur_item.cur_num < motor_one_cmd.end)//判斷是否讀取結束
{
SpiFlashRead((unsigned char *)redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//讀取命令
flag_RecFul = 1;//標志位為1，
cur_item.cur_num++;//
}
else//執行玩一遍
{
cur_item.cur_num = motor_one_cmd.start;
cur_item.tuoji_count--;//減一
}
}
else//執行完成
{
flag_online_run = 0;
if (flag_connect_run)//如果上位機選擇執行的功能，需要發送 AGF作為結束
{
UART_PutStr(USART1,"#AGF\r\n");
flag_connect_run = 0;
}
}
//讀取數據
}
}
}
else//脫機
{
if (file_list_count < 0)
{
return;
}
if (cur_item.tuoji_count > 0)
{
if ((send_mode & SEND_CC) != 0 || cur_item.cur_num == motor_one_cmd.start)//如果當前需要更新舵機命令
{
if (cur_item.cur_num < motor_one_cmd.end)//判斷是否讀取結束
{
SpiFlashRead((unsigned char *)redata,((long)cur_item.cur_num)<<WRITE_BIT_DEPTH,WRITE_SIZE);//讀取命令
flag_RecFul = 1;//標志位為1，
cur_item.cur_num++;//
}
else//執行玩一遍
{
cur_item.cur_num = motor_one_cmd.start;
cur_item.tuoji_count--;//減一
}
//讀取數據
}
}
else
{
ReadOneCmdInfor(file_list[cur_item.file_num]);
file_list_count--;
cur_item.file_num++;
}
}
}
/***************************************************************************************************************
函數名：發送串口狀態信息
功能描述：根據狀態標志位發送相應的信息
輸入參數：無
返回值：無
備注：
****************************************************************************************************************/
void SendUartState(void)
{
char buf[40] = {0};
unsigned char read_motor_num = 0;
unsigned int i = 0;
static int count = 0;
if (send_mode)//如果有狀態需要發送
{
if (send_mode & SEND_A) //發送A
{
UART_PutChar(USART1,'A');
send_mode &= ~SEND_A;//清狀態
}
if (send_mode & SEND_CC) //發送CC
{
UART_PutStr(USART1,"#CC\r\n");
send_mode &= ~SEND_CC;
}
if (send_mode & SEND_DOWN_OK)//發送下載ok的狀態字符串
{
sprintf(buf,"#Down+OK+%d\r\n",(int)motor_data.filecount-1);
UART_PutStr(USART1,buf);
send_mode &= ~SEND_DOWN_OK;
#if DEBUG
sprintf(buf,"%d\r\n",(uint)motor_data.filecount);
UART_PutStr(USART1,buf);
#endif
}
if (send_mode & SEND_START_OK)//發送連接時候的字符串
{
UART_PutStr(USART1,"#Veri+UART+OK+20160906+176\r\n");
send_mode &= ~SEND_START_OK;
}
if (send_mode & SEND_READ_FILE)//發送讀取文件的時候字符串
{
if (motor_data.filecount > 0)//如果保存的有舵機命令
{
//#Name:1.txt--Size:48--Name:2.txt--Size:190--Name:desktop.ini--Size:531--
UART_PutStr(USART1,"#");//發送
for (i = 0; i < motor_data.filecount;i++)
{
if (motor_data.file_flag[i] == 1)
{
ReadOneCmdInfor(i);
if (motor_one_cmd.end - motor_one_cmd.start <= 0)
{
motor_data.file_flag[i] = 0;
WriteMoterInfor();
#if DEBUG
sprintf(buf,"E=%d S=%d",motor_one_cmd.end, motor_one_cmd.start);
#endif
}
else
{
sprintf(buf,"Name:%d.txt--Size:%d--",i,motor_one_cmd.end - motor_one_cmd.start);//獲取命令個數
UART_PutStr(USART1,buf);//發送
}
}
}
UART_PutStr(USART1,"\r\n");
}
else
{
sprintf(buf,"#\r\n",motor_data.sum);
UART_PutStr(USART1,buf);
}
send_mode &= ~SEND_READ_FILE;
}
if (send_mode & SEND_SET_OFFLINE_OK)//設置脫機運行次數
{
WriteOneCmdInfor(motor_one_cmd.cur_file_num);//保存
UART_PutStr(USART1,"#Enable+OK...\r\n");
send_mode &= ~SEND_SET_OFFLINE_OK;
}
if (send_mode & SEND_SET_DISABLEOFFLINE_OK)//禁止脫機運行
{
for (i = 0; i < motor_data.filecount;i++)
{
if (motor_data.file_flag[i] == 1)
{
ReadOneCmdInfor(i);
motor_one_cmd.tuoji_count = 0;
WriteOneCmdInfor(i);
}
}
WriteMoterInfor();
UART_PutStr(USART1,"#Disable+OK...\r\n");
send_mode &= ~SEND_SET_DISABLEOFFLINE_OK;
}
if (send_mode & SEND_SET_ONLINE_OK)//發送聯機運行狀態
{
UART_PutStr(USART1,"#OK\r\n");
sprintf(buf,"#1GC%d\r\n",tuoji_count);
UART_PutStr(USART1,buf);
UART_PutStr(USART1,"#LP=0\r\n");
send_mode &= ~SEND_SET_ONLINE_OK;
flag_connect_run = 1;
}
if (send_mode & SEND_SET_DELETE_ONE_FILE_OK)//發送刪除文件命令
{
//cur_item.tuoji_count = 0;
if (cur_item.delete_num < motor_data.filecount)
{
motor_data.file_flag[cur_item.delete_num] = 0;
WriteMoterInfor();
updata_file_list();
if (cur_item.delete_num == motor_data.filecount-1)
{
motor_data.filecount = file_last_num + 1;
if (file_last_num == -1)
{
motor_data.sum = 0;
}
else
{
ReadOneCmdInfor(file_last_num);
motor_data.sum = motor_one_cmd.end;
motor_data.sum = (((long int)(motor_data.sum) >>4)<<4) + (1<<4);
}
WriteMoterInfor();
}
updata_file_list();
UART_PutStr(USART1,"#FDel+OK\r\n");
}
send_mode &= ~SEND_SET_DELETE_ONE_FILE_OK;
}
if (send_mode & SEND_SET_DELETE_ALL_FILE_OK)//發送擦除所有文件的命令
{
UART_PutStr(USART1,"#Format+Start\r\n");
SpiFlashEraseChip();
cur_item.tuoji_count = 0;
motor_data.sum = 0;
motor_data.filecount = 0;
memset(motor_data.file_flag,0,sizeof(motor_data.file_flag));
WriteMoterInfor();
UART_PutStr(USART1,"#Format+OK\r\n");
send_mode &= ~SEND_SET_DELETE_ALL_FILE_OK;
updata_file_list();
}
if (send_mode & SEND_SET_PS2_OK)
{
UART_PutStr(USART1,"#PS2+OK...\r\n");
send_mode &= ~SEND_SET_PS2_OK;
}
#define MATHION_HAND_NUM 20
if (send_mode & SEND_SET_READ_UART_MOTOR_ANGLE)
{
UART_PutStr(USART1,"\r\n");
if (cur_item.read_num < MATHION_HAND_NUM)
{
if (flag_uart2_rev)
{
read_motor_num = atoi(uart2_buf + 1);
if (read_motor_num == cur_item.read_num)
{
i = atoi(uart2_buf+5);
sprintf(uart2_buf,"#%dP%d",(int)read_motor_num,i);
UART_PutStr(USART1,uart2_buf);
cur_item.read_num++;
buf[0] = '#';
buf[1] = cur_item.read_num / 100 % 10 + 0x30;
buf[2] = cur_item.read_num / 10 % 10 + 0x30;
buf[3] = cur_item.read_num % 10 + 0x30;
buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
UART_PutStr(USART2,buf);
}
flag_uart2_rev = 0;
count = 0;
}
else
{
count++;
if (count >= 10)
{
cur_item.read_num++;
buf[0] = '#';
buf[1] = cur_item.read_num / 100 % 10 + 0x30;
buf[2] = cur_item.read_num / 10 % 10 + 0x30;
buf[3] = cur_item.read_num % 10 + 0x30;
buf[4] = 'P';buf[5] = 'R';buf[6] = 'A';buf[7] = 'D';buf[8] = '\r';buf[9] = '\n';
UART_PutStr(USART2,buf);
flag_uart2_rev = 0;
flag_uart2_rev_time_out = 0;
count = 0;
}
}
}
else
{
send_mode &= ~SEND_SET_READ_UART_MOTOR_ANGLE;
cur_item.read_num= 0;
UART_PutStr(USART1,"\r\n");
}
}
}
if (send_mode & SEND_SET_SET_UART_MOTOR_PULK)
{
if (cur_item.pulk_num < MATHION_HAND_NUM)
{
count++;
if (count >= 20)
{
sprintf(uart2_buf,"#%dPULK\r\n",(int)cur_item.pulk_num);
UART_PutStr(USART2,uart2_buf);
#if DEBUG
UART_PutStr(USART1,uart2_buf);
#endif
cur_item.pulk_num++;
count = 0;
}
}
else
{
send_mode &= ~SEND_SET_SET_UART_MOTOR_PULK;
}
}
if (send_mode & SEND_SET_SET_UART_MOTOR_ANGLE)
{
if (cur_item.pulk_num < MATHION_HAND_NUM)
{
count++;
if (count >= 5)
{
sprintf(uart2_buf,"#%dPMOD%d\r\n",(int)cur_item.pulk_num,(int)cur_item.angle_mode);
UART_PutStr(USART2,uart2_buf);
#if DEBUG
UART_PutStr(USART1,uart2_buf);
#endif
cur_item.pulk_num++;
count = 0;
}
}
else
{
sprintf(buf,"#255PMOD%d+0K...\r\n",(int)cur_item.angle_mode);
UART_PutStr(USART1,buf);
send_mode &= ~SEND_SET_SET_UART_MOTOR_ANGLE;
flag_uart2_rev = 0;
}
}
if (send_mode & SEND_SET_BEEP_ON)
{
ReadMoterInfor();
motor_data.beep_mode = 1;
//beep_mode = 1;
WriteMoterInfor();
UART_PutStr(USART1,"#FMQENABLE+OK...\r\n");
send_mode &= ~SEND_SET_BEEP_ON;
}
if (send_mode & SEND_SET_BEEP_OFF)
{
ReadMoterInfor();
motor_data.beep_mode = 0;
//beep_mode = 0;
WriteMoterInfor();
UART_PutStr(USART1,"#FMQDISABLE+OK...\r\n");
send_mode &= ~SEND_SET_BEEP_OFF;
}
}
/***************************************************************************************************************
函數名：作業初位置，末尾置更新函數
功能描述：從緩存中取一個新的目標位置替換原來的目標位置，原來的目標位置變為新的初位置，一次更替
：有效的數據是插補增量，和插補次數，知道這兩個量，和當前初位置即可
備注：先進先出，循環訪問
****************************************************************************************************************/
void change(void)
{
unsigned char s;
if(line>0) //緩存中有數據
{
line--; //執行一行
if(line<5) //緩存允許放入新的數據
flag_in=1;
point_now++; //取數位置更新
point_aim++;
if(point_aim==7)
point_aim=0;
if(point_now==7)
point_now=0;
n=pos[point_aim][0]*4/8; //計算新的插補次數
for(s=1;s<MOTOR_NUM;s++) //計算新的插補增量
{
if(pos[point_aim][s]>pos[point_now][s])
{
dp=pos[point_aim][s]-pos[point_now][s];
dp0[s]=dp/n;
}
if(pos[point_aim][s]<=pos[point_now][s])
{
dp=pos[point_now][s]-pos[point_aim][s];
dp0[s]=dp/n;
dp0[s]=-dp0[s];
}
//UART_Put_Inf("dp:",dp);
// UART_Put_Inf("n",n);
}
m=0; //m清0
flag_stop=0; //產生了新的目標位置，停止標志清零
}
else //沒有緩存數據，即line==0
{
flag_out=0; //緩存中沒有數據
}
}
/***************************************************************************************************************
函數名：vpwm()
功能描述：數據插補，插補時間間隔為20/12ms，由timer0控制，使舵機平滑實現速度控制
：另一個功能是執行完一行后去更新下一行數據，即調用change()
備注：
****************************************************************************************************************/
void vpwm(void)
{
unsigned char j=0;
unsigned char how=0;
static unsigned char flag_how;
static unsigned char flag_Tover;
if(flag_stop==1) //一行作業全部完成
{
if(flag_out==1) //緩沖數組中有數據
{
change(); //更新行
}
}
else //不是一行數據全部完成，處于中間插補階段
{
m++; //用來累加插補過的次數
if(m==n) //n是本行作業要插補的總次數
{
flag_Tover=1; //一行數據的執行時間已經完成
send_mode |= SEND_CC;
}
for(j=1;j<MOTOR_NUM;j++)
{
if(abs(CPWM[j]-pos[point_aim][j])<5)
{ //檢測靠近終點位置
how++; //是，則累加一個
CPWM[j]=pos[point_aim][j];//并且直接過度到終點位置
}
else //不靠近終點，繼續插補
{
CPWM[j]=pos[point_now][j]+m*dp0[j];
}
}
if(how==MOTOR_NUM-1)
flag_how=1; //16個舵機都到達終點
how=0;
if((flag_Tover==1)&&(flag_how==1))
{ //從插補次數，和脈寬寬度兩方面都到達終點，本作業行完成
flag_Tover=0;
flag_how=0;
flag_stop=1;
}
}
return;
}
/************************************************************************/
#if 0
void SerVor_Test(void)
{
CPWM[0]=700;CPWM[1]=700;CPWM[2]=700;CPWM[3]=700;CPWM[4]=700;CPWM[5]=700;
Delay_ms(2000);
CPWM[0]=2300;CPWM[1]=2300;CPWM[2]=2300;CPWM[3]=2300;CPWM[4]=2300;CPWM[5]=2300;
Delay_ms(2000);
}
#endif
#if 0
void Ps2_Test(void)
{
key = PS2_DataKey();
if(key!=0)
{
printf(" \r\n %d is pressed \r\n",key);
}
printf(" %5d %5d %5d %5d\r\n",PS2_AnologData(PSS_LX),PS2_AnologData(PSS_LY),
PS2_AnologData(PSS_RX),PS2_AnologData(PSS_RY) );
Delay_ms(1000);
}
#endif
#if 1
void Flash_Test(void)
{
while(SpiFlashReadID()!=W25Q64) //檢測不到W25Q64
{
UART_PutStr (USART1,"Not Found W25Q64\r\n");
}
UART_PutStr (USART1," Found W25Q64\r\n");
SpiFlashWrite((u8*)TEXT_Buffer,FLASH_SIZE-100,SIZE); //從倒數第100個地址寫入數據
UART_PutStr (USART1,"Flash Write Success\r\n");
SpiFlashRead(datatemp,FLASH_SIZE-100,SIZE); //讀取寫入flash中的數據
UART_PutStr (USART1,datatemp);
UART_PutStr (USART1,"\r\n Flash Read Success\r\n");
while(1);
}
#endif

復制代碼

所有資料51hei提供下載:

舵機控制器STM32源程序V1.rar (340.32 KB, 下載次數: 44)

ID:366809 · 發表于 2019-3-6 09:20

樓主有QQ嗎，下載了你的程序有一些疑問想請教

帳號		自動登錄	找回密碼
密碼			立即注冊

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

舵機控制器STM32源程序

評分