Giao Trinh Robocon Tom Tat Buoi 2

Giáo trình Robocon tóm tắt Trương Trọng Toại

www.nhom3c.com

Buổi 2 . TÌM HIỂU VỀ CÁC CẢM BIẾN TRÊN ROBOT Như buổi 1 đã đề cập ,cảm biến trên robot được bố trí thành 3 tầng (Tầng dưới ,tầng giữa ,và tầng trên ).Buổi hôm nay ta sẽ đi sâu vào các loại cảm biến này . 1.Cảm biến tầng trên :

Trong Robocon tầng trên thường được trang bị Camera . Ta sẽ tìm hiểu kĩ về CMU Cam . Ưu điểm của Camera này là được tích hợp chip xử lý ảnh sẵn nên việc ứng dụng nó rất dễ dàng . Chỉ cần dùng những loại vi điều khiển thông thường vẫn có thể giao tiếp tốt với CMU Cam .


www.nhom3c.com

(Hướng dẫn sử dụng và các phần mềm hỗ trợ sẽ được chỉ rõ trong CMUCam Manual ) Chương trình tham thảo: Dùng PIC16F877A giao tiếp với CMU Cam . // Spot Welding Mobile Robot V2 (Only Spot, No Welding Line) // 2 PIC877: (1)for Welding Process; (2) for Tracking Line using Camera sensor // PIC Comm.: Parallel (PSP) // Camera Comm.: RS232 // Optimal Code: OK //****************************** /* Parameters: (1) Rmin;Rmax;Gmin;Gmax;Bmin;Bmax (2) Sensitive */ //****************************** #include <16F877.h> #include <math.h> #include <string.h> #include <stdlib.h> #fuses hs, nowdt, noprotect, put, nolvp, brownout #use delay(clock=20000000) #use rs232(baud=115200,parity=N,xmit=PIN_C6,rcv=PIN_C7) #byte pie2 = 0x8c #byte rcsta = 0x18


www.nhom3c.com

int16 Count1,Count2; int16 WeldingSpeed=250; char Packet=' '; int16 mx=43; int16 my=0; int8 x1=0; int8 y1=0; int8 x2=0; int8 y2=0; int16 pixels=0; int16 confidence=0; int8 Rmin=160; int8 Rmax=255; int8 Gmin=170; int8 Gmax=255; int8 Bmin=100; int8 Bmax=255; int8 Sensitive=2; int max=7; int pos=0; char c; char s2[30]; int1 index=0; int16 duty=300; int16 duty1=300; //**************************************** void get_string2(char * s,int max) { int len; char c; max--; len=0; do { c=getc(); if ((c>=' ')&&(c<='~')&&(c!=13)) if(len<max) { s[len++]=c; //putc(c); } } while(c!=13); s[len]=0; return;


www.nhom3c.com

} //**************************************** void get_M_Packet() { char c; char Num[9]; int len=0; max=8; pos=0; do { c=getc(); if(c==' ') { // space Num[len]=0; switch (pos) { case 0: Packet=(char)Num[0]; Pos++; break; case 1: //mx=atoi(Num); Pos++; break; case 2: my=atoi(Num); Pos++; break; case 3: // x1=atoi(Num); Pos++; break; case 4: // y1=atoi(Num); Pos++; break; case 5: // x2=atoi(Num); Pos++; break; case 6: // y2=atoi(Num); Pos++; break; case 7: // pixels=atol(Num); //Pos++;


www.nhom3c.com

break; case 8: //confidence=atol(Num); Pos++; break; default: break; } len=0; } else //if ((c>=' ')&&(c<='~')&&(c!=13)) if (c!=13) if(len<max) Num[len++]=c; } while(c!=13); // confidence=atol(Num); return; } //**************************************** #INT_TIMER1 Sampling_Time() { //printf("TC %u %u %u %u %u %u\r", Rmin,Rmax,Gmin,Gmax,Bmin,Bmax); printf("TC 160 255 170 255 100 255\r"); get_string2(s2, 10); if(s2[0]=='N') { //getc();//Read : output_d(222); bit_clear(rcsta,1);//clear error bit_clear(rcsta,4);//clear bit_set(rcsta,4);//set return; } get_M_Packet(); duty=duty1=WeldingSpeed; if(my!=0) { duty=duty-(int16)((my-70)*Sensitive); duty1=duty1+(int16)((my-70)*Sensitive); } set_timer1(3036);//100ms Count2++; return; } //****************************************


www.nhom3c.com

#INT_EXT void EXT_isr() { if(input(PIN_C4)==0) { output_low(PIN_E0);//Brake output_low(PIN_C5);//Brake } else { output_high(PIN_E0);//Run output_high(PIN_C5);//Run } return; } //**************************************** main() { strcpy(s2,"ABCDE"); delay_ms(1000); bit_clear(rcsta,1);//clear error bit_clear(rcsta,4);//clear bit_set(rcsta,4);//set delay_ms(300); printf("PM 1\r");//One Packet at a time get_string2(s2, 10); getc();//Loai bo dau : output_d(1); delay_ms(300); printf("SM 0\r"); get_string2(s2, 10); getc();//Loai bo dau : output_d(2); delay_ms(300); //printf("SW 1 65 80 75\r");//1 65 80 75 printf("SW 34 1 46 140\r"); get_string2(s2, 10); getc();//Loai bo dau : output_d(3); delay_ms(300); printf("CR 18 44 \r");//Auto White Balance ON


www.nhom3c.com

get_string2(s2, 10); getc();//Loai bo dau : output_d(4); /* write_eeprom(0,160);//Rmin write_eeprom(1,255);//Rmax write_eeprom(2,170);//Gmin write_eeprom(3,255);//Gmax write_eeprom(4,100);//Bmin write_eeprom(5,255);*///Bmax /* Rmin=read_eeprom(0); Rmax=read_eeprom(1); Gmin=read_eeprom(2); Gmax=read_eeprom(3); Bmin=read_eeprom(4); Bmax=read_eeprom(5);*/ delay_ms(500);// Wait for slave available then read information for first time SETUP_CCP1(CCP_PWM); SETUP_CCP2(CCP_PWM);//Page 206 I/O PWM setup_timer_2(T2_DIV_BY_1,255,1); setup_timer_1(T1_INTERNAL | T1_DIV_BY_8); setup_adc_ports(NO_ANALOGS);// Phai co lenh nay A Port moi co tac dung on dinh enable_interrupts(INT_TIMER1); //enable_interrupts(INT_EXT); enable_interrupts(GLOBAL); output_low(PIN_C0);//Dir output_low(PIN_A5);//Dir /*output_low(PIN_E0);//Brake output_low(PIN_C5);*///Brake output_high(PIN_E0);//Brake output_high(PIN_C5);//Brake while(1) { if(input(PIN_C3)==1) WeldingSpeed=420; else WeldingSpeed=320;


www.nhom3c.com

if(Count2>1) { if(my<=140) output_d((int8)my); //if(duty>1000) duty=1000; //if(duty1>1000) duty1=1000; //if(my<=80 && my>=70) output_high(pin_a0); //else output_low(pin_a0); set_PWM1_duty(duty);//left wheel Note: Khong dung lenh nay nhieu lan, chi dung thong qua duty set_PWM2_duty(duty1); Count2=0; } } }

2.Cảm biến tầng giữa: Trong Robocon cảm biến tầng giữa được trang bị thường là Cảm biến siêu âm ,thường dùng là SFR02,SFR04,SFR08,… .Mình sẽ tập trung nguyên cứu SFR04 ,đây là loại được sử dụng phổ biến .


www.nhom3c.com

Giới thiệu : Cảm biến siêu âm SRF04 có khả năng đo được khoảng cách từ 3 cm đến 3m . Nó được gắn trên robot kèm theo một động cơ RC Servo để dò vật cản . Giao tiếp với SRF04: Sơ đồ chân của 5 đầu ra của SFR04 : Hình ảnh một robot sử dụng cảm biến siêu âm SRF04 :


www.nhom3c.com

Giản đồ xung của SRF04 :

Nguyên tắc hoạt động :

Vùng hoạt động của cảm biến siêu âm được xác định bằng cách truyền một xung âm thanh ra bên ngoài .Xung này sẽ di chuyển với tốc độ âm thanh (xấp xỉ 0,9 ft/ msec) trong vùng hình nón .


www.nhom3c.com

The

Và âm thanh phản hồi lại từ vùng này khi có bất kỳ vật cản nào trên đường đi của sóng siêu âm .

Để xác định được vùng làm việc bạn cần 2 chân : Chân input trigger và chân tạo xung output .

Cách sử dụng với SRF04 :

Để bắt đầu có thể làm việc với SRF04 bạn gởi tới chân input trigger của SRF04 một xung cạnh lên một khoảng thời gian nào đó (ít nhất khoảng 10us) . Sẽ có xung falling edge(mỗi xung có độ rộng một chu kỳ ) về trên chân input trigger ,sẽ có 8 xung này .Mạch nhận tín hiệu sẽ không nhận trong một khoảng thời gian ngắn khoảng 100us để tránh tiếng ồn từ âm thanh ban đầu này . Chân output vẫn giữ mức thấp cho đến khi mạch nhận cho phép , và sau đó xung ra trên chân output và để biết được khoảng cách tới vật thể ta đo chiều rộng của xung này . Tùy theo xung này ta sẽ biết được khoảng cách tới vật thể hoặc không có vật thể trong vùng làm việc .

Giá trị xung thô trả về được tính bằng micro giây .Từ đó ta tính được khoảng cách thông qua công thức sau : s [m] = 344m/s * ti [s] / 2. Với vùng phát hiện từ 3cm tới 3m ,ta có thể tính được độ dài của xung là từ 100us tới 18ms . Nếu không có vật thể nào phát hiện ,chiều rộng xung sẽ là 36 ms . Chương trình tham khảo : Sử dụng PIC16F84A ;----------------------------------------------------------------------

; cpu equates (memory map)

list p=16f84

radix hex

;----------------------------------------------------------------------


www.nhom3c.com

status equ 0x03 ; status equate

porta equ 0x05 ; port a equate

portb equ 0x06 ; port b equate

PWM equ 0x0c ; PWM signal length

count equ 0x0d ; general register

temp equ 0x0e ; general register

loop equ 0x0f ; general register

;---------------------------------------------------------------------

c equ 0 ; status bit to check after subtraction

;---------------------------------------------------------------------

; porta0 = input from sensor

; portb3 = command to sensor

; portb0 = command to servo

org 0x000

start movlw 0x00 ; load W with 0x00 make port B output

tris portb ; port B is outputs

movlw 0xFF ; load W with 0xFF make port A input

tris porta ; port A is inputs

movlw 0x00 ; load W with 0x00 to set intial value

of B

movwf portb ; set port b outputs to low

main clrf count ; clear count

clrf PWM ; clear PWM

bsf portb,3 ; tell sensor to make reading

movlw d'4' ; 4*3=12 clock delay

movwf count

call delay ; delay

bcf portb,3 ; end sensor command signal

movlw d'5' ; set the delay for measuring the output

movwf count

A0LOW btfss porta,0 ; if the output has gone high, skip the

next instruction

goto A0LOW ; else check again

A0HIGH incf PWM ; increment the length of the PWM signal

call delay ; delay for the count assigned above

nop

btfsc porta,0 ; check to see if the output is still

high

goto A0HIGH ; if it is, repeat

movf PWM,w ; move PWM to w

sublw d'200' ; subtract PWM cycle from 200 (2 msec)

btfsc status,c ; if PWM is greater than 200 (2 msec),

skip next instruction

goto skip1

movlw d'200' ; else set the max PWM length to 200

movwf PWM

skip1 movf PWM,w ; move PWM to w

sublw d'20' ; subtract PWM cycle from 200 (2 msec)

btfss status,c ; if PWM is greater than 200 (2 msec),

skip next instruction

goto skip2

movlw d'20' ; else set the min PWM length to 20


www.nhom3c.com

movwf PWM

skip2 movlw d'1' ; set the delay for generating the PWM

movwf count

bsf portb,0 ; start the PWM pulse

LoopPWM call delay

nop

nop

nop

decfsz PWM ; decrement the PWM length

goto LoopPWM ; as long as PWM is greater than 0, loop

bcf portb,0 ; when done looping, stop the pulse

movlw d'15' ; set the counter for generating the

rest of the PWM signal

movwf loop

del15 movlw d'255' ; set the delay counter

movwf count

call delay

decfsz loop

goto del15

goto main

;----------------------------------------------------------------------

delay movf count,w ; delay loop

movwf temp

del decfsz temp ; 3 clock cycles per delay loop

goto del

return

;----------------------------------------------------------------------

end

;----------------------------------------------------------------------

; at burn time, select:

; memory uprotected

; watchdog timer disabled

; standard crystal (4 MHz)

; power-up timer on

3.Cảm biến tầng dưới: Trong robocon thường sử dụng là cảm biến hồng ngoại . Ta sẽ tìm hiểu về Sharp GP2D12IR . Cách đây vài năm ,nhiều loại cảm biến hồng ngoại đo khoảng cách đã được Sharp giới thiệu .Các cảm biến này được đóng góp nhỏ gọn ,tiêu thụ dòng rất ít ,và nhiều sự lựa chọn cho tín hiệu output . Hình ảnh của GP2D12:


www.nhom3c.com

Sơ đồ khối :

Giản đồ trạng thái


www.nhom3c.com

Một số đặc tính của GP2D12 :

Absolute Max Ratings

Parameter Symbol Rating Unit Remarks

Supply Voltage VCC -0.3 to +7 V

Output Terminal Voltage VO -0.3 to VCC+0.3 V

Operating Tempurature Topr -10 to +60 °C

Storage Tempurature Tstg -40 to +70 °C

Operating Supply Voltage

Parameter Symbol Rating Unit Remark

Operating Supply Voltage VCC 4.5 to 5.5 V

Electro-Optical Characteristics

Parameter Symbol Conditions Min. Typ. Max. Unit

Measuring distance range

delta L *1 10 - 80 cm

Output Terminal Voltage

VO L = 80 cm *1 0.25 0.4 0.55 V

Output voltage difference

delta VO

Output change at L change (80 cm

-> 10 cm) *1 1.75 2.0 2.25 V

Average supply current

Icc L = 80 cm, *1 - 33 50 mA


www.nhom3c.com

L: Distance to reflected object *1 Using reflected object: White paper (Made by Kodak Co. Ltd. gray cards R-27, white face, reflective ratio: 90%) Lý thuyết hoạt động : Với sự giới thiệu về cảm biến GP2DXX của Sharp ,một phương pháp mới được sử dụng phát hiện vật thể ở khoảng cách xa hơn, và không bị ảnh hưởng nhiều của điều kiện ánh sáng xug quanh . Cảm biến hồng ngoại sử dụng phương pháp trắc đạt tam giác (triangulation) và một mảng CCD dài mỏng để tính toán khoảng cách và sự hiện diện của vật thể trong vùng quan sát . (Phương pháp trắc đạt tam giác sẽ được giới thiệu trong tài liệu kèm theo ) Ý tưởng cơ bản là : một xung của đèn IR được phát ra bởi đèn phát (emitter) . Tia sáng này sẽ đi trong trường quan sát và hoặc là đụng vật thể ,hoặc là không đụng vật thể .Trong trường hợp không gặp vật thể ,tia sáng sẽ không bao giờ phản hồi về và đọc là không có vật thể .Nếu tia sáng phản xạ lại ,có vật thể . Cảm biến sẽ thu và tạo ra một phép trắc đạc tam giác giữa 3 điểm : điểm phản xạ ,điểm phát và điểm thu .

Khác góc và khác khoảng cách Tín hiệu Output : Tín hiệu Output không tuyến tính .Bởi vì một vài phép lượng giác của tam giác trắc địa .


www.nhom3c.com

Đường cong quan hệ giữa khoảng cách và mức volt của GP2D12

Ở đây có hai điều lưu ý trong đường cong quan hệ trên : Đầu tiên ,tín hiệu output khoảng cách nằm giữa vùng từ 10 cm tới 80 cm thì không tuyến tính . Thứ hai là trên đường cong có thể chia thành hai vùng ,một vùng nhỏ hơn 10 cm và vùng từ 10 cm tới 80 cm .Hai vùng này đường đặc tuyến khác nhau . Nó có thể gây ảnh hưởng đến robot của bạn nếu robot của bạn di chuyển chậm hoặc khi robot của bạn tiến lại gần vật thể . Để tránh điều này cảm biến trên robot bạn có thể bố trí như hình dưới .

Ví dụ về vị trí đặt cảm biến trên robot

Góc của chùm tia hơi nhỏ ,vì khoảng cách giữa con phát và thu nhỏ chỉ có 16 cm .Nếu bạn muốn tăng góc mở , bạn có thể sử dụng kết hợp 2 cảm biến hồng ngoại như ví dụ ở hình dưới .


www.nhom3c.com

Ví dụ tăng góc quan sát của cảm biến

.

Việc sử dụng GP2D12 được liên kết nối tới 3 dây .Những kết nối này là : Ground ,Vcc ,và Output . Cảm biến sẽ đo khoảng cách và xuất ra output dưới dạng mức volt analog . Bởi vì việc thu ánh sáng xảy ra liên tục , không cần xung clock . Nên tốn nhiều năng lượng ,và có thể ảnh hưởng đến các ngoại vi khác ,và bộ xử lý nếu trên robot có nhiều cảm biến này . Tín hiệu Output có những xung gai nhỏ nên cần có thêm tụ lọc giữa Output và Ground .


www.nhom3c.com

Việc lập trình sử dụng cảm biến này thì rất đơn giản ,các bạn chỉ cần dùng vi điều khiển nào có tích hợp sẵn bộ ADC ,và đọc vào là có thể biết được khoảng cách . Cảm biến dò đường trong Robocon : Như buổi 1 đã phân tích ,có rất nhiều phương án bố trí cảm biến để có thể dò đường và đếm vạch được .Trong phần này ,chỉ ví dụ một phương án đó là sử dụng 8 cảm biến đặt thẳng hàng (Dùng led siêu sáng và quang trở ) ,vừa dò đường vừa đếm vạch . Sơ đồ mạch cảm biến :

R8

10K

13

2

1_+IN1

+5V

0

R32.2K

0

D32

INFARED LED_RECEIVER

12

1_+IN2

D32


12

1_-IN1

0

R647K

R71K

D31INFARED LED_TRANSMITTER

12


12

0

R32.2KR6

47K

R71K

+5V

1_-IN2

R8

10K

13

2

0

0

00

1_-IN4


12

R8

10K

13

2

+5V

1_+IN3

+5V

D32


12

R71K

R647K

R8

10K

13

2

R647K


12

1_+IN4

R32.2K

00

0

1_-IN3

R71K

R32.2K

0

D32


12

1_+IN1

1_OUT3

1_+IN3

1_-IN1

1_OUT4

1_-IN3

5V1_+IN4

1_OUT1

1_+IN2

0

U7LM339OUT2OUT1+V-IN1+IN1-IN2+IN2 -IN3

+IN3-IN4+IN4GNDOUT4OUT3

1234-5+6-7+ 8-

9+10-11+121314

1_OUT2

1_-IN21_-IN4


www.nhom3c.com

D33

LED

12

+5V

P0.21_OUT3

C8CAP NP

12

0

R9R

P0.0

C8CAP NP

12

P0.1

+5V

C8CAP NP

12

1_OUT1 1_OUT2

D33

LED

12

0

R9R

D33

LED

12

C8CAP NP

12

P0.3

R9R

1_OUT4

D33

LED

12

+5V

00

R9R

+5V

R8

10K

13

2

D32


12

2-IN1

2+IN3

R647K

R71K


12

0

D32


12

0

2+IN4

0

+5V

R32.2K

0

2-IN4


12

0

2-IN2

0

2-IN3

R8

10K

13

2

R71K

R8

10K

13

2

+5V

0

2+IN1


12

R32.2K

R71K

D32


12

0


12

R32.2K

R8

10K

13

2

+5V

+5V

D32


12

0

R647K

R647K

0

0

2+IN2

R32.2K

R647K

R71K

0


www.nhom3c.com

2-IN4

5V

2-IN3

2-IN1 2+IN4

U7LM339OUT2OUT1+V-IN1+IN1-IN2+IN2 -IN3

+IN3-IN4+IN4GNDOUT4OUT3

1234-5+6-7+ 8-

9+10-11+121314

2+IN3

2OUT42OUT2

2+IN2

2OUT3

0

2OUT1

2-IN22+IN1

0

2OUT2

C8CAP NP

12

P0.6

R9R

2OUT3

D33

LED

12

P0.5

+5V

D33

LED

12

C8CAP NP

12

D33

LED

12

R9R

+5V

P0.4

0

2OUT1

C8CAP NP

12

0

C8CAP NP

12

R9R

R9R

D33

LED

12

+5V

0

P0.7

+5V

2OUT4

P0.1P3.0

P1.5 P3.5

P0.1

P1.7

P2.4

J3

Port2

12345678

P0.3

P2.5

P2.0

P0.6

P3.4

P0.0

P0.4

P2.6

RN1

4k7

123456789

P0.4

P2.1

P1.4

P2.7

P3.2

C133p

0

P3.1

P1.4

PSEN

5V

P1.0

P3.1

P0.3

P2.0

P2.3

Y1

XM/SM

P1.2P1.3

5V

R18k2

0

R2100k

P3.7

P3.6

P0.6

J2

Port1

12345678

P1.3

P3.0

P1.7

C2 33p

0

P2.2

P1.1

P3.7

P3.6

P3.3

P3.5

P2.1

89V51RD2B

P0.0

P2.7

P1.1

P2.5

J4

Port3

12345678

5V

P0.5

P1.6

P1.0

SW1

SW_PB_SPST

P3.4

P0.5

P3.3

P1.2

P2.2

P2.6

J1

Port0

12345678 P0.7

P0.2

P0.7

U1

AT89C51

9

181920

293031

4012345678

2122232425262728

1011121314151617

3938373635343332

RST

XTAL2XTAL1GND

PSENALE/PROGEA/VPP

VCCP1.0P1.1P1.2P1.3P1.4P1.5P1.6P1.7

P2.0/A8P2.1/A9P2.2/A10P2.3/A11P2.4/A12P2.5/A13P2.6/A14P2.7/A15

P3.0/RXDP3.1/TXDP3.2/INT0P3.3/INT1P3.4/T0P3.5/T1P3.6/WRP3.7/RD

P0.0/AD0P0.1/AD1P0.2/AD2P0.3/AD3P0.4/AD4P0.5/AD5P0.6/AD6P0.7/AD7

P2.3P2.4

P0.2

P1.5

C310uF

P1.6

P3.2


www.nhom3c.com

Thực hành vẽ mạch cảm biến và làm mạch cảm biến : Sử dụng Orcad và vẽ 1 lớp tự làm mạch bằng tay để test .

Lập trình dò đường : Trong lập trình dò đường bạn có 2 cách để lập trình ,một là hỏi vòng liên tục ,hai là sử dụng ngắt .Viết theo ngắt thì hay hơn vì xử lý được các sự kiện ngay lập tức ,còn hỏi vòng phải mất thời gian quét nên có thể bỏ xử lý các tình huống không kịp thời . Chương trình tham khảo :(Sử dụng ngắt ) #include<REG52.H> #include<mod51rd2.h> extern bit nho_trai,nho_phai,co_cd1,co_cd2,co_cd3,enable_time,finish,finish1,finish2,enable_xoa,enable_dem,enable_vach,en_doduong; extern unsigned char tocdo,tocdolx,tocdok,tocdoka,m; extern unsigned int dem1;//,dem2,dem3; void timer0_ISR(void) interrupt 1 using 1 { unsigned char tocdol,tocdor; dem1++;


www.nhom3c.com

m++; if(m >= 250){m=1;} if(tocdolx>=m){klen=0;} else {klen=1;} if(tocdok>=m){kqua=0;} else{kqua=1;} ///////////////////////////////////////////////////////////////// if ((P0 & 0x3C) == 0x3C) {finish=1;} if ((P0 & 0x07) == 0x07) {finish1=1;} if ((P0 & 0xF0) == 0xF0) {finish1=1;} if ((P0 & 0xE0) == 0xE0) {finish1=1;} /////////////////////////////////////////////////////////////////////////////////////// if(en_doduong) { switch(P0) {// unsigned char tocdol,tocdor; //******************************************************************************** case 0xF3: // co 4 cam bien ben trai dk ve ben trai { nho_trai=1; nho_phai=0; finish = 1; tocdol = tocdo - tocdo/6; tocdor = tocdo + tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; // motor phai chay toi CCAP2H = tocdol; // motor trai chay toi CCAP1H = 0x00; CCAP3H = 0x00; } break; //*************************************************************************************


www.nhom3c.com

case 0xCF: // co 4 cam bien ben phai dk ve ben phai { finish = 1; nho_trai = 0; nho_phai = 1; tocdol = tocdo + tocdo/4; tocdor = tocdo - tocdo/6; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //******************************************************************************** case 0xFF: //ko co cam bien { finish=1; tocdol = tocdo/3; tocdor = tocdo/3; // chaytoi(tocdol,tocdol); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //******************************************************************************** case 0x00: //ko co cam bien { if (nho_trai) { // chaylui(15,0) ve ben phai. P1 = 0xFF; dir1 = 0; dir3 = 0; CCAP4H = 0x00;


www.nhom3c.com

CCAP2H = 0x00; CCAP1H = 0; CCAP3H = 25; //dong co phai lui } else if (nho_phai) { //chaylui(0,15); P1 = 0xFF; dir1 = 0; dir3 = 0; CCAP4H = 0x00; CCAP2H = 0x00; CCAP1H = 25; // dong co trai lui CCAP3H = 0; } else { tocdol = 0; tocdor = 0; // chaytoi(tocdol,tocdol); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } } break; //************************************************************************************* //************************************************************************************* case 0x01: // co cam bien senln2 dieu khien lech toc do toan bo ve trai { nho_trai=1;nho_phai=0; tocdol = 40; tocdor = 200; // quaytrai1(tocdol,tocdor); P1 = 0xFF;


www.nhom3c.com

dir1 = 0; dir2 = 0; CCAP4H = tocdol; CCAP2H = 0x00; CCAP1H = tocdor; CCAP3H = 0x00; } break; case 0x80: // co cam bien senrn2 dieu khien lech toc do toan bo ve phai { nho_trai=0;nho_phai=1; tocdol = 200; tocdor = 40; // quayphai1(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir3 = 0; CCAP4H = 0x00; CCAP2H = tocdor; CCAP1H = 0x00; CCAP3H = tocdol; } break; //***************************************************************************** //********************************************************************************* case 0xC0: // co 2 cam bien senln1 + senln2 dieu khien lech toc do vua ve trai { nho_trai=1;nho_phai=0; tocdol = 150; tocdor = 60; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x03: // co 2 cam bien senrn1 + senrn2 dieu khien lech toc do vua ve phai


www.nhom3c.com

{ nho_trai=0;nho_phai=1; tocdol = 60; tocdor = 150; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //***************************************************************************** case 0x02: // co cam bien senln1 dieu khien lech toc do nhieu ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo - tocdo/3 ; tocdor = tocdo + tocdo/5; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x40: // co cam bien senrn1 dieu khien lech toc do nhieu ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo + tocdo/5 ; tocdor = tocdo - tocdo/3; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00;


www.nhom3c.com

} break; //******************************************************************************* case 0x06: // co 2 cam bien senln + senln1 dieu khien lech toc do vua ve trai { nho_trai=1;nho_phai=0; tocdol = (2*tocdo)/3;//35;//30 tocdor = tocdo + tocdo/4;//30; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x60: // co 2 cam bien senrn + senrn1 dieu khien lech toc do vua ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo + tocdo/4;//30 tocdor = 2*tocdo/3;//30; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //******************************************************************************* case 0x04: // co cam bien senln dieu khien lech toc do vua ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo-(1*tocdo)/3; tocdor = tocdo+tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0;


www.nhom3c.com

CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x20: // co cam bien senrn dieu khien lech toc do vua ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo+(tocdo/4); tocdor = tocdo-(1*tocdo)/3; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //************************************************************************** case 0x0C: // co 2 cam bien senlt va senln dieu khien lech toc do vua ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo - tocdo/4; tocdor = tocdo + tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x30: // co 2 cam bien senrt + senrn dieu khien lech toc do vua ve phai { nho_trai=0;nho_phai=1; tocdor = tocdo - tocdo/4; tocdol = tocdo + tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0;


www.nhom3c.com

dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //************************************************************************************* case 0x08: //3 co cam bien senlt dieu khien lech toc do it ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo - tocdo/6; tocdor = tocdo + tocdo/6; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x10: // co cam bien senrt dieu khien lech toc do it ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo + tocdo/6; tocdor = tocdo - tocdo/6; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //*********************************************************************************


www.nhom3c.com

//********************************************************************************* case 0x18: //2 cam bien giua senlt + senrt { nho_trai=0;nho_phai=0; tocdol = tocdo; tocdor = tocdo; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //**************************************************************************** default: // truong hop khong co cam bien nao { tocdol = tocdo/3;//-(2*tocdo)/3; tocdor = tocdo/3;//-(2*tocdo)/3; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } } if ((P0 & 0x3C) == 0x3C) {finish=1;} if ((P0 & 0x07) == 0x07) {finish1=1;} if ((P0 & 0xF0) == 0xF0) {finish1=1;} if ((P0 & 0xE0) == 0xE0) {finish1=1;} } /////////////////////////////////////////////////////////////////////////////////////////


www.nhom3c.com

}

Documents

Giao Trinh Robocon Tom Tat Buoi 2