樂高機器人

1. 樂高機器人

2. 進度 • 遙控車製作

3. 遙控器 • #define BT_CONN 1 • #define OUTBOX 5 • #define OUTBOXX 4 • sub BTCheck(intconn) • { • if(!BluetoothStatus(conn)==NO_ERR) • { • TextOut(5,LCD_LINE2,"Error"); • Wait(1000); • Stop(true); • } • }

4. task main() • { • BTCheck(BT_CONN); • intx,y; • while(true) • { • ClearScreen(); • x=MotorTachoCount(OUT_B); • y=MotorTachoCount(OUT_C)/4;

5. if(x>100) • x=100; • else if(x<-100) • x=-100; • if(y>100) • y=100; • else if(y<-100) • y=-100;

6. NumOut(10,LCD_LINE2,x); • NumOut(10,LCD_LINE4,y); • SendRemoteNumber(BT_CONN,OUTBOX,x); • SendRemoteNumber(BT_CONN,OUTBOXX,y); • } • }

7. 遙控車 • #define INBOX 5 • #define INBOXX 4 • sub BTCheck(intconn) • { • if(!BluetoothStatus(conn)==NO_ERR) • { • TextOut(5,LCD_LINE2,"Error"); • Wait(1000); • Stop(true); • } • }

8. task main() • { • BTCheck(0); • intx,y,i,j,k,l; • intsmp=1; • while(true) • { • ReceiveRemoteNumber(INBOX,false,x); • ReceiveRemoteNumber(INBOXX,false,y);

9. 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); • }

10. 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); • }

11. 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); • }

12. 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); • } • }

13. End