/***************************************************************************************************************
文 件 名：main.c
功能描述：
备 注： 16路舵机控制
****************************************************************************************************************/
#include "STC15Fxxxx.H"//STC15系列单片机
#include
#include "UART.H"
#include "timer.h"
#include "util.h"
#include "ps2/ps2.h"
#include "flash/flash.h"//头文件
#include
#include
#include "LED/led.h"
#include
#include
#include "adc/adc.h"
extern uint8 flag_p;
extern bit flag_RecFul;
//extern uchar KEY[9];
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 pwm[MOTOR_NUM]= {1500,1500,1500,1500,1500,1500,1500,1500,1500};
uint16 UartRec[MOTOR_NUM]={1500,1500,1500,1500,1500,1500,1500,1500,1500};
uint8 redata[257] = {0}; // 定义接收数据变量数组
uint8 line=0;//缓存存入口与出口之间的距离，即当前缓存中有多少个没有执行的数据
uint8 point_now=0;//与point_aim一起标记缓存出口位置，即取数位置
uint8 point_aim=1;
uint8 point_in=2;//标记缓存入口位置，即上一行数据存放位置
bit flag_connect = 0;
bit flag_stop=1;//表示一行执行结束
uint8 flag_vpwm=0;//表示到达了该更新pwm[]的时间
bit flag_in=1; //表示缓存中有空闲空间
bit flag_out=0;//表示缓存中有可执行数据的标志位
bit flag_run_ready=0; //表示有要要放入缓存的EErom数据
uint16 n=1000;//用来计算需要建立多少个中间数据
uint16 m=1; //用来累计已经执行了多少中间数据
double dp;
double dp0[MOTOR_NUM] = {0};//插补增量
bit flag_download = 0;//判断是否下载
bit flag_read = 0;// 读取flash内容，发送上位机
bit flag_connect_run = 0;//连接上位机的执行flash保存的命令
bit flag_stop_download = 0;//停止下载
bit flag_online_run = 0;
bit flag_uart2_rev = 0;
bit flag_uart2_rev_time_out = 0;
bit flag_ps2_rev = 0;
bit flag_read_adc = 0;
unsigned long send_mode = 0;//当前串口接收到命令状态存储
MotorData motor_data;//舵机总信息
MotorOneCmd motor_one_cmd;//单个命令
CurrentItem cur_item;
uint16 tuoji_count = 0;//脱机执行次数
bit flag_scan_ps2 = 0;
uint8 error = 0;
uchar file_list[MAX_SUPPORT_FILE_SAVE] = {0};
int file_list_count = 0;
int file_last_num = 0;
char ps2_buf[120] = {0};
char uart2_buf[50] = {0};
uint cur_count = 0;
uchar ad_value = 0;
uchar beep_mode = 1;
uchar key_bak;
uchar ps2_key;
uchar ps2_mode=0;
void updata_file_list()
{
uchar i = 0;
uchar 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()
{
ReadMoterInfor();//读取舵机控制信息
updata_file_list();
memset(&cur_item,0,sizeof(cur_item));
beep_mode = motor_data.beep_mode;
}
/***************************************************************************************************************
函 数 名：主函数
功能描述：入口函数 ，进行各种初始化配置
输入参数：无
返 回 值：无
备 注：
****************************************************************************************************************/
void main(void)
{
uint temp = 0;
P0M1=0x00;//设置P0口为强推挽输出模式
P0M0=0xFF;
P1M1|=0x80;
P1M0|=0xc8;
P5M1|=0x00;
P5M0|=0x2d;
P4M0|=0x20;
P4M1|=0x00;
Timer_init(); //定时器初始化
Timer0(31); //定时任意值，启动定时器，进入定时器循环
#if MOTOR_NUM > 9
Timer1(30);
#endif
SpiFlashInit();//初始化flash
while((temp = SpiFlashReadID())!=W25Q64)LED_ALL_ON();//判断flash有没有接错
LED_ALL_OFF();
InitMotor();
UART1_Init(); //串口1初始化
UART2_Init();//串口2初始化
#if PS_SUPPORT
Timer3_init();
#endif
InitADC(7);
BEEP_On_Or_OFF();
while(1)
{
if(flag_vpwm==1)
{
vpwm();//更新pwm[]数组
flag_vpwm=0;
}
if( flag_RecFul==1) //串口接受完一条指令
{
DealRec();//处理串口缓存中的数据
flag_RecFul=0;
}
GetOneMotorCMD();//获取一个命令
SendUartState();//发送状态信息
#if PS_SUPPORT
scan_ps2();
#endif
LED_State();
Check_Power();
cur_count++;
}
}
void Check_Power()
{
if ((cur_count % 800) == 0)
{
StartADC(7);
//cur_count=0;
}
if (flag_read_adc)
{
flag_read_adc = 0;
//UART_Put_Inf("adc:",ad_value);
if (ad_value > 46)//根据电压粗略估计
{
//UART_Put_Inf("adc:",ad_value);
BEEP_OFF();
// LED2_OFF();
}
else
{
//UART_Put_Inf("adc1111:",ad_value);
BEEP=~BEEP;
LED1=0;
LED2=0;
LED3=0;
}
}
}
void LED_State()
{
uint error_count =0;
if (error != 0)
{
if (error & ERROR_FLASH_FULL)
{
error_count = 100;
}
if (error & ERROR_FLASH_FILE_FULL)
{
error_count += 100;
}
if (error & ERROR_FLASH_WRITE)
{
error_count += 100;
}
if (error & ERROR_FLASH_WRITE1)
{
error_count += 100;
}
if ((cur_count % error_count) == 0)//判断flash是否正确
{
LED1_ON_OR_OFF();
}
}
if (flag_ps2_rev)
{
LED1_ON();
if ((cur_count % 100) == 0)
{
flag_ps2_rev = 0;
LED1_OFF();
}
}
if (flag_connect)
{
LED3_ON();
}
else
{
LED3_OFF();
}
}
void scan_ps2()
{
intkind = 0;
char *p = NULL;
char buf[15] = {0};
char i = 0;
if (flag_scan_ps2)//
{
flag_scan_ps2 = 0;
ps2_key=PS2_DataKey();
ps2_mode=PS2_RedLight();
//UART_Put_Inf("mode",ps2_mode);
if(ps2_mode==0)
{
if(key_bak == ps2_key)return;
key_bak=ps2_key;
BEEP=~BEEP;
switch(ps2_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)<
sprintf(buf,"%dK",kind);
//UART1_SendStr(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;
}
UART1_SendStr(redata);
}
}
}
else if(ps2_mode==1)//绿灯模式
{
switch(ps2_key)
{
case PSB_PAD_UP:pwm[1]+=10;if(pwm[1]>=2300) pwm[1]=2300;break;
case PSB_PAD_DOWN:pwm[1]-=10;if(pwm[1]<=700) pwm[1]=700;break;
case PSB_PAD_LEFT:pwm[2]+=10;if(pwm[2]>=2300) pwm[2]=2300;break;
case PSB_PAD_RIGHT:pwm[2]-=10;if(pwm[2]<=700) pwm[2]=700;break;
case PSB_TRIANGLE:pwm[3]+=10;if(pwm[3]>=2300) pwm[3]=2300;break;
case PSB_CROSS:pwm[3]-=10;if(pwm[3]<=700) pwm[3]=700;break;
case PSB_PINK:pwm[4]+=10;if(pwm[4]>=2300) pwm[4]=2300;break;
case PSB_CIRCLE:pwm[4]-=10;if(pwm[4]<=700) pwm[4]=700;break;
case PSB_L1:pwm[5]+=10;if(pwm[5]>=2300) pwm[5]=2300;break;
case PSB_L2:pwm[5]-=10;if(pwm[5]<=700)pwm[5]=700;break;
case PSB_R1:pwm[6]+=10;if(pwm[6]>=2300) pwm[6]=2300;break;
case PSB_R2:pwm[6]-=10;if(pwm[6]<=700)pwm[6]=700;break;
default:break;
}
}
}
}
/***************************************************************************************************************
函 数 名：从flash读取舵机总的信息
功能描述：初始化舵机控制信息
输入参数：无
返 回 值：无
备 注：
****************************************************************************************************************/
void ReadMoterInfor()
{
memset(&motor_data,0,sizeof(motor_data));//清 0
SpiFlashRead((char *)&motor_data,(CMD_FLASH_ADDR)<
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((char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<
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()
{
uchar temp = 0;
motor_data.CRC1 = 0x12345678;//校验码
motor_data.duoji_count = MOTOR_NUM-1;
temp = motor_data.filecount;
SpiFlashEraseSector(CMD_FLASH_ADDR >> 4);//擦除以前存储的信息
SpiFlashWrite((char *)&motor_data,CMD_FLASH_ADDR<
ReadMoterInfor();
if (temp != motor_data.filecount)
{
error |= ERROR_FLASH_WRITE;
}
else
{
error &= ~ERROR_FLASH_WRITE;
}
}
void WriteOneCmdInfor(unsigned int addr)
{
uchar 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((char *)&motor_one_cmd,((((unsigned long)addr)<<4) + FILE_FLASH_ADDR)<
ReadOneCmdInfor(addr);
if (temp !=motor_one_cmd.end)
{
error |= ERROR_FLASH_WRITE1;
}
else
{
error &= ~ERROR_FLASH_WRITE1;
}
}
/***************************************************************************************************************
函 数 名：获取一串舵机的控制字符串
功能描述：处理从flash读取的舵机控制字符串处理
输入参数：无
返 回 值：无
备 注：
****************************************************************************************************************/
void GetOneMotorCMD()
{
#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]);
UART1_SendStr(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]);
UART1_SendStr(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
{
UART1_SendStr("#Start\r\n");
send_mode &= ~SEND_READ_FILE;
}
memset(redata,0,WRITE_SIZE);//清 0
SpiFlashRead(redata,(((long)cur_item.cur_num)<
#if DEBUG
sprintf(buf,"%d\r\n",cur_item.cur_num);
UART1_SendStr(buf);
#endif
UART1_SendStr(redata);//发送
cur_item.cur_num++;
}
else//否则
{
if (cur_item.cur_num > 0)
UART1_SendStr("#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(redata,((long)cur_item.cur_num)<
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作为结束
{
UART1_SendStr("#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(redata,((long)cur_item.cur_num)<
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()
{
uchar buf[40] = {0};
uchar read_motor_num = 0;
uint i = 0;
static int count = 0;
if (send_mode)//如果有状态需要发送
{
if (send_mode & SEND_A) //发送A
{
UART1_SendOneChar('A');
send_mode &= ~SEND_A;//清状态
}
if (send_mode & SEND_CC) //发送CC
{
UART1_SendStr("#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);
UART1_SendStr(buf);
send_mode &= ~SEND_DOWN_OK;
#if DEBUG
sprintf(buf,"%d\r\n",(uint)motor_data.filecount);
UART1_SendStr(buf);
#endif
}
if (send_mode & SEND_START_OK)//发送连接时候的字符串
{
UART1_SendStr("#Veri+UART+OK+0906+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--
UART1_SendStr("#");//发送
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);//获取命令个数
UART1_SendStr(buf);//发送
}
}
}
UART1_SendStr("\r\n");
}
else
{
sprintf(buf,"#\r\n",motor_data.sum);
UART1_SendStr(buf);
}
send_mode &= ~SEND_READ_FILE;
}
if (send_mode & SEND_SET_OFFLINE_OK)//设置脱机运行次数
{
WriteOneCmdInfor(motor_one_cmd.cur_file_num);//保存
UART1_SendStr("#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();
UART1_SendStr("#Disable+OK...\r\n");
send_mode &= ~SEND_SET_DISABLEOFFLINE_OK;
}
if (send_mode & SEND_SET_ONLINE_OK)//发送联机运行状态
{
UART1_SendStr("#OK\r\n");
sprintf(buf,"#%dGC%d\r\n",cur_item.file_num,tuoji_count);
UART1_SendStr(buf);
UART1_SendStr("#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();
UART1_SendStr("#FDel+OK\r\n");
}
send_mode &= ~SEND_SET_DELETE_ONE_FILE_OK;
}
if (send_mode & SEND_SET_DELETE_ALL_FILE_OK)//发送擦除所有文件的命令
{
UART1_SendStr("#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();
UART1_SendStr("#Format+OK\r\n");
send_mode &= ~SEND_SET_DELETE_ALL_FILE_OK;
updata_file_list();
}
if (send_mode & SEND_SET_PS2_OK)
{
UART1_SendStr("#PS2+OK...\r\n");
send_mode &= ~SEND_SET_PS2_OK;
}
#define MATHION_HAND_NUM 20
if (send_mode & SEND_SET_READ_UART_MOTOR_ANGLE)
{
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);
UART1_SendStr(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';
UART2_SendStr(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';
UART2_SendStr(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;
UART1_SendStr("\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);
UART2_SendStr(uart2_buf);
#if DEBUG
UART1_SendStr(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);
UART2_SendStr(uart2_buf);
#if DEBUG
UART1_SendStr(uart2_buf);
#endif
cur_item.pulk_num++;
count = 0;
}
}
else
{
sprintf(buf,"#255PMOD%d+0K...\r\n",(int)cur_item.angle_mode);
UART1_SendStr(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();
UART1_SendStr("#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();
UART1_SendStr("#FMQDISABLE+OK...\r\n");
send_mode &= ~SEND_SET_BEEP_OFF;
}
}
/***************************************************************************************************************
函 数 名：作业初位置，末尾置更新函数
功能描述：从缓存中取一个新的目标位置替换原来的目标位置，原来的目标位置变为新的初位置，一次更替
：有效的数据是插补增量，和插补次数，知道这两个量，和当前初位置即可
备 注： 先进先出，循环访问
****************************************************************************************************************/
void change(void)
{
uchar 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/5; //计算新的插补次数
for(s=1;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];
}
}
m=0;//m清0
flag_stop=0; //产生了新的目标位置，停止标志清零
}
else //没有缓存数据，即line==0
{
flag_out=0;//缓存中没有数据
}
}
/***************************************************************************************************************
函 数 名：vpwm()
功能描述：数据插补，插补时间间隔为20/12ms，由timer0控制，使舵机平滑实现速度控制
：另一个功能是执行完一行后去更新下一行数据，即调用change()
备 注：
****************************************************************************************************************/
void vpwm(void)
{
uchar j=0;
uchar how=0;
static uchar flag_how;
static uchar 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
{
if(abs(pwm[j]-pos[point_aim][j])<5)
{ //检测靠近终点位置
how++; //是，则累加一个
pwm[j]=pos[point_aim][j];//并且直接过度到终点位置
}
else //不靠近终点，继续插补
pwm[j]=pos[point_now][j]+m*dp0[j];
}
//UART_Put_Inf("pwm",pwm[1]);
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;
}
