国内精品1区2区3区思瑞,欧美高清精品一区二区不卡,国产精品一区二区高清在线观看

/***************************************************/
/* 尋跡小車 FollowMe 項目 */
/* —— 主控程序軌跡控制模塊 */
/* 之程序部分 */
/* 20060905 */
/* By DingQi */
/***************************************************/
// 注：以下文檔的 TAB 為 2 個字符！
/*------------------------------------------------------------------------
此程序為"尋跡小車FollowMe"項目中單板控制模式的走軌跡控制部分，附帶相關(guān)調(diào)試功能。
要實現(xiàn)：
1)接收各種調(diào)試命令，并解析；
2)通過串口反饋所需的調(diào)試信息；
3)獲取軌跡采樣部分處理后的信息，產(chǎn)生對策，發(fā)給電機驅(qū)動部分實施。
根據(jù)上述要實現(xiàn)的功能，通訊部分歸此模塊管理。
第一步先將原來的電機驅(qū)動功能整合到一個MCU中，將原來的通訊功能從電機驅(qū)動模塊中分解出來。
目前的電機控制由串口實現(xiàn)，通訊協(xié)議定義如下：
1、幀格式：
幀頭（2字節(jié)）幀長（1字節(jié)）命令字（1字節(jié)）數(shù)據(jù)區(qū)（N字節(jié)）校驗和（1字節(jié)）
其中：
幀頭 —— 0x55 0xAA
幀長 —— 命令字 + 數(shù)據(jù)區(qū)的長度
命令字 —— 0x01 ：電機轉(zhuǎn)動控制參數(shù)，開環(huán)模式，電機的PWM值、轉(zhuǎn)動持續(xù)脈沖數(shù)；
0x02 ：電機轉(zhuǎn)動控制參數(shù)，閉環(huán)模式，電機的轉(zhuǎn)速、轉(zhuǎn)動持續(xù)脈沖數(shù)；
0x03 ：電機工作參數(shù)，PWM頻率、PID參數(shù)
數(shù)據(jù)區(qū) —— 命令01：電機1數(shù)據(jù)（2字節(jié)PWM值，2字節(jié)轉(zhuǎn)動持續(xù)脈沖數(shù)）電機2數(shù)據(jù)（2字節(jié)PWM值，2字節(jié)轉(zhuǎn)動持續(xù)脈沖數(shù)），共 8字節(jié)；
命令02：電機1數(shù)據(jù)（2字節(jié)轉(zhuǎn)速值，2字節(jié)轉(zhuǎn)動持續(xù)脈沖數(shù)）電機2數(shù)據(jù)（2字節(jié)轉(zhuǎn)速值，2字節(jié)轉(zhuǎn)動持續(xù)脈沖數(shù)），共 8字節(jié)；
命令03：2字節(jié)PWM頻率，2字節(jié)比例系數(shù)，2字節(jié)積分系數(shù)，2字節(jié)微分系數(shù)，2字節(jié)PID系數(shù)的分母，共10字節(jié)，兩個電機驅(qū)動器相同；
校驗和 —— 從命令字開始到數(shù)據(jù)區(qū)結(jié)束所有字節(jié)的算術(shù)和的反碼，取低字節(jié)。
上述數(shù)據(jù)中，PWM值，速度值、PWM頻率、PID系數(shù)等定義如下：
PWM值 —— 2字節(jié)有符號數(shù)，正對應(yīng)正轉(zhuǎn)，負(fù)對應(yīng)反轉(zhuǎn)，數(shù)值為占空比的百分?jǐn)?shù)，
取值范圍：- 1000 —— +1000，對應(yīng) 0.1% ~ 100%；1001為電機“惰行”，1002為“剎車”；
轉(zhuǎn)動持續(xù)脈沖數(shù) —— 2字節(jié)無符號數(shù)，0 表示連續(xù)轉(zhuǎn)動；
轉(zhuǎn)速值 —— 2字節(jié)有符號數(shù)，正對應(yīng)正轉(zhuǎn)，負(fù)對應(yīng)反轉(zhuǎn)，單位為：0.1轉(zhuǎn)/每分鐘；
取值范圍：- 10000~ +10000，10001為電機“惰行”，10002為“剎車”；
PWM頻率 —— 2字節(jié)整數(shù)，單位Hz，取值范圍：200 – 2000；
PID系數(shù) —— 均為 2字節(jié)無符號數(shù)；
PID系數(shù)分母 —— 2字節(jié)無符號數(shù)，為避免使用浮點數(shù)而增加了此參數(shù)，實際作用的PID系數(shù)為上述值除此值；
如：比例系數(shù)為190 ，PID分母為200，實際比例系數(shù)為0.95。
以上所有2字節(jié)的數(shù)據(jù)均為先低后高。
暫時不設(shè)計應(yīng)答幀，因為一幀命令包含了兩個電機的驅(qū)動數(shù)據(jù)。
通訊數(shù)據(jù)格式為：19200 8 N 1。
此時，一幀數(shù)據(jù)約占 7ms。
為了調(diào)試，添加轉(zhuǎn)速讀取命令 0x04 ,原來的讀轉(zhuǎn)速命令是分開實現(xiàn)的：
0x55 0xAA 0x02（幀長） 0x04 (讀轉(zhuǎn)速命令) 電機序號（1字節(jié)）校驗和（1字節(jié)）
對應(yīng)的返回幀為：
0xAA 0x55 0x04（幀長） 0x84 (轉(zhuǎn)速值返回) 電機序號（1字節(jié)）轉(zhuǎn)速（2字節(jié)）校驗和（1字節(jié)）
因為合并到一個MCU中了，所以對應(yīng)將協(xié)議改為：
0x55 0xAA 0x01（幀長） 0x04 (讀轉(zhuǎn)速命令) 校驗和（1字節(jié)）
對應(yīng)的返回幀為：
0xAA 0x55 0x05（幀長） 0x84 (轉(zhuǎn)速值返回) 電機1轉(zhuǎn)速（2字節(jié)）電機2轉(zhuǎn)速（2字節(jié)）校驗和（1字節(jié)）
因為集成了走軌跡控制功能，所以要添加一個控制命令，使小車啟動，進入到走軌跡狀態(tài)或結(jié)束走軌跡的狀態(tài)。
走軌跡控制命令： 0x05 . 命令參數(shù) —— 1 啟動走軌跡， 2 —— 啟動走直線，0 停止,
命令幀為：
0x55 0xAA 0x02 0x05 0x01 CS —— 啟動走軌跡命令
0x55 0xAA 0x02 0x05 0x02 CS —— 啟動直線走命令
0x55 0xAA 0x02 0x05 0x00 CS —— 停止命令
為了調(diào)試方便，增加一個內(nèi)存數(shù)據(jù)讀取命令：
內(nèi)存數(shù)據(jù)讀取命令：0x06
命令幀為：
0x55 0xAA 0x04 0x06 讀數(shù)據(jù)低地址讀數(shù)據(jù)高地址讀數(shù)據(jù)長度 CS
返回幀為：
0x55 0xAA 幀長 0x86 讀數(shù)據(jù)低地址讀數(shù)據(jù)高地址讀數(shù)據(jù)長度數(shù)據(jù)N字節(jié) CS
------------------------------------------------------------------------
因為用雙輪驅(qū)動的小車由于電機等驅(qū)動元素的差異，導(dǎo)致走直線成為問題，故在此嘗試
用這兩個簡易的碼盤來實現(xiàn)直線行走。
因為碼盤的分辨率太低，所以直接用脈沖計數(shù)方式控制似乎有些不夠精確，所以考慮用
兩路脈沖的觸發(fā)時間差別來控制。
實質(zhì)上這是一種周期測量的變換，因為不能保證每個脈沖周期是真實的（即由于干擾會
導(dǎo)致某個周期變大或變小），所以用累計的方式消除之。
具體的方式如下：
設(shè)立兩個 4 字節(jié)的計數(shù)器，2字節(jié)紀(jì)錄PCA的溢出值，2字節(jié)紀(jì)錄 PCA 的計時器值，這樣
構(gòu)成了一個對PCA計數(shù)脈沖（Fosc/2)的長整形計數(shù)器，可紀(jì)錄約 388秒（2^32/11.0592M)，
這個值一般可以應(yīng)付大部分比賽項目中的需要。
將這個時間計數(shù)器作為兩個輪子采樣脈沖（只取下降沿）的時標(biāo)，根據(jù)兩者的差值確定兩輪
的驅(qū)動差。也就是要保證兩個輪子對應(yīng)脈沖到達的時間相同，這樣，如果沒有漏計或打滑，兩
個輪子行走的距離應(yīng)當(dāng)是一樣的，軌跡也應(yīng)當(dāng)是直線！
這個控制也可以考慮使用PID控制，其控制的量為兩個計數(shù)器的差值，定值為“0”。
------------------------------------------------------------------------*/
#pragma PR
#pragma OT(5,size)
#pragma Listinclude
#pragma code
#include <E:\dingqi\keilc51\inc\math.h>
#include <STC12C5410AD.h> /* STC12C5410AD 的頭文件*/
#include <Port_Def.H>
#include <ComConst.H>
#include <LC_Const.H>
#include <LC_Var.H>
void init_SIO(unsigned char baud); // 初始化串口
void rcvdata_proc(void); // 接收數(shù)據(jù)幀
void getCommandData(void); // 從數(shù)據(jù)幀中取出命令數(shù)據(jù)
unsigned int calStopCntValue(unsigned int uiRun_Num,unsigned char No); // 根據(jù)命令中的行走脈沖數(shù)計算出停止點
char setMotorStat(int iRunValue); // 根據(jù)命令中的PWM值或轉(zhuǎn)速設(shè)置電機狀態(tài)
void followLineControl(void); // 走軌跡控制
void straightRun(void); // 走直線控制
/******************************** 外部調(diào)用函數(shù) *********************************/
// 以下函數(shù)為公共函數(shù)，需要在調(diào)用的模塊中聲明。
/********************************************/
/* 名稱：init_LineCtrl_Hardware */
/* 用途：初始化串口等, 以保證相應(yīng)硬件工作 */
/********************************************/
void init_LineCtrl_Hardware(void)
{
//初始化串口
init_SIO(B_19200);
// 初始化相關(guān)中斷
IE = IE|EnUART_C; // 允許 UART 中斷
}
/********************************************/
/* 名稱：init_LineCtrl_Var */
/* 用途：初始化自身工作變量 */
/********************************************/
void init_LineCtrl_Var(void)
{
unsigned char j;
// 接收數(shù)據(jù)變量初始化
gi_ucSavePtr=0;
gi_ucGetPtr=0;
gb_NewData = FALSE;
gb_StartRcv = FALSE;
gb_DataOK = FALSE;
// 命令數(shù)據(jù)存放單元初始化
for(j=0;j<2;j++)
{
ga_iPWM_Value[j] = FLOAT_PWM;
ga_iRotateSpeed[j] = FLOAT_SPEED;
ga_uiRotateNum[j] = 0;
}
g_uiPWM_Freq = INIT_PWM_FREQ; // 初始化時，將PWM的頻率置為 200Hz
gb_M1CalOutValue = TRUE; // 上電計算一次輸出，以保證電機的正常工作狀態(tài)
gb_M2CalOutValue = TRUE; // 上電計算一次輸出，以保證電機的正常工作狀態(tài)
// PID 控制初始化
g_uiKp = DEFAULT_KP; // 加載默認(rèn)系數(shù)
g_uiTi = DEFAULT_TI;
g_uiTd = DEFAULT_TD;
g_uiPID_Ratio = DEFAULT_PID_RATIO;
g_fKp = ((float)g_uiKp)/g_uiPID_Ratio; // 在此處計算好，減少每次 PID 的運算量
g_fTi = ((float)g_uiTi)/g_uiPID_Ratio;
g_fTd = ((float)g_uiTd)/g_uiPID_Ratio;
gb_EnablePID = FALSE; // 禁止調(diào)速 PID 功能
gb_StartLineFollow = FALSE;
gb_StartStraightRun = FALSE;
g_ucDownSampCnt = 0; // 初始化時將脈沖采樣計數(shù)清為“0”
}
/********************************************/
/* 名稱：lineCtrl_proc */
/* 用途：軌跡控制部分處理入口函數(shù)，根據(jù)帶入 */
/* 的消息作相應(yīng)處理。 */
/*入口參數(shù)：要處理的消息 */
/********************************************/
void lineCtrl_proc(unsigned char ucMessage)
{
switch(ucMessage)
{
case NEW_RCV_DATA:
{
rcvdata_proc(); // 處理接收緩沖區(qū)數(shù)據(jù)
if(gb_DataOK)
{
gb_DataOK = FALSE;
getCommandData(); // 從數(shù)據(jù)幀中提取命令數(shù)據(jù)
}
break;
}
case NEW_SAMP_DATA:
{
followLineControl();
break;
}
case SAMPLE_DOWN_PULS:
{
straightRun();
break;
}
default: break;
}
}
/***************************** 模塊自用函數(shù) *******************************/
// 以下函數(shù)只由模塊自身使用，別的模塊不用聲明。
/********************************************/
/* 名稱：init_SIO */
/* 用途：初始化串口, */
/* 參數(shù): 波特率 , 模式固定為:1 */
/* 1 START 8 DATA 1 STOP */
/********************************************/
void init_SIO(unsigned char baud)
{
// 波特率表
unsigned char code TH_Baud[5]={B4800_C,B9600_C,B19200_C,B38400_C,B57600_C};
AUXR = AUXR|SET_T1X12_C;
TH1 = TH_Baud[baud];
TL1 = TH_Baud[baud];
TR1 = TRUE;
SCON = UART_MODE1_C|EN_RCV_C; // 8 位模式( MODE 1)
}
/********************************************/
/*名稱: rcvdata_proc */
/*用途: 檢測接收緩沖區(qū)數(shù)據(jù)， */
/*說明: 如果收到正確的數(shù)據(jù)幀則建立標(biāo)志 */
/********************************************/
void rcvdata_proc(void)
{
unsigned char i,j,k;
if(gb_StartRcv == FALSE)
{
/* 檢測幀頭 0x55 0xAA LEN */
i=(gi_ucGetPtr-2)&(MaxRcvByte_C-1); // 指向0x55
j=(gi_ucGetPtr-1)&(MaxRcvByte_C-1); // 指向0xAA
if((ga_ucRcvBuf[i]==0x55)&&(ga_ucRcvBuf[j]==0xAA))
{
i=gi_ucGetPtr;
if(ga_ucRcvBuf[i]<= (MaxRcvByte_C-1));
{
//幀頭正確，啟動數(shù)據(jù)區(qū)接收
gb_StartRcv=TRUE;
gc_ucDataLen=ga_ucRcvBuf[i];
gi_ucStartPtr=(gi_ucGetPtr+1)&(MaxRcvByte_C-1);
gi_ucEndPtr= (gi_ucGetPtr + gc_ucDataLen+1)&(MaxRcvByte_C-1);
}
}
gi_ucGetPtr=(gi_ucGetPtr+1)&(MaxRcvByte_C-1);
}
else
{
//開始接收數(shù)據(jù)處理
if(gi_ucGetPtr==gi_ucEndPtr)
{
/* 數(shù)據(jù)幀接收完 */
gb_StartRcv=FALSE;
j=gi_ucStartPtr;
k= 0;
for(i=0;i<gc_ucDataLen;i++)
{
// 計算CS
k +=ga_ucRcvBuf[j];
j=(j+1)&(MaxRcvByte_C-1);
}
// 取校驗和
k +=ga_ucRcvBuf[j];
if( k == 0xFF)
{
// 數(shù)據(jù)校驗正確
gb_DataOK=TRUE;
}
}
gi_ucGetPtr=(gi_ucGetPtr+1)&(MaxRcvByte_C-1);
}
}
/********************************************/
/*名稱: getCommandData */
/*用途: 從接收緩沖區(qū)中取出數(shù)據(jù)， */
/*說明: 建立對應(yīng)標(biāo)志，通知相應(yīng)的處理 */
/********************************************/
void getCommandData(void)
{
union
{
unsigned int all;
unsigned char b[2];
}uitemp;
union
{
int all;
unsigned char b[2];
}itemp;
unsigned char ucCommand,i,j,sum,n;
unsigned char idata *ucI_Ptr;
unsigned char xdata *ucX_Ptr;
ucCommand = ga_ucRcvBuf[gi_ucStartPtr]; // 取出數(shù)據(jù)幀中的命令字
switch (ucCommand)
{
case PWM_MODE:
{
// 處理PWM開環(huán)控制命令
i = (gi_ucStartPtr + 1)&(MaxRcvByte_C - 1); // 指向電機 1 數(shù)據(jù)區(qū)
for(j=0;j<2;j++) // 循環(huán) 2 次完成兩個電機的數(shù)據(jù)提取
{
itemp.b[1] = ga_ucRcvBuf[i]; // 注意，在C51中，整形等多字節(jié)數(shù)據(jù)在內(nèi)存中是先高后低存放！
i =(i+1)&(MaxRcvByte_C-1);
itemp.b[0] = ga_ucRcvBuf[i];
if(itemp.all < (-1000)) // PWM值合法性處理
{
itemp.all = -1000;
}
if(itemp.all > 1002)
{
itemp.all = 1000;
}
ga_iPWM_Value[j] = itemp.all; // 得到 PWM 值
// 行走脈沖計數(shù)數(shù)據(jù)處理
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i];
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
ga_uiRotateNum[j] = uitemp.all; // 得到轉(zhuǎn)動脈沖計數(shù)值
ga_uiStopCnt[j] = calStopCntValue(ga_uiRotateNum[j],j); // 計算出停止計數(shù)值
ga_cMotorStat[j] = setMotorStat(ga_iPWM_Value[j]); // 根據(jù)命令設(shè)置電機運轉(zhuǎn)標(biāo)志
i = (gi_ucStartPtr + 1+4)&(MaxRcvByte_C - 1); // 指向電機 2 數(shù)據(jù)區(qū)
}
gb_EnablePID = FALSE; // 收到PWM控制命令后，禁止 PID 控制
gb_M1CalOutValue =TRUE; // 建立計算電機控制輸出值標(biāo)志，因為PWM數(shù)據(jù)變化
gb_M2CalOutValue =TRUE;
break;
}
case SPEED_MODE:
{
// 處理轉(zhuǎn)速閉環(huán)控制命令
i = (gi_ucStartPtr + 1 )&(MaxRcvByte_C-1); // 指向電機 1 數(shù)據(jù)區(qū)
for(j=0;j<2;j++) // 循環(huán) 2 次完成兩個電機的數(shù)據(jù)提取
{
itemp.b[1] = ga_ucRcvBuf[i]; // 注意，在C51中，整形等多字節(jié)數(shù)據(jù)在內(nèi)存中是先高后低存放！
i =(i+1)&(MaxRcvByte_C-1);
itemp.b[0] = ga_ucRcvBuf[i];
if(itemp.all < (-10000)) // 轉(zhuǎn)速數(shù)據(jù)合法性處理
{
itemp.all = -10000;
}
if(itemp.all > 10002)
{
itemp.all = 10000;
}
ga_iRotateSpeed[j] = itemp.all; // 得到轉(zhuǎn)速
// 行走脈沖數(shù)據(jù)處理
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i];
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
ga_uiRotateNum[j] = uitemp.all; // 得到轉(zhuǎn)動脈沖計數(shù)值
ga_uiStopCnt[j] = calStopCntValue(ga_uiRotateNum[j],j); // 計算出停止計數(shù)值
ga_cMotorStat[j] = setMotorStat(ga_iRotateSpeed[j]); // 根據(jù)命令設(shè)置電機運轉(zhuǎn)標(biāo)志
i = (gi_ucStartPtr + 1 + 4)&(MaxRcvByte_C-1); // 指向電機 2 數(shù)據(jù)區(qū)
}
if(gb_EnablePID)
{
// 已啟動PID控制
}
else
{
// 啟動 PID 控制
gb_EnablePID = TRUE;
gac_ucGetSpeedCnt[MOTOR1] = 3; // 電機 1 采集3次速度數(shù)據(jù)后才允許計算PID
gac_ucGetSpeedCnt[MOTOR2] = 3; // 電機 2 采集3次速度數(shù)據(jù)后才允許計算PID
ga_iPWM_Value[MOTOR1] = INI_PWM_VALUE; // 電機 1 輸出PWM初值，啟動電機
ga_iPWM_Value[MOTOR2] = INI_PWM_VALUE; // 電機 2 輸出PWM初值，啟動電機
gb_M1CalOutValue = TRUE; // 通知輸出計算
gb_M2CalOutValue = TRUE;
}
break;
}
case SET_PARA:
{
// 處理參數(shù)設(shè)置命令
i = (gi_ucStartPtr + 1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i]; // 注意，在C51中，整形等多字節(jié)數(shù)據(jù)在內(nèi)存中是先高后低存放！
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
g_uiPWM_Freq = uitemp.all;
if(g_uiPWM_Freq <200) // 數(shù)據(jù)合法性處理
{
g_uiPWM_Freq = 200;
}
if(g_uiPWM_Freq >2000)
{
g_uiPWM_Freq = 2000;
}
gb_M1CalOutValue =TRUE; // 建立計算電機控制輸出值標(biāo)志，因為PWM的頻率變了。
gb_M2CalOutValue =TRUE;
// 取 PID 參數(shù)
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i];
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
g_uiKp = uitemp.all;
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i];
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
g_uiTi = uitemp.all;
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i];
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
g_uiTd = uitemp.all;
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i];
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
if(uitemp.all >0)
{
g_uiPID_Ratio = uitemp.all;
}
g_fKp = ((float)g_uiKp)/g_uiPID_Ratio; // 在此處計算好，減少每次 PID 的運算量
g_fTi = ((float)g_uiTi)/g_uiPID_Ratio;
g_fTd = ((float)g_uiTd)/g_uiPID_Ratio;
break;
}
case READ_SPEED:
{
// 讀取轉(zhuǎn)速命令處理
ga_ucTxdBuf[0] = 0xAA;
ga_ucTxdBuf[1] = 0x55; // 幀頭
ga_ucTxdBuf[2] = 0x05; // 幀長
ga_ucTxdBuf[3] = 0x80+READ_SPEED; // 返回命令
sum = ga_ucTxdBuf[3];
i=4;
for(j=0;j<2;j++) // 循環(huán) 2 次，返回 2 個電機的轉(zhuǎn)速
{
itemp.all = ga_iCurSpeed[j];
ga_ucTxdBuf[i] = itemp.b[1]; // 返回轉(zhuǎn)速值，先低后高
sum += ga_ucTxdBuf[i];
i++;
ga_ucTxdBuf[i] = itemp.b[0];
sum += ga_ucTxdBuf[i];
i++;
}
ga_ucTxdBuf[i] = ~sum; // 校驗和
gc_ucTxdCnt = 9; // 發(fā)送字節(jié)計數(shù)
gi_ucTxdPtr = 0; // 發(fā)送指針
SBUF = ga_ucTxdBuf[0]; // 啟動發(fā)送
break;
}
case FOLLOW_LINE_CTRL:
{
i = (gi_ucStartPtr + 1)&(MaxRcvByte_C-1);
switch (ga_ucRcvBuf[i])
{
case FOLLOW_LINE:
{
break;
}
case STRAIGHT_RUN:
{
gb_StartStraightRun = TRUE;
gc_uiPCA_OverCnt = 0;
g_ucDownSampCnt = 0;
//gb_EnSpeed_Hi_Low = TRUE; // 啟動速度上下限控制
g_iInit_PWM = INI_PWM_VALUE; // 啟動電機
ga_iPWM_Value[MOTOR1] = g_iInit_PWM;
ga_cMotorStat[MOTOR1] = setMotorStat(ga_iPWM_Value[MOTOR1]); // 設(shè)置電機運轉(zhuǎn)標(biāo)志
ga_iPWM_Value[MOTOR2] = g_iInit_PWM;
ga_cMotorStat[MOTOR2] = setMotorStat(ga_iPWM_Value[MOTOR2]); // 設(shè)置電機運轉(zhuǎn)標(biāo)志
m_iDiffPWM = 0;
gb_M1CalOutValue =TRUE; // 建立計算電機控制輸出值標(biāo)志，
gb_M2CalOutValue =TRUE;
m_iError_Int = 0; // 初始化PID計算數(shù)據(jù)
m_iErrorOld = 0;
break;
}
case STOP_RUN:
{
ga_iPWM_Value[MOTOR1] = BRAKE_PWM;
ga_cMotorStat[MOTOR1] = setMotorStat(ga_iPWM_Value[MOTOR1]); // 設(shè)置電機運轉(zhuǎn)標(biāo)志
ga_iPWM_Value[MOTOR2] = BRAKE_PWM;
ga_cMotorStat[MOTOR2] = setMotorStat(ga_iPWM_Value[MOTOR2]); // 設(shè)置電機運轉(zhuǎn)標(biāo)志
gb_EnSpeed_Hi_Low = FALSE;
gb_StartStraightRun = FALSE;
gb_StartLineFollow = FALSE;
gb_M1CalOutValue =TRUE; // 建立計算電機控制輸出值標(biāo)志，
gb_M2CalOutValue =TRUE;
break;
}
default: break;
}
break;
}
case READ_MEMORY:
{
// 讀內(nèi)存數(shù)據(jù)處理
i = (gi_ucStartPtr + 1)&(MaxRcvByte_C-1);
uitemp.b[1] = ga_ucRcvBuf[i]; // 取讀數(shù)據(jù)地址
i =(i+1)&(MaxRcvByte_C-1);
uitemp.b[0] = ga_ucRcvBuf[i];
i =(i+1)&(MaxRcvByte_C-1);
n = ga_ucRcvBuf[i]; // 取讀數(shù)據(jù)長度
if(n>(MaxTxdByte_C - 4))
{
n = (MaxTxdByte_C - 4); // 受發(fā)送緩沖區(qū)限制，減 4 個字節(jié)對應(yīng)：命令地址長度
}
ga_ucTxdBuf[0] = 0xAA;
ga_ucTxdBuf[1] = 0x55; // 幀頭
ga_ucTxdBuf[2] = n + 4; // 幀長
ga_ucTxdBuf[3] = 0x80+READ_MEMORY; // 返回命令
ga_ucTxdBuf[4] = uitemp.b[1]; // 將要讀數(shù)據(jù)的地址和長度返回
ga_ucTxdBuf[5] = uitemp.b[0];
ga_ucTxdBuf[6] = n;
sum = ga_ucTxdBuf[3]+ga_ucTxdBuf[4]+ga_ucTxdBuf[5]+ga_ucTxdBuf[6];
i = 7; // 數(shù)據(jù)區(qū)起始指針
if(uitemp.b[0] == 0)
{
ucI_Ptr = uitemp.b[1]; // 如果高地址為 0 ，則讀IDATA內(nèi)容
for(j=0;j<n;j++)
{
ga_ucTxdBuf[i] = *ucI_Ptr;
i++;
ucI_Ptr++;
}
}
else
{
ucX_Ptr = uitemp.b[1]; // 如果高地址不為“0”，則讀XDATA內(nèi)容，因為只有256字節(jié)的XDATA，所以只取低字節(jié)。
for(j=0;j<n;j++)
{
ga_ucTxdBuf[i] = *ucX_Ptr;
i++;
ucX_Ptr++;
}
}
ga_ucTxdBuf[i] = ~sum; // 校驗和
gc_ucTxdCnt = i+1; // 發(fā)送字節(jié)計數(shù)
gi_ucTxdPtr = 0; // 發(fā)送指針
SBUF = ga_ucTxdBuf[0]; // 啟動發(fā)送
break;
}
default:
{
break;
}
}
}
/********************************************/
/*名稱: calStopCntValue */
/*用途: 根據(jù)得到的行走脈沖數(shù)計算出停止點 */
/********************************************/
unsigned int calStopCntValue(unsigned int uiRun_Num,unsigned char No)
{
unsigned int cnt1;
if(uiRun_Num !=0)
{
cnt1 = gac_uiPulsCnt[No];
while(cnt1 != gac_uiPulsCnt[No])
{
cnt1 = gac_uiPulsCnt[No]; // 防護處理，避免正好在PCA中斷時取數(shù)
}
cnt1 = cnt1 + uiRun_Num; // 得到停止的判斷點
}
else
{
cnt1 = 65535; // g_uiRotateNum =0；設(shè)置為最大值，永不停止
}
return(cnt1);
}
/*********************************************/
/*名稱: setMotorStat */
/*用途: 根據(jù)命令中的PWM值或轉(zhuǎn)速值設(shè)置電機狀態(tài)*/
/*********************************************/
char setMotorStat(int iRunValue)
{
char stat;
switch (iRunValue)
{
case 0:
{
stat = IN_STOP;
break;
}
case FLOAT_PWM:
{
stat = IN_STOP;
break;
}
case BRAKE_PWM:
{
stat = IN_STOP;
break;
}
case FLOAT_SPEED:
{
stat = IN_STOP;
break;
}
case BRAKE_SPEED:
{
stat = IN_STOP;
break;
}
default:
{
if(iRunValue >0)
{
stat = IN_FORWARD;
}
else
{
stat = IN_BACKWARD;
}
break;
}
}
return(stat);
}
/*********************************************/
/*名稱: followLineControl */
/*用途: 根據(jù)采樣輸出值g_cSampleOut 控制尋跡 */
/*********************************************/
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復(fù)制代碼

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136