Robofun 機器人論壇

標題: 循跡自走車 程式幫幫忙 [打印本頁]
作者: linpetin    時間: 2010-6-21 00:12
標題: 循跡自走車 程式幫幫忙
循跡自走車 照著電工膠帶跑

我適用 5顆CNY70  馬達驅動IC TA7257  2個

看圖~

我是用C語言寫的

一直有問題

如果用CASE寫的話 是不是會比較好

例如 CNY70 那邊掃到 CASE 0X08的時候  P1=0X05  馬達直走

這樣寫可以嗎

有沒有高手可以指導一下

謝謝

自走車電路.JPG (2.28 MB, 下載次數: 331)

電路圖

電路圖
作者: mac1    時間: 2010-6-21 09:22
case 就是 狀態機的用法,在作業系統的書籍裡面都有
如果要即時性的話, 用中斷會比較好
中斷要注意別卡在中斷裡 出不來,互等資源是最常發生的問題喔~
作者: vegewell    時間: 2010-6-21 19:00
//9212011 感應器的值要反轉..終點的表示法要變動..
//9212031 將mouseState 改為mouseState..加入ab_direction..開始做search..
//9212051 將陣列值反映到led 上..微調for 電池..top[0]當陣列第一個
// 當最兩邊感應到時..往前走..直到空白或終點為止
// 轉彎修正18 0c 會重複到..也要當往前走
#include <AT89X51.H>
//----------全域變數-----------
unsigned char time0_temp=0;
unsigned char time1_temp=0;
unsigned char mainState=0; //功能鍵
unsigned char mouseState=0; //電腦鼠的狀態
unsigned char ab_direction=1; //絕對方向
bit correct=0; //禁止中斷影響
//bit norepeat=0; //用在左T..讓他不要重複存值
bit GOTOEND=0; //走到終點
unsigned char r=20; //轉90 度所需要的時間25->22->20->15
unsigned char step=0; //走幾格
unsigned char littleStep=1; //走一小步
unsigned char countStep=0; //4in1 記步器
unsigned char maxStack=30;
unsigned char stack[30]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//stack[0]不用
unsigned char top=0; //top=0 當作pop 底
unsigned char top2=0;
//-----------函數宣告----------
void time_init(void);
void delayX1ms(unsigned char);
void delayX5ms(unsigned char);
- 72 -
void delayX10ms(unsigned char);
void delay2X10ms(unsigned char);
void beep(void);
void push(unsigned char);
//char pop(void);
void adjust_direction(unsigned char t);
void modulate(void);
void algorithm(void);
char sensor(void);
void algorithm_action(unsigned char t);
void action(unsigned char t);
void correct_straight(void);
void correct_turn(void);
void straight(void);
void turnleft(void);
void turnright(void);
void goback(void);
//-----------------------------
void main(){
//-----------宣告區-------------
unsigned char temp=0;
unsigned char temp2=0;
unsigned char i=0;
bit temp_bit=0;
//-----------初始化區-----------
time_init();
//-------------------------------
while(mainState==0){
P3=0xff;
temp=P3;
temp=~temp;
- 73 -
if (temp==0x01){ //test1 感應器輸出到led
mainState=1;
}
else if (temp==0x02){ //test2 動作測試
mainState=2;
}
else if(temp==0x04){ //test3 地形偵測
mainState=3;
}
else if(temp==0x08){ //test4 地形判斷
mainState=4;
}
else if(temp==0x10){ //test5 連續地形判斷
mainState=5;
}
else if(temp==0x20){
mainState=6; //test6 邊走邊修正+連續地形判斷
}
else if(temp==0x40){
mainState=7; //test7 搜尋
}
else if(temp==0x80){
mainState=8; //test8
}
else{}
}
//------------test1-----------------
while(mainState==1){
P2=P0;
}
//------------test2-----------------
while(mainState==2){
P3=0xff;
temp=P3;
temp=~temp;
if (mouseState!=0){ //有動作正在進行中
continue;
- 74 -
}
switch(temp){
case 0x01: //直走一格
mouseState=1;
straight();
break;
case 0x02: //左轉
mouseState=2;
turnleft();
break;
case 0x04: //右轉
mouseState=3;
turnright();
break;
case 0x08: //迴轉
mouseState=4;
goback();
break;
default:;
}
}
//------------test3-----------------
while(mainState==3){
}
//------------test4-----------------
while(mainState==4){
}
//------------test5-----------------
while(mainState==5){
}
//------------test6-----------------
while(mainState==6){
}
//-------------test7----------------
while(mainState==7){
P3=0xff;
temp=P3;
- 75 -
temp=~temp;
if (temp==0x01){
while (GOTOEND==0){
if(mouseState==1){
temp2=sensor();
if (temp2!=0){
action(temp2);
}else {}
correct_straight();
}else if(mouseState==0){
step=10;
mouseState=1;
straight();
}else {}
delayX10ms(1);
P2=~((top&0x0f)|(stack[top-1]<<4));
}
i=0;
do{
P2=~0x00;
delayX10ms(10);
P2=~((i&0x0f)|(stack[i]<<4));
if (i>=top){
i=0;
}else {
i++;
}
delayX10ms(250);
P3=0xff;
temp=P3;
temp=~temp;
}while(temp!=0x02);
beep();
beep();
beep();
- 76 -
algorithm();
}
else {}
}
//-------------test8-------------------
while(mainState==8){
}
//----------------------------------------
}
void algorithm_action(unsigned char t){
// P2=~0x00;
// delayX10ms(10);
while(stack[top2]==6){
top2++;
}
P2=~stack[top2];
if(t==3||t==4||t==5){
switch(stack[top2]){
case 1:
if (ab_direction==4){//right
mouseState=3;
turnright();
correct_turn();
ab_direction++;
}
else if(ab_direction==2){//left
mouseState=2;
turnleft();
correct_turn();
ab_direction--;
}
else {
}
break;
case 2:
if (ab_direction==1){//right
- 77 -
mouseState=3;
turnright();
correct_turn();
ab_direction++;
}
else if(ab_direction==3){//left
mouseState=2;
turnleft();
correct_turn();
ab_direction--;
}
else {
}
break;
case 3:
if (ab_direction==2){//right
mouseState=3;
turnright();
correct_turn();
ab_direction++;
}
else if(ab_direction==4){//left
mouseState=2;
turnleft();
correct_turn();
ab_direction--;
}
else {
}
break;
case 4:
if (ab_direction==1){//left
mouseState=2;
turnleft();
correct_turn();
ab_direction--;
}
- 78 -
else if(ab_direction==3){//right
mouseState=3;
turnright();
correct_turn();
ab_direction++;
}
else {
}
break;
default:
;
}
top2++;
if (ab_direction==0){
ab_direction=4;
}
else if(ab_direction==5){
ab_direction=1;
}
else{
}
}
else if(t==6){
GOTOEND=1;
mouseState=0;
P1_0=1;
P1_1=1;
}
else{
}
}
void action(unsigned char t){
unsigned char temp;
switch (t){
case 1://直線
break;
case 2://牆
- 79 -
mouseState=4;
goback();
correct_turn();
adjust_direction(2);
push(ab_direction);
break;
case 3://T
mouseState=3;
turnright();
correct_turn();
adjust_direction(3);
push(ab_direction);
break;
case 4://左T
adjust_direction(4);
push(ab_direction);
while ((temp = sensor())!=1){
if(mouseState==0){
step=10;
mouseState=1;
straight();
}
}
break;
case 5://右T
mouseState=3;
turnright();
correct_turn();
adjust_direction(5);
push(ab_direction);
break;
case 6://終點
break;
case 7://最兩邊感應到
correct=1;
P1_0=0;
P1_1=0;
- 80 -
delayX10ms(2);
P1_0=1;
P1_1=1;
correct=0;
if ((temp = sensor())==4){
goto action_7;
}
temp = sensor();
while(!(temp==3||temp==6)){//直到T 或終點..才會繼續感應下一個值
if(mouseState==0){
step=10;
mouseState=1;
straight();
}
temp = sensor();
}
if (temp==6){
GOTOEND=1;
mouseState=0;
P1_0=1;
P1_1=1;
adjust_direction(6);
push(ab_direction);
}
action_7:
break;
default:
beep();
}
}
void adjust_direction(unsigned char t){
switch (t){
case 1:
break;
case 2://牆
if (ab_direction==1){
- 81 -
ab_direction=3;
}
else if (ab_direction==2){
ab_direction=4;
}
else if (ab_direction==3){
ab_direction=1;
}
else if (ab_direction==4){
ab_direction=2;
}
else {
beep();
}
break;
case 3://T
ab_direction++;
if (ab_direction==0){
ab_direction=4;
}
else if(ab_direction==5){
ab_direction=1;
}
else {}
break;
case 4://左T
break;
case 5://右T
ab_direction++;
if (ab_direction==0){
ab_direction=4;
}
else if(ab_direction==5){
ab_direction=1;
}
else {}
break;
作者: vegewell    時間: 2010-6-21 19:02
本帖最後由 vegewell 於 2010-6-21 19:04 編輯

- 82 -
case 6://終點
ab_direction=5;
break;
default:
;
}
}
void modulate(){
bit modulation=0;
unsigned char i=0,j=0;
modulateStart:
while (stack!=5&&i<top){
while (stack==6&&i<top){
i++;
}
j=i+1;
while (stack[j]==6&&j<top){
j++;
}
if (stack==1&&stack[j]==3){
stack=6;
stack[j]=6;
modulation=1;
i++;
}
else if (stack==3&&stack[j]==1){
stack=6;
stack[j]=6;
modulation=1;
i++;
}
else if (stack==2&&stack[j]==4){
stack=6;
stack[j]=6;
- 83 -
modulation=1;
i++;
}
else if (stack==4&&stack[j]==2){
stack=6;
stack[j]=6;
modulation=1;
i++;
}
else {
i++;
while (stack==6&&i<top){
i++;
}
}
}
if (modulation==1){
i=0;
j=0;
modulation=0;
goto modulateStart;
}
else {
// goto mainState3;
}
}
void algorithm(){
unsigned char temp=0;
unsigned char i=0;
//----------------------------------
modulate();//先處理陣列的值得到最快路徑
i=0;
do{
P2=~0x00;
delayX10ms(10);
P2=~((i&0x0f)|(stack<<4));
- 84 -
if (i>=top){
i=0;
}else {
i++;
}
delayX10ms(250);
P3=0xff;
temp=P3;
temp=~temp;
}while(temp!=0x01);
//-------search initial--------------
mouseState=0;
GOTOEND=0;
correct=0;
ab_direction=1;
top2=0;//指到目前的陣列
//-----------------------------------
while (GOTOEND==0){
if(mouseState==1){
temp=sensor();
if (temp!=0){
algorithm_action(temp);
}else {}
correct_straight();
}
else if(mouseState==0){
step=10;
mouseState=1;
straight();
}else {}
delayX10ms(1);
}
}
char sensor(){
- 85 -
unsigned char temp;
P0=0xff;
temp=P0&0x7F; //把P0.7 去掉
if (temp==0x22|| //直線
temp==0x66||temp==0x44||temp==0x4C||
temp==0x33||temp==0x15||temp==0x1F){
return 1;
}
else if(temp==0x2A||
temp==0x7E||temp==0x54||temp==0x7C||
temp==0x3F||temp==0x15||temp==0x1F){ //牆
return 2;
}
else if(temp==0x00){ //T or 十字
return 3;
}
else if(temp==0x02||
temp==0x04||temp==0x60||temp==0x07||
temp==0x03||temp==0x01){ //右T->左T
return 4;
}
else if(temp==0x20||
temp==0x10||temp==0x18||temp==0x30|| //去掉
temp==0x30 0x20
temp==0x60||temp==0x40){ //左T->右T
return 5;
}
else if(temp==0x1C||
temp==0x1E||temp==0x0E||//temp==0x0F||temp==07||
temp==0x3C||temp==0x38){//temp==0x78||temp==70){ //終點
return 6;
}
else if(temp==0x63||
temp==0x67||temp==0x47||temp==0x4F||
temp==0x73||temp==0x71||temp==0x79){ //最兩邊感應到
return 7;
- 86 -
}
else{ //nothing
return 0;
}
}
void correct_straight(void){
unsigned char right=5;
unsigned char left=5;
unsigned char temp;
P0=0xff;
temp=P0&0x7F; //把P0.7 去掉
if (temp==0x4C){
correct=1; //禁止計時中斷影響
delayX10ms(10);
P1_0=1;
P1_1=0; //左馬達停一下讓右馬
達轉回
delayX10ms(left);
P1_1=1;
delayX10ms(10);
correct=0;
}
else if(temp==0x44){
correct=1; //禁止計時中斷影響
delayX10ms(10);
P1_0=1;
P1_1=0; //左馬達停一下讓右馬
達轉回
delayX10ms(left-2);
P1_1=1;
delayX10ms(10);
correct=0;
}
else if(temp==0x66){
correct=1; //禁止計時中斷影響
delayX10ms(10);
- 87 -
P1_0=1;
P1_1=0; //左馬達停一下讓右馬
達轉回
delayX10ms(left-3);
P1_1=1;
delayX10ms(10);
correct=0;
}
else if(temp==0x19){
correct=1;
delayX10ms(10);
P1_1=1;
P1_0=0;
delayX10ms(right);
P1_0=1;
delayX10ms(10);
correct=0;
}
else if(temp==0x11){
correct=1;
delayX10ms(10);
P1_1=1;
P1_0=0;
delayX10ms(right-2);
P1_0=1;
delayX10ms(10);
correct=0;
}
else if(temp==0x33){
correct=1;
delayX10ms(10);
P1_1=1;
P1_0=0;
delayX10ms(right-3);
P1_0=1;
delayX10ms(10);
correct=0;
- 88 -
}
else {
//return 0;
}
}
void correct_turn(void){
unsigned char right=10;
unsigned char left=10;
unsigned char temp;
unsigned char temp2=0;
bit turn=0; //0->先右轉..1->左轉
correct=1;
correct_start:
P0=0xff;
temp=P0&0x7F;//把P0.7 去掉
switch (temp){
case 0x08: //0001000 往前走一點點再測試一次
case 0x18:
case 0x0C:
delayX10ms(100);
P1_0=0;
P1_1=0;
delayX10ms(1);
P1_0=1;
P1_1=1;
delayX10ms(100);
goto correct_start;
break;
case 0x00:
while(temp==0x00){
if (turn==0){
delayX10ms(100);
P1_1=1;
P1_0=0;
delayX10ms(left);
- 89 -
P1_0=1;
delayX10ms(100);
if (temp2++>4){
turn=1;
temp2=0;
}
}
else {
delayX10ms(100);
P1_0=1;
P1_1=0;
delayX10ms(left);
P1_1=1;
delayX10ms(100);
if (temp2++>4){
beep();
}
}
delayX10ms(1);
P0=0xff;
temp=P0&0x7F;
}
// beep();
goto correct_start;
break;
case 0x10: //偏右part1
case 0x30:
delayX10ms(100);
P1_0=1;
P1_1=0; //左馬達停一下讓右馬達轉回
delayX10ms(left);
P1_1=1;
delayX10ms(100);
break;
case 0x20: //偏右part2
case 0x60:
- 90 -
case 0x40:
delayX10ms(100);
P1_0=1;
P1_1=0; //左馬達停一下讓右馬達轉回
delayX10ms(left+5);
P1_1=1;
delayX10ms(100);
break;
case 0x04: //偏左part1
case 0x06:
delayX10ms(100);
P1_1=1;
P1_0=0;
delayX10ms(right);
P1_0=1;
delayX10ms(100);
break;
case 0x02: //偏右part2
case 0x03:
case 0x01:
delayX10ms(100);
P1_1=1;
P1_0=0;
delayX10ms(right+5);
P1_0=1;
delayX10ms(100);
break;
default:
;
}
correct=0;
}
void beep(){
P1_7=0;
time0_temp=19;
while(time0_temp!=0){}
- 91 -
P1_7=1;
time0_temp=19;
while(time0_temp!=0){}
}
void straight(){
P1_0=1;
P1_1=1;
time1_temp=littleStep;
countStep=0;
}
void turnleft(){
P1_0=1;
P1_1=1;
delayX10ms(100); //靜止一下在動
time1_temp=r;
P1_1=0; //右馬達轉
while(time1_temp!=0){}
P1_1=1;
P1_0=1;
delayX10ms(100); //靜止一下在動
mouseState=0; //動作完畢
}
void turnright(){
P1_0=1;
P1_1=1;
delayX10ms(100); //靜止一下在動
time1_temp=r;
P1_0=0; //右馬達轉
while(time1_temp!=0){}
P1_0=1;
- 92 -
P1_1=1;
delayX10ms(100); //靜止一下在動
mouseState=0; //動作完畢
}
void goback(){
unsigned char temp1=r*3+10; //270 度
// unsigned char temp2=r*2; //180 度
//--------------------------------------------
P1_0=1;
P1_1=1;
delayX10ms(100); //靜止一下在動
time1_temp=temp1; //左270 度
P1_0=0;
while(time1_temp!=0){}
//-------------------------------------------
P1_0=1;
P1_1=1;
delayX10ms(100); //靜止一下在動
time1_temp=r; //左邊90 度
P1_1=0;
while(time1_temp!=0){}
//--------------------------------------------
P1_1=1;
P1_0=1;
delayX10ms(100); //動作完畢
mouseState=0;
}
void push(unsigned char temp){
stack[top]=temp;
if (top>=maxStack){
beep();
}
else {
- 93 -
top++;
}
}
void time_init(void){
time0_temp=0;
time1_temp=0;
PT0=1; //TIME0 高優先
PT1=0; //TIME1 低優先
EA=1; //所有中斷自己搞
TMOD=0x12; //TIME0->MODE2,TIME1->MOD1
TH0=0x9C; //0.1MS/1US=100->256-100=156=9C
TL0=0x9C;
ET0=1; //TIME0 中斷啟動
TR0=1; //TIME0 啟動
TH1=0x3C; //50MS/1US=50000->65536-50000=15536=3CB0H
TL1=0xB0;
ET1=1; //TIME1 中斷啟動
TR1=1; //TIME1 啟動
}
void delayX5ms(unsigned char temp){
int i,j;
for (i=0;i<temp;i++){
for(j=0;j<600;j++){
;
}
}
}
void delayX1ms(unsigned char temp){
int i,j;
for (i=0;i<temp;i++){
for(j=0;j<120;j++){
;
- 94 -
}
}
}
void delayX10ms(unsigned char temp){
int i,j;
for (i=0;i<temp;i++){
for(j=0;j<1200;j++){
;
}
}
}
void delay2X10ms(unsigned char temp){
int i,j;
for (i=0;i<temp;i++){
for(j=0;j<1200;j++){
;
}
}
}
void time0_vector(void) interrupt 1{ //TIME0 0.1Ms 中斷一次
if (time0_temp>0){
time0_temp--;
}
}
void time1_vector(void) interrupt 3{ //TIME1 50Ms 中斷一次
if(time1_temp>0){
time1_temp--;
}
else if (time1_temp==0){
if (mouseState==1&&correct==0){
if(countStep<step){
countStep++;
time1_temp=littleStep;
P1_0=0;
- 95 -
P1_1=0;
delay2X10ms(1);
P1_0=1;
P1_1=1;
delay2X10ms(10);
}
else if(countStep>=step){
mouseState=0;
P1_0=1;
P1_1=1;
}
else {}
}
else {}
}
else {}
TMOD=0x12; //TIME0->MODE2,TIME1->MOD1
TH1=0x3C; //50MS/1US=50000->65536-50000=15536=3CB0H
TL0=0x9C;
}
========================
以上適用 三顆CNY70   ic -->89C51
作者: linpetin    時間: 2010-6-24 18:41
謝謝vegewell  我再來瞭解看看..



歡迎光臨 Robofun 機器人論壇 (https://www.robofun.net/forum/) Powered by Discuz! X3.2