Download
1 / 13
130 likes | 273 Views
樂高機器人. 進度. 遙控車製作. 遙控器. #define BT_CONN 1 #define OUTBOX 5 #define OUTBOXX 4 sub BTCheck(int conn) // 檢查藍芽是否連線 { if(!BluetoothStatus(conn)==NO_ERR) { TextOut(5,LCD_LINE2,"Error"); Wait(1000); Stop(true); } }. task main() {
E N D
進度 • 遙控車製作
遙控器 • #define BT_CONN 1 • #define OUTBOX 5 • #define OUTBOXX 4 • sub BTCheck(int conn) //檢查藍芽是否連線 • { • if(!BluetoothStatus(conn)==NO_ERR) • { • TextOut(5,LCD_LINE2,"Error"); • Wait(1000); • Stop(true); • } • }
task main() • { • BTCheck(BT_CONN); //檢查藍芽連線 • int x,y; • while(true) • { • ClearScreen(); • x=MotorTachoCount(OUT_B); • y=MotorTachoCount(OUT_C)/4;
if(x>100) //馬達電力值在100~-100，當角度超過100或-100就限制住 • x=100; • else if(x<-100) • x=-100; • if(y>100) • y=100; • else if(y<-100) • y=-100;
NumOut(10,LCD_LINE2,x); • NumOut(10,LCD_LINE4,y); • SendRemoteNumber(BT_CONN,OUTBOX,x); • //傳送X值 • SendRemoteNumber(BT_CONN,OUTBOXX,y); • //傳送Y值 • } • }
遙控車 • #define INBOX 5 • #define INBOXX 4 • sub BTCheck(int conn) • { • if(!BluetoothStatus(conn)==NO_ERR) • { • TextOut(5,LCD_LINE2,"Error"); • Wait(1000); • Stop(true); • } • }
task main() • { • BTCheck(0); 以接收端來看，傳送端都是在line 0 • int x,y,i,j,k,l; • int smp=1; • while(true) • { • ReceiveRemoteNumber(INBOX,false,x); //接收X值 • ReceiveRemoteNumber(INBOXX,false,y);//接收Y值
if(y>0 && x>=0) //前進左轉 • { • if(x==0) • i=-y; • else • { • i=x-y; • if(i<0) • i=0; • } • OnFwdReg(OUT_B,x,OUT_REGMODE_SPEED); • OnFwdReg(OUT_C,i,OUT_REGMODE_SPEED); • }
else if(y<0 && x>=0) //前進右轉 • { • if(x==0) • j=y; • else • { • j=x+y; • if(j<0) • j=0; • } • OnFwdReg(OUT_B,j,OUT_REGMODE_SPEED); • OnFwdReg(OUT_C,x,OUT_REGMODE_SPEED); • }
else if(y>0 && x<0) //後退左轉 • { • k=x+y; • if(k>0) • k=0; • OnFwdReg(OUT_B,x,OUT_REGMODE_SPEED); • OnFwdReg(OUT_C,k,OUT_REGMODE_SPEED); • }
else if(y<0 && x<0) //後退右轉 • { • l=x-y; • if(l>0) • l=0; • OnFwdReg(OUT_B,l,OUT_REGMODE_SPEED); • OnFwdReg(OUT_C,x,OUT_REGMODE_SPEED); • } • else //直線前進後退 • OnFwdReg(OUT_BC,x,OUT_REGMODE_SPEED); • } • }
