วันอังคารที่ 29 กันยายน พ.ศ. 2552

GPS หยอดเหรียญ ???











































ดองใว้นาน!!! ตัวหยอดเหรียญ ตอนนั้นซื้อมา 20 บาท ที่ตลาดเซพวัน ดองจนลืม วันนี้จับมาปัดฝุ่น เลยเอามาต่อ MCU เล่น แต่ก็คิดไม่ออกว่าจะทำอะไร จึงจับรวมกับ GPS และ จอ 3310 เป็น GPS หยอดเหรียญ
จากรูปที่สอง เป็นรูปที่ยังไม่หยอดเหรียญ เมื่อหยอดแล้วจะได้รูปที่สามครับ พร้อมทั้งนับเวลาถอยหลังเป็นวินาที (บันทัดสุดท้ายของรูปที่สาม) ซึ่งการเช็คหยอดเหรียญนั้น เช็คแบบ Interrupt ทำให้สามารถหยอดเหรียญได้อย่างต่อเนื่อง การนับถอยหลังก็นับเวลาจากฐานเวลา GPS ซึ่งก็เดินตรงดี แถมด้วยเวลา GPS หลุด(จับดาวเทียมไม่ได้)ก็จะหยุดทำการนับเวลาถอยหลังด้วย เมื่อสัญญานมาก็จะทำการนับถอยหลังต่อ ทำให้ผู้ใช้บริการไม่ขาดทุนได้ข้อมูลตามที่จ่ายเงิน ไม่เอาเปรียบผู้บริโภค เอิ๊กๆ ขนาดจำลอง เล่นเอาซะเต็มที่เลย จากการทดลองนี้ทำให้ข้าพเจ้า ได้ศึกษาและเรียนรู้การติดต่อเครื่องหยอดเหรียญกับไมโครคอนโทรลเลอร์ ( หลอกเครื่องหยอดเหรียญไม่ได้ ) มันมีวงจรเช็คน้ำหนักแบบไม่ต้องช่างน้ำหนักได้ด้วย เก่งจริงๆ ขนาดรุ่นเก่านะนี่ ถ้ารุ่นใหม่ หมดสิทธิหลอกเครื่องแน่นอน

โค๊ดครับ
#include <18f452.h>
#include
#use delay(clock=30000000)
#fuses HS,NOPROTECT,NOWDT,NOBROWNOUT
#use rs232(baud=9600,xmit=pin_c6,rcv=pin_c7,stream=GPS,stream=HOSTPC,stream=B,errors)

#include
#include
#include


#define nok_sclk PIN_B6
#define nok_sda PIN_B5
#define nok_dc PIN_B4
#define nok_cs PIN_B3
#define nok_res PIN_B2

//char image[];

char timei[10],lati[15],loni[15],speedi[10],dayi[10];
float time,lat,lon,speed,day;
float lat1,lat2,lat3,lon1,lon2,lon3,time0,time1,time2,time3,day0,day1,day2;
float t1,t2,t3,d1,d2,d3;

char char_row,charsel,charpos,chardata; // for nokia_3310 lcd
int16 ddram;
int32 plot_value32;
int32 plot_umsb,plot_lmsb,plot_ulsb,plot_llsb;

int16 nub=0;
#INT_EXT
void EXT_ISR(void)
{
delay_us(200);
nub=nub+60;
}

BYTE const TABLE5[240]=
{0x00,0x00,0x00,0x00,0x00, // 20 space ASCII table for NOKIA LCD: 96 rows * 5 bytes= 480 bytes
0x00,0x00,0x5f,0x00,0x00, // 21 !
0x00,0x07,0x00,0x07,0x00, // 22 "
0x14,0x7f,0x14,0x7f,0x14, // 23 #
0x24,0x2a,0x7f,0x2a,0x12, // 24 $
0x23,0x13,0x08,0x64,0x62, // 25 %
0x36,0x49,0x55,0x22,0x50, // 26 &
0x00,0x05,0x03,0x00,0x00, // 27 '
0x00,0x1c,0x22,0x41,0x00, // 28 (
0x00,0x41,0x22,0x1c,0x00, // 29 )
0x14,0x08,0x3e,0x08,0x14, // 2a *
0x08,0x08,0x3e,0x08,0x08, // 2b +
0x00,0x50,0x30,0x00,0x00, // 2c ,
0x08,0x08,0x08,0x08,0x08, // 2d -
0x00,0x60,0x60,0x00,0x00, // 2e .
0x20,0x10,0x08,0x04,0x02, // 2f /
0x3e,0x51,0x49,0x45,0x3e, // 30 0
0x00,0x42,0x7f,0x40,0x00, // 31 1
0x42,0x61,0x51,0x49,0x46, // 32 2
0x21,0x41,0x45,0x4b,0x31, // 33 3
0x18,0x14,0x12,0x7f,0x10, // 34 4
0x27,0x45,0x45,0x45,0x39, // 35 5
0x3c,0x4a,0x49,0x49,0x30, // 36 6
0x01,0x71,0x09,0x05,0x03, // 37 7
0x36,0x49,0x49,0x49,0x36, // 38 8
0x06,0x49,0x49,0x29,0x1e, // 39 9
0x00,0x36,0x36,0x00,0x00, // 3a :
0x00,0x56,0x36,0x00,0x00, // 3b ;
0x08,0x14,0x22,0x41,0x00, // 3c < 3d =" 0x00,0x41,0x22,0x14,0x08,">
0x02,0x01,0x51,0x09,0x06, // 3f ?
0x32,0x49,0x79,0x41,0x3e, // 40 @
0x7e,0x11,0x11,0x11,0x7e, // 41 A
0x7f,0x49,0x49,0x49,0x36, // 42 B
0x3e,0x41,0x41,0x41,0x22, // 43 C
0x7f,0x41,0x41,0x22,0x1c, // 44 D
0x7f,0x49,0x49,0x49,0x41, // 45 E
0x7f,0x09,0x09,0x09,0x01, // 46 F
0x3e,0x41,0x49,0x49,0x7a, // 47 G
0x7f,0x08,0x08,0x08,0x7f, // 48 H
0x00,0x41,0x7f,0x41,0x00, // 49 I
0x20,0x40,0x41,0x3f,0x01, // 4a J
0x7f,0x08,0x14,0x22,0x41, // 4b K
0x7f,0x40,0x40,0x40,0x40, // 4c L
0x7f,0x02,0x0c,0x02,0x7f, // 4d M
0x7f,0x04,0x08,0x10,0x7f, // 4e N
0x3e,0x41,0x41,0x41,0x3e // 4f O 48
};

BYTE const TABLE6[245]=
{0x7f,0x09,0x09,0x09,0x06, // 50 P
0x3e,0x41,0x51,0x21,0x5e, // 51 Q
0x7f,0x09,0x19,0x29,0x46, // 52 R
0x46,0x49,0x49,0x49,0x31, // 53 S
0x01,0x01,0x7f,0x01,0x01, // 54 T
0x3f,0x40,0x40,0x40,0x3f, // 55 U
0x1f,0x20,0x40,0x20,0x1f, // 56 V
0x3f,0x40,0x38,0x40,0x3f, // 57 W
0x63,0x14,0x08,0x14,0x63, // 58 X
0x07,0x08,0x70,0x08,0x07, // 59 Y
0x61,0x51,0x49,0x45,0x43, // 5a Z
0x00,0x7f,0x41,0x41,0x00, // 5b [
0x02,0x04,0x08,0x10,0x20, // 5c
0x00,0x41,0x41,0x7f,0x00, // 5d
0x04,0x02,0x01,0x02,0x04, // 5e
0x40,0x40,0x40,0x40,0x40, // 5f
0x00,0x01,0x02,0x04,0x00, // 60
0x20,0x54,0x54,0x54,0x78, // 61 a
0x7f,0x48,0x44,0x44,0x38, // 62 b
0x38,0x44,0x44,0x44,0x20, // 63 c
0x38,0x44,0x44,0x48,0x7f, // 64 d
0x38,0x54,0x54,0x54,0x18, // 65 e
0x08,0x7e,0x09,0x01,0x02, // 66 f
0x0c,0x52,0x52,0x52,0x3e, // 67 g
0x7f,0x08,0x04,0x04,0x78, // 68 h
0x00,0x44,0x7d,0x40,0x00, // 69 i
0x20,0x40,0x44,0x3d,0x00, // 6a j
0x7f,0x10,0x28,0x44,0x00, // 6b k
0x00,0x41,0x7f,0x40,0x00, // 6c l
0x7c,0x04,0x18,0x04,0x78, // 6d m
0x7c,0x08,0x04,0x04,0x78, // 6e n
0x38,0x44,0x44,0x44,0x38, // 6f o
0x7c,0x14,0x14,0x14,0x08, // 70 p
0x08,0x14,0x14,0x18,0x7c, // 71 q
0x7c,0x08,0x04,0x04,0x08, // 72 r
0x48,0x54,0x54,0x54,0x20, // 73 s
0x04,0x3f,0x44,0x40,0x20, // 74 t
0x3c,0x40,0x40,0x20,0x7c, // 75 u
0x1c,0x20,0x40,0x20,0x1c, // 76 v
0x3c,0x40,0x30,0x40,0x3c, // 77 w
0x44,0x28,0x10,0x28,0x44, // 78 x
0x0c,0x50,0x50,0x50,0x3c, // 79 y
0x44,0x64,0x54,0x4c,0x44, // 7a z
0x00,0x08,0x36,0x41,0x00, // 7b
0x00,0x00,0x7f,0x00,0x00, // 7c
0x00,0x41,0x36,0x08,0x00, // 7d
0x10,0x08,0x08,0x10,0x08, // 7e
0x78,0x46,0x41,0x46,0x78 // 7f
};



//
void readMessage();
void convert();
void nokia_init(void);
void nokia_write_command(char bytefornokia_command);
void nokia_write_data(char bytefornokia_data);
void nokia_write_dorc(char bytefornokia);
void nokia_gotoxy(int8 xnokia, int8 ynokia);
void nokia_erase_y(int8 ynokia);
void nokia_erase_x(int8 xnokia);
void nokia_printchar(int8 cvar);
void nokia_clean_ddram(void);
void table_to_nokialcd(void);
void nokia_plot(int8 xnokia,int8 plot_value8);
void nokia_write_data_inv(char bytefornokia_data_inv);
void nokia_clear_screen(void);
void nokia_clear_xy(int8 xnokia, int8 ynokia);
void nokia_print_uparrow(void);
void nokia_print_downarrow(void);
void nokia_print_leftarrow(void);
void nokia_print_rightarrow(void);
void nokia_print_degree(void);
void nokia_print_lowbatdegree(void);
void lcd_wrimg(char image[],unsigned char x1,y1);
void Init_MCU(void);

void lcd_wrimg(char image[],unsigned char x1,y1)
{
int tablept = 0;
unsigned char y=0,x=0,pix=0,x2=0,y2=0;

x2 = image[tablept];
tablept++;
y2 = image[tablept];
tablept++;
for (y=y1;y<(y1+y2);y++) // Draw the columns to print the character. { nokia_gotoxy(x1,y); for (x=0;x0;ddram--){nokia_write_data(0x00);}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void nokia_write_command(char bytefornokia_command)
{
output_low(nok_dc); // byte is a command it is read with the eight SCLK pulse
output_low(nok_cs); // chip enabled
nokia_write_dorc(bytefornokia_command);
output_high(nok_cs); // chip disabled
}
/////////////////////////////////////////////////////////////////////////////////
void nokia_write_data(char bytefornokia_data)
{
output_high(nok_dc);
output_low(nok_cs); // chip enabled
nokia_write_dorc(bytefornokia_data);
output_high(nok_cs); // chip disabled
}
//////////////////////////////////////////////////////////////////////////////////
void nokia_write_dorc(char bytefornokia) // serial write data or command subroutine
{
char caa;
for (caa=8;caa>0;caa--) {
output_low(nok_sclk);
// delay_us(2);
if ((bytefornokia&0x80)==0x00){output_low(nok_sda);}
else {output_high(nok_sda);}
output_high(nok_sclk);
bytefornokia=bytefornokia<<1; column="0;column!=" charsel="cvar;">0xD2)return;

for (char_row=0;char_row<5;char_row++) charpos="(((charsel&0xff)-0x20)*5);chardata=">0x4f){charpos=(((charsel&0xff)-0x50)*5);chardata=TABLE6[(charpos+char_row)];} // use TABLE6
nokia_write_data(chardata); // send data to nokia
}
nokia_write_data(0x00); // 1 byte (always blank)
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void nokia_plot(int8 xnokia,int8 plot_value8)
{
char i;
plot_value32=0;
// plot_value32=1; // unremark this if you want dotgraph instead of bargraph

for (i=0;i!=plot_value8;i++){

plot_value32=1; // remark this if you want dotgraph instead of bargraph
plot_value32<<=1; } plot_value32=2; // bottom line is always filled plot_llsb=(plot_value32&0xff); plot_ulsb=((plot_value32>>8)&0xff);
plot_lmsb=((plot_value32>>16)&0xff);
plot_umsb=((plot_value32>>24)&0xff);

nokia_gotoxy(xnokia,1);
nokia_write_data_inv(plot_umsb);

nokia_gotoxy(xnokia,2);
nokia_write_data_inv(plot_lmsb);

nokia_gotoxy(xnokia,3);
nokia_write_data_inv(plot_ulsb);

nokia_gotoxy(xnokia,4);
nokia_write_data_inv(plot_llsb);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void nokia_write_data_inv(char bytefornokia_data_inv)
{
char caa;
output_high(nok_dc);
output_low(nok_cs); // chip enabled
for (caa=8;caa>0;caa--) {
output_low(nok_sclk);
delay_us(2);
if ((bytefornokia_data_inv&0x01)==0){output_low(nok_sda);}
else {output_high(nok_sda);}
output_high(nok_sclk);
bytefornokia_data_inv=bytefornokia_data_inv>>1;
}
output_high(nok_cs); // chip disabled
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void nokia_clear_screen(void)
{
nokia_erase_y(0);
nokia_erase_y(1);
nokia_erase_y(2);
nokia_erase_y(3);
nokia_erase_y(4);
nokia_erase_y(5);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void nokia_clear_xy(int8 xnokia, int8 ynokia)
{
nokia_gotoxy(xnokia,ynokia);
nokia_printchar(" ");
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void nokia_print_uparrow(void)
{
nokia_write_data(0x04);
nokia_write_data(0x02);
nokia_write_data(0x7f);
nokia_write_data(0x02);
nokia_write_data(0x04);
nokia_write_data(0x00);
}
//
void nokia_print_downarrow(void)
{
nokia_write_data(0x10);
nokia_write_data(0x20);
nokia_write_data(0x7f);
nokia_write_data(0x20);
nokia_write_data(0x10);
nokia_write_data(0x00);
}
//
void nokia_print_leftarrow(void)
{
nokia_write_data(0x08);
nokia_write_data(0x1c);
nokia_write_data(0x2a);
nokia_write_data(0x08);
nokia_write_data(0x08);
nokia_write_data(0xf8);
}
//
void nokia_print_rightarrow(void)
{
nokia_write_data(0x08);
nokia_write_data(0x08);
nokia_write_data(0x2a);
nokia_write_data(0x1c);
nokia_write_data(0x08);
nokia_write_data(0x00);
}
//
void nokia_print_degree(void)
{
nokia_write_data(0x00);
nokia_write_data(0x06);
nokia_write_data(0x09);
nokia_write_data(0x09);
nokia_write_data(0x06);
nokia_write_data(0x00);
}
//
void nokia_print_lowbatdegree(void)
{
nokia_write_data(0x00);
nokia_write_data(0x06);
nokia_write_data(0x0f);
nokia_write_data(0x0f);
nokia_write_data(0x06);
nokia_write_data(0x00);
}
//////////////////////////////////////////////////////////////////////////////
void readMessage()
{
char c;
char buffer[80];
char k;
char sentent[]="GPRMC";
int16 i,j=0,time_count=0,lat_count=0,lon_count=0,speed_count=0,day_count=0;
do
{
for(i=0;i<15;i++) k="0;k<5;k++)" c="0;" c =" fgetc(GPS);" i="0;i
{
if (buffer[i]==','){j++;}
if (j==1)
{
timei[time_count++]=buffer[i+1];
}
if (j==3)
{
lati[lat_count++]=buffer[i+1];
}
if (j==5)
{
loni[lon_count++]=buffer[i+1];
}
if (j==7)
{
speedi[speed_count++]=buffer[i+1];
}
if (j==9)
{
dayi[day_count++]=buffer[i+1];
}
}
time = atof(timei);
lat = atof(lati);
lon = atof(loni);
speed = atof(speedi);
day = atof(dayi);
}
//////////////////////////////////////////////////////////////////////////////
void convert(void)
{

day0=day*0.0001;
day1=fmod(day0,1);
day2=day0-day1;
///////////////////
d1=day2;
///////////////////
day0=day1*100;
day1=fmod(day0,1);
day2=day0-day1;
///////////////////
d2=day2;
///////////////////
day2=day1*100;
///////////////////
d3=day2;
///////////////////



time0=time*0.0001;
time1=fmod(time0,1);
time2=time0-time1;
if(time2<=17) { time2=time2+7; } else { time2=(17-time2)*-1; } time3=(time2+time1)*10000; /////////////////// t1=time2; /////////////////// time0=time1*100; time1=fmod(time0,1); time2=time0-time1; /////////////////// t2=time2; /////////////////// time2=time1*100; /////////////////// t3=time2; /////////////////// lat=lat/100; lat1=fmod(lat,1); lat2=lat-lat1; lat1=(lat1/60)*100; lat3=lat1+lat2; lon=lon/100; lon1=fmod(lon,1); lon2=lon-lon1; lon1=(lon1/60)*100; lon3=lon1+lon2; } ////////////////////////////////////////////////////////////////////////////// void Init_MCU(void) { enable_interrupts(GLOBAL); enable_interrupts(INT_EXT); ext_int_edge(H_TO_L); } ////////////////////////////////////////////////////////////////////////////// void main() { nokia_init(); Init_MCU(); while(true) { if(nub!=0) { readMessage(); convert(); nokia_clear_screen(); nokia_gotoxy(0,0); printf(nokia_printchar,"DAY : %02.0f:%02.0f:%02.0f",d1,d2,d3); nokia_gotoxy(0,1); printf(nokia_printchar,"T : %02.0f:%02.0f:%02.0f",t1,t2,t3); nokia_gotoxy(0,2); printf(nokia_printchar,"LAT : %4.5f",lat3); nokia_gotoxy(0,3); printf(nokia_printchar,"LON :%4.5f",lon3); nokia_gotoxy(0,4); printf(nokia_printchar,"SP : %3.0f",speed); nokia_gotoxy(0,5); //printf(nokia_printchar," BY NONT_PEET "); printf(nokia_printchar," %lu S ",nub); delay_ms(100); nub--; } else { output_toggle(pin_c2); nokia_clear_screen(); nokia_gotoxy(0,0); printf(nokia_printchar," R "); nokia_gotoxy(0,1); printf(nokia_printchar," M "); nokia_gotoxy(0,2); printf(nokia_printchar," U "); nokia_gotoxy(0,3); printf(nokia_printchar," T "); nokia_gotoxy(0,4); printf(nokia_printchar," I "); lcd_wrimg(aa,15,1); delay_ms(100); } } }

LCD 3310 (ต่อ)(อีก)















ครั้งก่อนทำรูปไม่ได้ แต่วันนี้ทำได้ แต่โค๊ดก็ยังแปลกๆ ยังงงอยู่ เพราะนอนดึกรึเปล่า!!! ตอนนี้ก็ ตี1 แล้ว
แอบติดชื่อมหาลัยด้วย อิอิ... เล่นไปเล่นมาเกิดอยากเล่นจอสีแล้ว แต่ก็รอของอีกตามเคย ทำไมโคราชไม่มีบ้านหม้อบ้างนะ จะได้สบายก็เค้าหน่อย แอบบ่นนิด

วันอาทิตย์ที่ 27 กันยายน พ.ศ. 2552

LCD 3310 (ต่อ)















วันนี้ตื่นสายมัวแต่เล่น LCD 3310 เมื่อคืน ได้นอนตีสองกว่า เอ้าตื่นมาสามโมง ก็เล่นต่อ โดยจับรวมกับ GPS ที่ทำใว้ ซึ่งจออันเก่า 16*2 มันแสดงได้แต่ LAT กับ LON เท่านั้น เพราะหน้าจอมันไม่พอ เจอจอ 3310 เข้าไป แสดงได้หมดเลย แต่จอ 3310 ไม่มีไฟ Back Light ก็เลยสั่ง 5110 มาลอง อีกซักสามวันคงได้มั้ง หลายคนที่เข้ามาคง งง อัพบล๊อกบ่อยจัง ก็ช่วงนี้มันเซ็งๆ เป็นอย่างไรไม่รู้ก็เลยหันหน้าเข้า MCU เพื่อคลายเครียด อ่ะนะ แต่สำหรับบางคนหันหน้าเข้าแล้วคงเครียดหนักกว่าเดิม อิอิ... อย่าไปซีเรียส ทำไม่ได้ก็พักก่อน เดี๋ยวก็ได้เอง ไม่ต้องรีบ สู้ๆครับ

คืนว่างๆกับ LCD 3310















เหตุเกิดจากคืนนี้มันว่างๆไม่มีอะไรทำ เหนื่อยจากการทำรายงานฝึกงานด้วย (ทำยังไม่เสร็จ) ก็เลยคิดอะไรเล่นๆ ก็เลยคิดเอาจอ 3310 มาต่อ MCU เล่น มันมีเหตุผลนะ คือจอ LCD 16*2 ที่เค้าขายกันมันแพง ประมาณ 150 บาท ส่วน 20*4 ก็ไปไกลเลย 400 บาทขึ้นไป ไอ้เราก็งบน้อยแต่อยากมีอะไรเล่นกับเค้ามั่ง ก็เลยนั่งทำมันคืนนี้ล่ะ ผลก็ออกมาดังรูป แสดงตัวอักษร แสดงตัวเลขได้ แต่ยังหาวิธีแสดงรูปไม่ได้ มันดึกแล้ว(รึว่าเช้าแล้วหว่า) ก็เลยพักไว้ก่อน เอาเท่านี้ล่ะ ฝันดีครับ

วันอังคารที่ 22 กันยายน พ.ศ. 2552

เวปขายของจร้า ฝากด้วยครับ

http://www.vt-recycle.com/ เป็นสินค้ามือสองนะครับ ทุกชิ้นเป็นสินค้าที่คัดแล้วจร้าาา

วันจันทร์ที่ 21 กันยายน พ.ศ. 2552

ติดต่อ GPS ได้แล้วครับ






























การอ่านค่าของ GPS

หลักการอ่านค่าของ GPS นั้นต้องศึกษา โปรโตคอล NMEA 0183 ซึ่งจะมีข้อมูลส่งออกมาในรูปแบบการสื่อสายแบบอนุกรม RS-232 Baud rate(bps) 9600 Data bit 8 Parity None จากนั้นจะได้ข้อมูลออกมาดังนี้

06:27:58 $GPRMC,062758.00,V,,,,,,,210909,,,N*70
06:27:58 $GPVTG,,,,,,,,,N*30
06:27:58 $GPGGA,062758.00,,,,,0,00,99.99,,,,,,*68
06:27:58 $GPGSA,A,1,,,,,,,,,,,,,99.99,99.99,99.99*30
06:27:58 $GPGSV,4,1,13,25,73,147,,13,31,206,,11,71,087,,19,19,034,*75
06:27:58 $GPGSV,4,2,13,07,83,005,,23,10,172,,32,05,127,,08,42,332,*7C
06:27:58 $GPGSV,4,3,13,07,83,005,,28,20,316,,20,12,150,,17,26,251,*78
06:27:58 $GPGSV,4,4,13,08,42,332,*47
06:27:58 $GPGLL,,,,,062758.00,V,N*44
06:27:58 $GPZDA,062758.00,21,09,2009,00,00*69
06:27:59 $GPRMC,062759.00,V,,,,,,,210909,,,N*71
06:27:59 $GPVTG,,,,,,,,,N*30
06:27:59 $GPGGA,062759.00,,,,,0,00,99.99,,,,,,*69
06:27:59 $GPGSA,A,1,,,,,,,,,,,,,99.99,99.99,99.99*30
06:27:59 $GPGSV,4,1,13,25,73,147,,13,31,206,,11,71,087,,19,19,034,*75
06:27:59 $GPGSV,4,2,13,07,83,005,,23,09,171,,32,05,127,,08,42,332,*77
06:27:59 $GPGSV,4,3,13,07,83,005,,28,20,316,,20,12,150,,17,26,251,*78
06:27:59 $GPGSV,4,4,13,08,42,332,*47
06:27:59 $GPGLL,,,,,062759.00,V,N*45
06:27:59 $GPZDA,062759.00,21,09,2009,00,00*68

เมื่อได้ข้อมูลต่างๆมาแล้ว ก็มาทำการคัดเอาเฉพาะโปรโตคอลที่ต้องการ ยกตัวอย่างเช่น $GPGGA หรือ $GPRMC จากนั้นก็รับค่าเข้ามาเก็บใน Buffer แล้วทำการตัดขอมูลที่ได้ออกเป็นช่วงๆ เพื่อให้ได้ตำแหน่งที่ต้องการ เช่น ตำแหน่งของ Latitude กับ Longitude แล้วทำการแปลงตัวอักขระที่เก็บใว้ใน Buffer ให้เป็นตัวเลข เพื่อที่จะได้คำนวนหาค่าที่ใช้ได้ เพราะมาตรฐานของค่าที่ได้ไม่ตรงกับ Google Map

การคำนวนค่า GPS

เมื่อทำการแปลงค่าเป็นตัวเลขได้แล้ว ก็นำค่าที่ได้มาหารด้วย 100 เพื่อเลื่อนจุดทศนิยม จากนั้นก็ mod ด้วย 1 เพื่อทำการคัดเอาแต่ทศนิยม จากนั้นก็เอาค่าหลังจุดทศนิยมที่ mod ได้มาลบกับค่าแรก ก็จะได้ค่า ก่อนจุดทศนิยม จากนั้นเอา ค่าหลังจุดทศนิยมมา หารด้วย 60 จากนั้นก็คูณด้วย 100 ก็จะได้ค่าหลังจุดทศนิยมค่าใหม่ จากนั้นก็เอาค่าหลังจุดทศนิยมค่าใหม่ที่ได้บวกกับค่าก่อนจุดทศนิยม ก็จะได้ ค่าที่สามารรถใช้กับ Google ได้

เช่น 12.3456 mod 1 จะได้ 0.3456 จากนั้นก็เอา 12.3456-0.3456 จะได้ 12 จากนั้นเอา (0.3456/60)*100 = 0.576 จากนั้นก็เอามาบวกกับ 12 ก็จะได้ 12+0.576 = 12.576

12.576 คือค่าที่ทำการคำนวณแล้ว ที่สามารถใช้กับ Google ได้ทันที

ส่วนการคำนวนเวลาซึ่งต้องบวกเพิ่มอีก 7 ชั่วโมงลองดูเอานะครับ ผมเขียนแบบลูกทุ่งเลย



เหนื่อยจิงๆกับการอดหลับอดนอนมาเป็นอาทิตย์ แต่ก็คุ้มเพราะเป็นความรู้ใหม่ที่เราต้องตามให้ทันเทคโนโลยี

โค๊ดครับ
#include <18f452.h>
#include
#use delay(clock=20000000)
#fuses HS,NOPROTECT,NOWDT,NOBROWNOUT
#use rs232(baud=9600,xmit=pin_c6,rcv=pin_c7,stream=GPS,stream=HOSTPC,stream=B,errors)

#include
#include
#include

void readMessage();
void convert();
void show();

char timei[10],lati[15],loni[15],speedi[10],dayi[10];
float time,lat,lon,speed,day;
float lat1,lat2,lat3,lon1,lon2,lon3,time1,time2,time3;
int sh;

void readMessage()
{
char c;
char buffer[80];
char k;
char sentent[]="GPRMC";
int16 i,j=0,time_count=0,lat_count=0,lon_count=0,speed_count=0,day_count=0;
do
{
for(i=0;i<15;i++)
{
timei[i]= 0;
lati[i]= 0;
loni[i]= 0;
speedi[i]= 0;
dayi[i]= 0;
}

do
{
while ( fgetc(GPS) != '$' );
for (k=0;k<5;k++) buffer[k]=fgetc(GPS);
} while (strncmp(buffer,sentent,5)!=0);
c=0;
while ( c !='*' && k<79)
{
c = fgetc(GPS);
buffer[k++]= c;
}
if(k<40)
{
lcd_send_byte(0,0x01);
lcd_putc(" Connecting GPS");
lcd_putc("\n BY NONT_PEET");
output_high(pin_c3);
delay_ms(250);
output_low(pin_c3);
delay_ms(250);
}
}while(k<40);

for (i=0;i
{
if (buffer[i]==','){j++;}
if (j==1)
{
timei[time_count++]=buffer[i+1];
}
if (j==3)
{
lati[lat_count++]=buffer[i+1];
}
if (j==5)
{
loni[lon_count++]=buffer[i+1];
}
if (j==7)
{
speedi[speed_count++]=buffer[i+1];
}
if (j==9)
{
dayi[day_count++]=buffer[i+1];
}
}

time = atof(timei);
lat = atof(lati);
lon = atof(loni);
speed = atof(speedi);
day = atof(dayi);

}


void show(void)
{
fprintf(B,"\n\rtime : %6.0f",time3);
fprintf(B,"\n\rLat : %4.5f",lat3);
fprintf(B,"\n\rLon : %5.5f",lon3);
fprintf(B,"\n\rSpeed : %3.0f",speed);
fprintf(B,"\n\rDay : %6.0f",day);
}
void convert(void)
{

time=time*0.0001;
time1=fmod(time,1);
time2=time-time1;
if(time2<=17)
{
time2=time2+7;
}
else
{
time2=(17-time2)*-1;
}
time3=(time2+time1)*10000;

lat=lat/100;
lat1=fmod(lat,1);
lat2=lat-lat1;
lat1=(lat1/60)*100;
lat3=lat1+lat2;

lon=lon/100;
lon1=fmod(lon,1);
lon2=lon-lon1;
lon1=(lon1/60)*100;
lon3=lon1+lon2;
}


void main()
{
lcd_init();
while(true)
{
readMessage();
convert();
//show();
if(sh==1)
{
lcd_send_byte(0,0x01);
printf(lcd_putc,"LAT : %4.5f",lat3);
printf(lcd_putc,"\nLON : %4.5f",lon3);
//fprintf(B,"\n\rDATA:%6.0f:%4.5f:%4.5f:%3.0f:%6.0f:",time3,lat3,lon3,speed,day);
delay_ms(1500);
sh=0;
}
else
{
lcd_send_byte(0,0x01);
printf(lcd_putc,"DAY : %6.0f",day);
printf(lcd_putc,"\nTIME : %6.0f",time3);
delay_ms(1500);
sh=1;
}
output_toggle(pin_c2);
//delay_ms(500);
}

วันอาทิตย์ที่ 30 สิงหาคม พ.ศ. 2552

แจกโค็ดวิ่งตามเส้น (แข่งในเกมส์ ฮาลองเบย์)

ส่วนพอร์ตต่างๆนั้น ดูเอานะครับ ติดตรงไหน เมล์มาถามได้เลยครับ
-ใช้ pic18f452 ความถี่ 40M
-มีการเช็คสีของก้อนไข่มุกด้วยนะครับ
-มีสีละ8โปรแกรม (เล่นเอาคนปล่อยหุ่นงงเลยครับ)
-sensor ใช้ตัวที่ลงใว้ก่อนหน้า เรียงจากซ้ายไปขวา a b c d || e f g h

define osc 40
DEFINE ADC_BITS 8
DEFINE ADC_SAMPLEUS 50

TRISA =255
TRISB =255
TRISC =00
TRISD =255

adcon1 = 2

collor VAR PORTb.0
pro var portb.1
start1 VAR PORTe.0
start2 var porte.1
start3 var porte.2
start4 var portb.2

dr VAR portc.3
dl VAR portc.0

a VAR PORTd.0
b VAR PORTd.1
c VAR PORTd.2
d VAR PORTd.3
e VAR PORTd.4
f VAR PORTd.5
g VAR PORTd.6
h VAR PORTd.7

i VAR BYTE
sen_up var byte
sb var byte
CL VAR BYTE
cg var byte

cg=130

PORTC =111111
pause 1000
gosub set
pause 1000

chk_st:
if (start1=0)and(start2=1)and(start3=1)and(start4=1)and(collor=0)and(pro=1) then
goto red1 :endif
if (start1=1)and(start2=0)and(start3=1)and(start4=1)and(collor=0)and(pro=1) then
goto red2 :endif
if (start1=1)and(start2=1)and(start3=0)and(start4=1)and(collor=0)and(pro=1) then
goto red3 :endif
if (start1=1)and(start2=1)and(start3=1)and(start4=0)and(collor=0)and(pro=1) then
goto red4 :endif
if (start1=0)and(start2=1)and(start3=1)and(start4=1)and(collor=0)and(pro=0) then
goto red5 :endif
if (start1=1)and(start2=0)and(start3=1)and(start4=1)and(collor=0)and(pro=0) then
goto red6 :endif
if (start1=1)and(start2=1)and(start3=0)and(start4=1)and(collor=0)and(pro=0) then
goto red7 :endif
if (start1=1)and(start2=1)and(start3=1)and(start4=0)and(collor=0)and(pro=0) then
goto red8 :endif

if (start1=0)and(start2=1)and(start3=1)and(start4=1)and(collor=1)and(pro=1) then
goto green1 :endif
if (start1=1)and(start2=0)and(start3=1)and(start4=1)and(collor=1)and(pro=1) then
goto green2 :endif
if (start1=1)and(start2=1)and(start3=0)and(start4=1)and(collor=1)and(pro=1) then
goto green3 :endif
if (start1=1)and(start2=1)and(start3=1)and(start4=0)and(collor=1)and(pro=1) then
goto green4 :endif
if (start1=0)and(start2=1)and(start3=1)and(start4=1)and(collor=1)and(pro=0) then
goto green5 :endif
if (start1=1)and(start2=0)and(start3=1)and(start4=1)and(collor=1)and(pro=0) then
goto green6 :endif
if (start1=1)and(start2=1)and(start3=0)and(start4=1)and(collor=1)and(pro=0) then
goto green7 :endif
if (start1=1)and(start2=1)and(start3=1)and(start4=0)and(collor=1)and(pro=0) then
goto green8 :endif
goto chk_st


red1:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub left
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub left
gosub forward
gosub forward
gosub right
gosub forward
gosub forward
gosub left
gosub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub right
gosub set
gosub chk_line
gosub forward
gosub right
gosub forward
gosub forward
gosub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub forward
gosub left
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub left1
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub left
gosub forward
gosub left
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub left
gosub down
gosub left
gosub forward
gosub up1
gosub forward1
pause 500
gosub beep
gosub up
gosub bb
gosub right
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub left
gosub forward
gosub right
gosub set
End
red2:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
gosub right
GoSub forward
GoSub forward
GoSub forward
gosub left
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub left
GoSub forward
GoSub forward
GoSub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub forward
gosub forward
gosub left
gosub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub right
gosub set
gosub chk_line
gosub forward
gosub right
gosub forward
gosub forward
gosub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub forward
gosub left
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub left1
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub left
gosub forward
gosub left
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub left
gosub down
gosub left
gosub forward
gosub up1
gosub forward1
pause 500
gosub beep
gosub up
gosub bb
gosub right
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub left
gosub forward
gosub right
gosub set
End
red3:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
goSub forward
GoSub forward
goSub forward
GoSub forward
goSub forward
gosub left
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub forward
gosub left
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub left1
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub left
gosub forward
gosub left
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub left
gosub down
gosub left
gosub forward
gosub up1
gosub forward1
pause 500
gosub beep
gosub up
gosub bb
gosub right
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub left
gosub forward
gosub right
gosub set
End
red4:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
goSub forward
GoSub forward
GoSub forward
gosub left
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
GoSub forward
GoSub forward
gosub left
GoSub forward
goSub forward
GoSub forward
goSub forward
GoSub forward
GoSub forward
GoSub forward
gosub right
goSub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub up
pause 2000
gosub beep
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
gosub bb
gosub left
End
red5:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
goSub forward
GoSub forward
goSub forward
gosub left
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
goSub forward
gosub left
goSub forward
goSub forward
gosub right
goSub forward
gosub left
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub bb
gosub left1
gosub set
gosub chk_line
goSub forward
gosub right
goSub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub left
GoSub forward
goSub forward
GoSub forward
goSub forward
gosub right
goSub forward
goSub forward
gosub left
goSub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub left
gosub left
gosub chk_line
gosub down
gosub beep_up
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub right
pause 500
gosub chk_line
gosub set
gosub forward
gosub left
gosub forward
gosub bbb
End
red6:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub left
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub left
gosub forward
gosub forward
gosub right
gosub forward
gosub forward
gosub left
gosub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub right
gosub set
gosub chk_line
gosub forward
gosub right
gosub forward
gosub forward
gosub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
gosub forward
gosub forward
gosub forward
gosub left
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
pause 500
gosub chk_line
gosub up
gosub right
pause 500
gosub chk_line
gosub forward1
gosub d_paste
gosub bb
End
red7:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
gosub right
GoSub forward
GoSub forward
GoSub forward
gosub left
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
pause 500
gosub chk_line
gosub up1
gosub right
pause 500
gosub chk_line
gosub forward1
gosub beep
pause 500
gosub paste
gosub up
pause 1000
gosub beep
gosub bb
gosub left
gosub set
gosub chk_line
gosub forward
End
red8:
gosub ffff
gosub left_v
gosub ffff2
gosub chk_line
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub left
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor1
gosub left1
gosub forward
GoSub forward
GoSub forward
gosub left
gosub forward
gosub right
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
pause 2000
gosub up
pause 2000
gosub beep
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
gosub paste
End
green1:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub right
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub right
gosub forward
gosub forward
gosub left
gosub forward
gosub forward
gosub right
gosub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub left
gosub set
gosub chk_line
gosub forward
gosub left
gosub forward
gosub forward
gosub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub forward
gosub right
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub left1
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub right
gosub forward
gosub right
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub right
gosub down
gosub right
gosub forward
gosub up1
gosub forward1
pause 500
gosub beep
gosub up
gosub bb
gosub left
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub right
gosub forward
gosub left
gosub set
End
green2:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
gosub left
GoSub forward
GoSub forward
GoSub forward
gosub right
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub right
GoSub forward
GoSub forward
GoSub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub forward
gosub forward
gosub right
gosub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub left
gosub set
gosub chk_line
gosub forward
gosub left
gosub forward
gosub forward
gosub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub forward
gosub right
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub left1
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub right
gosub forward
gosub right
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub right
gosub down
gosub right
gosub forward
gosub up1
gosub forward1
pause 500
gosub beep
gosub up
gosub bb
gosub left
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub right
gosub forward
gosub left
gosub set
End
green3:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
goSub forward
GoSub forward
goSub forward
GoSub forward
goSub forward
gosub right
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub forward
gosub right
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub left1
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub right
gosub forward
gosub right
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub right
gosub down
gosub right
gosub forward
gosub up1
gosub forward1
pause 500
gosub beep
gosub up
gosub bb
gosub left
gosub down
gosub beep_up
gosub chk_line
gosub forward
gosub right
gosub forward
gosub left
gosub set
End
green4:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
goSub forward
GoSub forward
GoSub forward
gosub right
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
GoSub forward
GoSub forward
gosub right
GoSub forward
goSub forward
GoSub forward
goSub forward
GoSub forward
GoSub forward
GoSub forward
gosub left
goSub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub up
pause 2000
gosub beep
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
gosub bb
gosub right
End
green5:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
goSub forward
GoSub forward
goSub forward
gosub right
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
goSub forward
gosub right
goSub forward
goSub forward
gosub left
goSub forward
gosub right
pause 1500
gosub chk_line
gosub up
pause 1000
gosub chk_line
GoSub forward1
gosub d_paste
gosub bb
gosub left1
gosub set
gosub chk_line
goSub forward
gosub left
goSub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub right
GoSub forward
goSub forward
GoSub forward
goSub forward
gosub left
goSub forward
goSub forward
gosub right
goSub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub d_box1
gosub bb
gosub right
gosub right
gosub chk_line
gosub forward
pause 300
gosub forward1
gosub d_paste
gosub bb
gosub left
pause 500
gosub chk_line
gosub set
gosub forward
gosub right
gosub forward
gosub bbb
End
green6:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub right
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub right
gosub forward
gosub forward
gosub left
gosub forward
gosub forward
gosub right
gosub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
gosub bb
gosub left
gosub set
gosub chk_line
gosub forward
gosub left
gosub forward
gosub forward
gosub forward
gosub bbb
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
gosub forward
gosub forward
gosub forward
gosub right
gosub forward
gosub forward
gosub forward
gosub forward
gosub forward
pause 500
gosub chk_line
gosub up
gosub left
pause 500
gosub chk_line
gosub forward1
gosub d_paste
gosub bb
End

green7:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
gosub left
GoSub forward
GoSub forward
GoSub forward
gosub right
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
pause 500
gosub chk_line
gosub up1
gosub left
pause 500
gosub chk_line
gosub forward1
gosub beep
pause 500
gosub paste
gosub up
pause 1000
gosub beep
gosub bb
gosub right
gosub set
gosub chk_line
gosub forward
End
green8:
gosub ffff
gosub right_v
gosub ffff2
gosub chk_line
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
GoSub forward
gosub right
pause 2000
gosub chk_line
pause 1000
gosub chk_line
gosub wait_box
GoSub forward1
gosub beep_up
gosub chk_cor2
gosub left1
gosub forward
GoSub forward
GoSub forward
gosub right
gosub forward
gosub left
gosub forward
gosub bbb
gosub up
gosub forward1
gosub d_paste
pause 2000
gosub up
pause 2000
gosub beep
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
pause 30000
gosub paste
End


forward:
IF (d=0)AND(e=0) Then
high dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 6
EndIF
IF (d=1)AND(e=0) Then
low dl : high dr
low portc.2 'motor r
high portc.1 'motor l
Pause 1
EndIF
IF (d=0)AND(e=1) Then
high dl : low dr
high portc.2 'motor r
low portc.1 'motor l
Pause 1
EndIF
IF (d=1)AND(e=1) Then
high portc.2 'motor r
high portc.1 'motor l
goto out
EndIF
IF (c=1)AND(f=0) Then
low dl :high dr
low portc.2 'motor r
low portc.1 'motor l
Pause 4
EndIF
IF (c=0)AND(f=1) Then
high dl : low dr
low portc.2 'motor r
low portc.1 'motor l
Pause 4
EndIF
IF (b=1)AND(g=0) Then
low dl :high dr
low portc.2 'motor r
low portc.1 'motor l
Pause 8
EndIF
IF (b=0)AND(g=1) Then
high dl : low dr
low portc.2 'motor r
low portc.1 'motor l
Pause 8
EndIF
IF (d=1)AND(e=1) Then
high portc.2 'motor r
high portc.1 'motor l
goto out
EndIF
GoTo forward

out:
high dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 100
IF (c=0)AND(f=0) Then
low portc.2 'motor r
low portc.1 'motor l
Pause 60
gosub line_ff
return
endif
goto out

chk_line:
IF (a=1) Then
high dr : low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 4
high portc.2 'motor r
high portc.1 'motor l
pause 120
EndIF
IF (h=1) Then
low dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 4
high portc.2 'motor r
high portc.1 'motor l
pause 120
EndIF
IF (b=1) Then
high dr : low dl
PAUSE 20
low portc.2 'motor r
low portc.1 'motor l
Pause 2
high portc.2 'motor r
high portc.1 'motor l
pause 200
EndIF
IF (g=1) Then
low dr : high dl
PAUSE 20
low portc.2 'motor r
low portc.1 'motor l
Pause 2
high portc.2 'motor r
high portc.1 'motor l
pause 200
EndIF
IF (c=1) Then
high dr : low dl
PAUSE 20
low portc.2 'motor r
low portc.1 'motor l
Pause 1
high portc.2 'motor r
high portc.1 'motor l
pause 200
EndIF
IF (f=1) Then
low dr : high dl
PAUSE 20
low portc.2 'motor r
low portc.1 'motor l
Pause 1
high portc.2 'motor r
high portc.1 'motor l
pause 200
EndIF
IF (d=1) Then
high dr : low dl
Pause 20
low portc.2 'motor r
low portc.1 'motor l
Pause 1
high portc.2 'motor r
high portc.1 'motor l
pause 200
EndIF
IF (e=1) Then
high dl : low dr
Pause 20
low portc.2 'motor r
low portc.1 'motor l
Pause 1
high portc.2 'motor r
high portc.1 'motor l
pause 200
EndIF
IF (a=0)AND(b=0)AND(c=0)AND(d=0)and(e=0)and(f=0)AND(g=0)AND(h=0) Then
high dr : high dl
high portc.2 'motor r
high portc.1 'motor l
Return
EndIF
GoTo chk_line

line_ff:
IF (a=1) Then
high dr : low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 20
high portc.2 'motor r
high portc.1 'motor l
EndIF
IF (h=1) Then
low dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 20
high portc.2 'motor r
high portc.1 'motor l
EndIF
IF (b=1) Then
high dr : low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 20
high portc.2 'motor r
high portc.1 'motor l
EndIF
IF (g=1) Then
low dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 20
high portc.2 'motor r
high portc.1 'motor l
EndIF
IF (c=1) Then
high dr : low dl
Pause 5
low portc.2 'motor r
low portc.1 'motor l
Pause 4
high portc.2 'motor r
high portc.1 'motor l
Pause 30
EndIF
IF (f=1) Then
low dr : high dl
Pause 5
low portc.2 'motor r
low portc.1 'motor l
Pause 4
high portc.2 'motor r
high portc.1 'motor l
Pause 30
EndIF
IF (a=0)AND(b=0)AND(c=0)and(f=0)AND(g=0)AND(h=0) Then
high dr : high dl
high portc.2 'motor r
high portc.1 'motor l
Return
EndIF
GoTo line_ff

set:
gosub beep
gosub down
gosub paste
return

left:
high dr : low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 3
high portc.2 'motor r
high portc.1 'motor l
pause 100
IF (h=1) Then
goto lefta
EndIF
IF (g=1) Then
goto lefta
EndIF
goto left
lefta:
high dr : low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 3
high portc.2 'motor r
high portc.1 'motor l
pause 100
IF (d=1) Then
pause 300
gosub chk_line
return
EndIF
IF (c=1) Then
pause 300
gosub chk_line
return
EndIF
goto lefta

left_v:
high dr : low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 3
high portc.2 'motor r
high portc.1 'motor l
pause 100
IF (b=1) Then
pause 300
gosub chk_line
return
EndIF
IF (c=1) Then
pause 300
gosub chk_line
return
EndIF
goto left_v

right:
low dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 3
high portc.2 'motor r
high portc.1 'motor l
pause 100
IF (a=1) Then
goto lefta
EndIF
IF (b=1) Then
goto righta
EndIF
goto right
righta:
low dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 3
high portc.2 'motor r
high portc.1 'motor l
pause 100
IF (f=1) Then
pause 300
gosub chk_line
return
EndIF
IF (e=1) Then
pause 300
gosub chk_line
return
EndIF
goto righta

right_v:
low dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 3
high portc.2 'motor r
high portc.1 'motor l
pause 100
IF (f=1) Then
pause 300
gosub chk_line
return
EndIF
IF (g=1) Then
pause 300
gosub chk_line
return
EndIF
goto right_v

left1:
High dr :Low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 2000
high portc.2 'motor r
high portc.1 'motor l
Pause 500'20
chkl1:
High dr :Low dl
low portc.2 'motor r
low portc.1 'motor l
Pause 5
high portc.2 'motor r
high portc.1 'motor l
Pause 100'20
IF (d=1) Then
return
EndIF
IF (c=1) Then
return
EndIF
goto chkl1

beep:
for i= 1 to 60
if portb.4 = 0 then
low portc.6
return
else
high portc.6
pause 25
endif
next
low portc.6
return

beep_up:
if portb.4 = 1 then
high portc.6
pause 25
goto beep_up
else
low portc.6
low portc.4 'dir up
low portc.5 'motor up
pause 1000
high portc.4 'dir up
high portc.5 'motor up
pause 1000
return
endif
return

paste:
for i= 1 to 60
if portb.5 = 0 then
low portc.7
return
else
high portc.7
pause 25
endif
next i
low portc.7
return
return

up:
for i= 1 to 100
if portb.6 = 0 then
high portc.4 'dir up
high portc.5 'motor up
return
else
low portc.4 'dir up
low portc.5 'motor up
pause 100
endif
next i
high portc.4 'dir up
high portc.5 'motor up
return
return

up1:
low portc.4 'dir up
low portc.5 'motor up
pause 2200
high portc.4 'dir up
high portc.5 'motor up
return

up_xx:
low portc.4 'dir up
low portc.5 'motor up
pause 250
high portc.4 'dir up
high portc.5 'motor up
return


down:
for i = 1 to 180
if portb.7 = 0 then
high portc.4 'dir down
high portc.5 'motor down
return
else
high portc.4 'dir up
low portc.5 'motor up
pause 40
endif
next i
high portc.4 'dir up
high portc.5 'motor up
return
return

d_paste:
pause 1000
high dr : high dl
low portc.2 'motor r
low portc.1 'motor l
Pause 4
high portc.2 'motor r
high portc.1 'motor l
d_p:
if portb.7 = 0 then
return:endif
high portc.4 'dir down
low portc.5 'motor down
pause 20
adcin 1,sen_up
if (sen_up <>=105) then
High dr :High dl
high portc.2 'motor r
high portc.1 'motor l
return
EndIF
IF (d=1)AND(e=0) Then
low dl :High dr
low portc.2 'motor r
high portc.1 'motor l
Pause 8
high portc.2 'motor r
high portc.1 'motor l
pause 40
EndIF
IF (d=0)AND(e=1) Then
low dr :High dl
high portc.2 'motor r
low portc.1 'motor l
Pause 8
high portc.2 'motor r
high portc.1 'motor l
pause 40
EndIF
IF (d=0)AND(e=0) Then
High dr :High dl
low portc.2 'motor r
low portc.1 'motor l
Pause 6
high portc.2 'motor r
high portc.1 'motor l
pause 100
EndIF
IF (d=1)AND(e=1) Then
high dr : high dl
high portc.2 'motor r
high portc.1 'motor l
return
EndIF
GoTo forward1

chk_cor1: 'red
adcin 2,cl
if (cl > cg) then
goto no_red1
else
return
endif
no_red1:
gosub left1
gosub forward
gosub left
gosub forward
gosub set
gosub right
gosub right
gosub forward
gosub right
pause 1500
gosub chk_line
gosub wait_box
gosub forward1
gosub beep_up
goto chk_cor1


chk_cor2: 'green
adcin 2,cl
if (cl < cg ) then
goto no_green 'no green
else
return
endif
no_green:
gosub left1
gosub forward
gosub left
gosub forward
gosub set
gosub right
gosub right
gosub forward
gosub right
pause 1500
gosub chk_line
gosub wait_box
gosub forward1
gosub beep_up
goto chk_cor2