一二三四在线观看免费高清中文在线观看 ,婷婷开心久久网,精品按摩偷拍

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

復制代碼

wwwxxx国产_337p日本欧洲亚洲大胆张筱雨_免费在线看成人av_日本黄色不卡视频_国产精品成熟老女人_99视频一区_亚洲精品97久久中文字幕_免费精品视频在线_亚洲色图欧美视频_欧美一区二三区