|
|
控制程序源代碼
void DodgeEdge(void)
{
FindEdge(); // 開始檢測(cè)邊緣
while(g_edge_find_flag) // 如果到達(dá)邊緣
{
MoveBack(15); // 機(jī)器人后退
FindEdge(); // 再次檢測(cè)邊緣
}
}
檢測(cè)敵人：
temp16 = read_gpio(); // 紅外傳感器觸發(fā)時(shí)輸出為低
io_in = (uint8)(temp16>>8);
io_in &= (FIND_ENEMY_SENSOR); // #define FIND_ENEMY_SENSOR 0x80
if((io_in & FIND_ENEMY_SENSOR) == 0) // 檢測(cè)傳感器如檢測(cè)到輸入則再檢測(cè)4次防止誤判斷
{
temp8++;
or(i = 0 ; i < 3 ; i++) // 120ms內(nèi)檢測(cè)再3次傳感器累加值大于2即說(shuō)明傳感器輸入有效
{delay(2); // 延時(shí)2*20=40ms
temp16 = read_gpio();
io_in = (uint8)(temp16>>8);
io_in &= (FIND_ENEMY_SENSOR); // 默認(rèn)傳感器觸發(fā)時(shí)輸出為低
if((io_in & FIND_ENEMY_SENSOR) == 0)
{
temp8++;
}
}
}
碰撞檢測(cè)：
FindEnemy(); // 開始檢測(cè)敵人
if(g_enemy_find_flag == 1) //前超聲傳感器檢測(cè)到敵人
{
g_enemy_find_times = 0; //前超聲傳感器檢測(cè)敵人次數(shù)初始為0
DodgeEdge(); // 開始檢測(cè)邊緣
MoveOn(20); // 前進(jìn)驅(qū)趕敵人
FindEnemy();
}
else //即前后超聲傳感器都沒檢測(cè)到敵人
{
DodgeEdge(); // 開始檢測(cè)邊緣
TurnRight(3); // 轉(zhuǎn)動(dòng)一個(gè)角度
delay(10);
FindEnemy();
}
武術(shù)擂臺(tái)賽機(jī)器人程序主函數(shù)說(shuō)明如下 int main(void)
{
Sys_Init(); // 系統(tǒng)初始化
gpio_mode_set(IO_MODE); // 設(shè)置io口模式IO0-IO7全為輸入IO8-IO15全為輸出(1為輸出0為輸入)
write_gpio(g_io_value); // 設(shè)置io輸出值及輸入狀態(tài)IO0-IO7輸入使能由于電路原因。
// 輸出為低時(shí)led才會(huì)亮IO8-IO15輸出信號(hào)為低0。
g_find_redblock_mode = 1; //進(jìn)入尋心模式
delay(10); //延時(shí)10x0.1s=1s
武術(shù)擂臺(tái)攻擊型機(jī)器人控制系統(tǒng)設(shè)計(jì)
30 FindRedBlockFirstTime();
while(TRUE) // TRUE,FALSE
{
while(g_find_redblock_mode) //進(jìn)入尋心模式
{
FindblackBlock(); //尋找擂臺(tái)黑色圓心
}
while(g_anti_enemy_mode) //進(jìn)入尋找敵人攻擊敵人模式
{
AntiEnemyAction(); //尋找并攻擊敵人
}
}
while(FALSE) // 單模式調(diào)試用
{
AntiEnemyAction(); //尋找并攻擊敵人
}
while(FALSE) // 單模式調(diào)試用
{
FindblackBlock(); //尋找擂臺(tái)黑色圓心
}
}
機(jī)器人前行與后退;
void MoveOn(uint8 move_time) / /機(jī)器人前行
{
uint8 array_dc[8] = {0}; //定義機(jī)器人運(yùn)動(dòng)數(shù)組
array_dc[0] = 0;
array_dc[1] = 1 * move_time; //電機(jī)運(yùn)動(dòng)函數(shù)的分辨率為0.1s注意控制延時(shí)的長(zhǎng)短
array_dc[2] = 0xFE; //左右前輪為12電機(jī)左右后輪為三四電機(jī)兩兩速度相反 rray_dc[3] = 1 * move_time;
array_dc[4] = 0;
array_dc[5] = 1 * move_time;
rray_dc[6] = 0xFE;
array_dc[7] = 1 * move_time;
dc_moto_control(array_dc);
delay(5 * move_time);
}
void MoveBack(uint8 move_time) //機(jī)器人后退
{
uint8 array_dc[8]={0};
array_dc[0]=0xFE;
array_dc[1]=1*move_time;
array_dc[2]=0;
array_dc[3]=1*move_time;
array_dc[4]=0xFE;
array_dc[5]=1*move_time;
array_dc[6]=0;
array_dc[7]=1*move_time;
dc_moto_control(array_dc);
delay(5*move_time);
}
機(jī)器人左右轉(zhuǎn)轉(zhuǎn)：
void TurnRight(uint8 move_time) //機(jī)器人右轉(zhuǎn)
{
uint8 array_dc[8]={0};
array_dc[0]=0;
array_dc[1]=1*move_time;
array_dc[2]=0;
array_dc[3]=1*move_time;
array_dc[4]=0;
array_dc[5]=1*move_time;
array_dc[6]=0;
array_dc[7]=1*move_time;
dc_moto_control(array_dc);
delay(5*move_time);
}
void TurnLeft(uint8 move_time) //機(jī)器人3轉(zhuǎn)
{
uint8 array_dc[8]={0};
array_dc[0]=0xFE;
array_dc[1]=1*move_time;
array_dc[2]=0xFE;
array_dc[3]=1*move_time;
array_dc[4]=0xFE;
array_dc[5]=1*move_time;
array_dc[6]=0xFE;
array_dc[7]=1*move_time;
dc_moto_control(array_dc);
delay(5*move_time);
}
|
|
