FIre fighting source code codevision avr

#include
#include

// Alphanumeric LCD Module functions
/*#asm
.equ __lcd_port=0x1B ;PORTA
#endasm
#include
#define spasi 0x20
#include */
#define kompas PINA

//uvtron
#define uv_1 PIND.6 //Q
#define uv_2 PIND.7 //Q bar
#define uv2 PINC.6 //Q bar

//IR
#define IR_depan PIND.4
#define IR_blkg PIND.5

//switch
//3 -> 1,2 off
//2 -> 1 on
//1 -> 2 on
#define sw1 PINB.0
#define sw2 PINB.1

#define servo PORTB.3
#define kipas PORTB.4 //aktif low
//pwm kipas
// Timer 2 overflow interrupt service routine
//TIMSK|=0x40
/*unsigned char duty_cyc_kipas,y;
interrupt [TIM2_OVF] void timer2_ovf_isr(void)
{
TCNT2=0xc0;
y++;
if(y==255) y=0;
if(y>=duty_cyc_kipas) kipas=1;
else kipas=0;
}*/


//rotary PB.2
unsigned int rotary;
// External Interrupt 2 service routine
interrupt [EXT_INT2] void ext_int2_isr(void)
{
rotary++;

}
void rst_rotary()
{
rotary=0;
}

//mka lebih cepat
//motor
#define pwmKi PORTC.0
#define pwmKa PORTC.1

#define mKi_plus PORTC.2
#define mKi_min PORTC.3
#define mKa_plus PORTC.4
#define mKa_min PORTC.5


unsigned char pwmKi_val,pwmKa_val,x,i = 0;
// Timer 0 overflow interrupt service routine
interrupt [TIM0_OVF] void timer0_ovf_isr(void)
{
TCNT0=0xC0;

x++;
// Place your code here
if(x==255){x=0;}
if(x>=pwmKa_val)pwmKa=0;
else pwmKa=1;
if(x>=pwmKi_val)pwmKi=0;
else pwmKi=1;

}

void rem()
{
pwmKi_val = 0;
pwmKa_val = 0;
mKi_plus = 1;
mKi_min = 1;
mKa_plus = 1;
mKa_min = 1;
//TOIE0 = 0;
TIMSK &= 0xfe;
}

void maju()
{
mKi_plus = 1;
mKi_min = 0;
mKa_plus = 1;
mKa_min = 0;
}

void mundur()
{
mKi_plus = 0;
mKi_min = 1;
mKa_plus = 0;
mKa_min = 1;
}

void blk_ka()
{
mKi_plus = 1;
mKi_min = 0;
mKa_plus = 0;
mKa_min = 1;
}

void blk_ki()
{
mKi_plus = 0;
mKi_min = 1;
mKa_plus = 1;
mKa_min = 0;
}

void cw_ki()
{
mKi_plus = 1;
mKi_min = 0;
}

void ccw_ki()
{
mKi_plus = 0;
mKi_min = 1;
}

void cw_ka()
{
mKa_plus = 1;
mKa_min = 0;
}

void ccw_ka()
{
mKa_plus = 0;
mKa_min = 1;
}

void gas() //used after rem only, and dir & pwm must be setted before
{
TCNT0=0xC0; //set F
//TOIE0 = 1;
TIMSK |= 0x01;
}

#define utara 192
#define timur 223
#define selatan1 254
#define selatan2 128
#define barat 160

//pwm80
#define p_utara 168
#define p_timur 224
#define p_selatan 252
#define p_barat 146

void putar(unsigned char set_kompas, unsigned char pwm, unsigned char kiri)
{
if (kiri==0) {
blk_ka();
pwmKi_val=pwm;
pwmKa_val=pwm;
} else {
blk_ki();
pwmKi_val=pwm;
pwmKa_val=pwm;
}
gas();
while (!(kompas==(set_kompas-1) || kompas==set_kompas || kompas==(set_kompas+1)))
{
};
pwmKi_val=0;
pwmKa_val=0;
rem();
}

void enable_bumper() {
GICR|=0xC0;
}

void disable_bumper() {
GICR&=0x3f;
}

bit kiri_hit, kanan_hit;
// External Interrupt 0 service routine
//bumper kiri
interrupt [EXT_INT0] void ext_int0_isr(void)
{
rem();
mundur();
pwmKi_val=80;
pwmKa_val=80;
gas();
delay_ms(150);
rem();

blk_ka();
pwmKi_val=80;
pwmKa_val=80;
gas();
delay_ms(200);
rem();
delay_ms(10);
maju();
gas();
}

// External Interrupt 1 service routine
//bumper kanan
interrupt [EXT_INT1] void ext_int1_isr(void)
{
rem();
mundur();
pwmKi_val=80;
pwmKa_val=80;
gas();
delay_ms(150);
rem();

blk_ki();
pwmKi_val=80;
pwmKa_val=80;
gas();
delay_ms(200);
rem();
delay_ms(10);
maju();
gas();
}


//from left to right : ping0 - ping5
unsigned char dataPing[6];

//USART
#define RXB8 1
#define TXB8 0
#define UPE 2
#define OVR 3
#define FE 4
#define UDRE 5
#define RXC 7

#define FRAMING_ERROR (1<#define PARITY_ERROR (1<#define DATA_OVERRUN (1<#define DATA_REGISTER_EMPTY (1<#define RX_COMPLETE (1<
// USART Receiver buffer
#define RX_BUFFER_SIZE 7
unsigned char rx_buffer[RX_BUFFER_SIZE];
unsigned char rx_wr_index,rx_rd_index,rx_counter;
// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;

// USART Receiver interrupt service routine
#pragma savereg-
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
#asm
push r26
push r27
push r30
push r31
in r26,sreg
push r26
#endasm
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer[rx_wr_index]=data;
//dataPing[rx_wr_index]=data;
if (data==0x00 || (++rx_wr_index == RX_BUFFER_SIZE)) rx_wr_index=0;
if (++rx_counter == RX_BUFFER_SIZE)
{
rx_counter=0;
rx_buffer_overflow=1;
};
dataPing[4] = rx_buffer[0];
dataPing[0] = rx_buffer[1];
dataPing[3] = rx_buffer[2];
dataPing[1] = rx_buffer[3];
dataPing[5] = rx_buffer[4];
dataPing[2] = rx_buffer[5];
};
#asm
pop r26
out sreg,r26
pop r31
pop r30
pop r27
pop r26
#endasm
}
#pragma savereg+

#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index];
if (++rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif



// Standard Input/Output functions
#include
#include
// Declare your global variables here
void tampil(unsigned char dat)
{
unsigned char data;

data = dat / 100;
data+=0x30;
// lcd_putchar(data);

dat%=100;
data = dat / 10;
data+=0x30;
// lcd_putchar(data);

dat%=10;
data = dat + 0x30;
// lcd_putchar(data);

}





/*
void debugPING()
{
lcd_gotoxy(0,0);
tampil(dataPing[0]);

lcd_gotoxy(4,0);
tampil(dataPing[1]);

lcd_gotoxy(8,0);
tampil(dataPing[2]);

lcd_gotoxy(0,1);
tampil(dataPing[3]);

lcd_gotoxy(4,1);
tampil(dataPing[4]);

lcd_gotoxy(8,1);
tampil(dataPing[5]);
} */

//wall following
//ari's rule
//us0,5
#define jauh_sekali_MAX -22
#define jauh_sekali_MIN -12
#define jauh_MAX -12
#define jauh_MIN -2
#define pas_MAX -2
#define pas_ONE 2
#define pas_MIN 12
#define dekat_MIN 12
//us1,4
#define _jauh_sekali_MAX -31
#define _jauh_sekali_MIN -21
#define _jauh_MAX -21
#define _jauh_MIN -1
#define _pas_MAX -1
#define _pas_ONE 1
#define _pas_MIN 21
#define _dekat_MIN 21
int err4_0, err5_0;
unsigned char set1, set2;
bit ski,bki,lurus,bka,ska;
void rule_sedang(unsigned char speed, unsigned char max, unsigned char deriv, unsigned char left)
{
int data_us0, data_us1, data_us4, data_us5;
int x_tot, x_ceq, steer, val_bka, val_bki, val_ska, val_ski,val_lurus;
unsigned char jauh_pol5, jauh5, pas5,dkt5,jauh_pol4,jauh4,pas4,dkt4;
unsigned char val_jauhpol4,val_jauh4,val_pas4,val_dkt4;
unsigned char val_jauhpol5,val_jauh5,val_pas5,val_dkt5,rs;
int err4,derr4,err5,derr5,mka,mki;

data_us5 = dataPing[5];
data_us4 = dataPing[4];
data_us0 = dataPing[0];
data_us1 = dataPing[1];

if(left==0)
{
err5=set1-data_us5;
err4=set2-data_us4;
}
else
{
err5=set1-data_us0;
err4=set2-data_us1;
}

//error5
derr5=err5-err5_0;
//jauh sekali
if (err5<=jauh_sekali_MAX)
{
jauh_pol5=1;
val_jauhpol5 = 100;
} else if (err5 {
jauh_pol5=1;
val_jauhpol5=100-(10*(err5-(jauh_sekali_MAX)));
}
else jauh_pol5 = 0;
//jauh
if (err5<=jauh_sekali_MAX) jauh5=0;
else if (err5<=jauh_MAX) {
jauh5=1;
val_jauh5=10*(err5-(jauh_sekali_MAX));
}
else if (err5 jauh5=1;
val_jauh5=100-(10*(err5-(jauh_MAX)));
}
else jauh5=0;
//pas
if(err5<=jauh_MAX) pas5=0;
else if (err5 pas5=1;
val_pas5=10*(err5-(jauh_MAX));
}
else if (err5<=pas_ONE) {
pas5=1;
val_pas5=100;
}
else if(err5 pas5=1;
val_pas5=100-(10*(err5-pas_ONE));
}
else pas5=0;
//dekat
if (err5<=pas_ONE) dkt5=0;
else if(err5 {
dkt5=1;
val_dkt5=(10*(err5-pas_ONE));
}
else
{
dkt5=1;
val_dkt5=100;
}

//error4
derr4=err4-err4_0;
//jauh sekali
if (err4<=_jauh_sekali_MAX)
{
jauh_pol4=1;
val_jauhpol4 = 100;
} else if (err4<_jauh_sekali_MIN)
{
jauh_pol4=1;
val_jauhpol4=100-(10*(err4-(_jauh_sekali_MAX)));
}
else jauh_pol4 = 0;
//jauh
if (err4<=_jauh_sekali_MAX) jauh4=0;
else if (err4<=_jauh_MAX) {
jauh4=1;
val_jauh4=10*(err4-(_jauh_sekali_MAX));
}
else if (err4<_jauh_MIN) {
jauh4=1;
val_jauh4=100-(10*(err4-(_jauh_MAX)));
}
else jauh4=0;
//pas
if(err4<=_jauh_MAX) pas4=0;
else if (err4<_pas_MAX) {
pas4=1;
val_pas4=10*(err4-(_jauh_MAX));
}
else if (err4<=_pas_ONE) {
pas4=1;
val_pas4=100;
}
else if(err4<_pas_MIN) {
pas4=1;
val_pas4=100-(10*(err4-_pas_ONE));
}
else pas4=0;
//dekat
if (err4<=_pas_ONE) dkt4=0;
else if(err4<_dekat_MIN)
{
dkt4=1;
val_dkt4=(10*(err4-_pas_ONE));
}
else
{
dkt4=1;
val_dkt4=100;
}

bki=0;
ski=0;
lurus=0;
ska=0;
bka=0;
val_bki=0;
val_ski=0;
val_lurus=0;
val_ska=0;
val_bka=0;

//rule
if (jauh_pol4!=0){
if (jauh_pol5!=0)
{ bka=1;
if (val_jauhpol4 else rs=val_jauhpol5;
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauhpol4 else rs=val_jauh5;
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauhpol4 else rs=val_pas5;
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ ska=1;
if (val_jauhpol4 else rs=val_dkt5;
if (rs>val_ska) val_ska=rs;
}
}
if (jauh4!=0){
if (jauh_pol5!=0)
{ bka=1;
if (val_jauh4 else rs=val_jauhpol5;
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauh4 else rs=val_jauh5;
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauh4 else rs=val_pas5;
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_jauh4 else rs=val_dkt5;
if (rs>val_lurus) val_lurus=rs;
}
}
if (pas4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_pas4 else rs=val_jauhpol5;
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_pas4 else rs=val_jauh5;
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ lurus=1;
if (val_pas4 else rs=val_pas5;
if (rs>val_lurus) val_lurus=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_pas4 else rs=val_dkt5;
if (rs>val_lurus) val_lurus=rs;
}
}
if (dkt4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_dkt4 else rs=val_jauhpol5;
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_dkt4 else rs=val_jauh5;
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ bki=1;
if (val_dkt4 else rs=val_pas5;
if (rs>val_bki) val_bki=rs;
}
if (dkt5!=0)
{ bki=1;
if (val_dkt4 else rs=val_dkt5;
if (rs>val_bki) val_bki=rs;
}
}
//defuzzyfikasi motor:
x_tot=0;
if (bki!=0)
{ x_ceq = (-0.1)*(val_bki+100);
x_tot += (x_ceq*val_bki);
}
if (ski!=0)
{ x_tot += ((-50) * val_ski);
}
if (ska!=0)
{ x_tot += (70 * val_ska);
}
if (bka!=0)
{ x_ceq = 0.7*(val_bka+100);
x_tot += (x_ceq * val_bka);
}
steer = x_tot / (val_bki + val_ski + val_lurus + val_ska + val_bka);
steer -= (derr4*deriv);

if (left==0)
{ mka=speed-steer;
mki=speed+steer;
}
else
{ mki=speed-steer;
mka=speed+steer;
}

if (mka<0) mka=0;
if (mki<0) mki=0;

if (mka>max) mka=max;
if (mki>max) mki=max;

pwmKa_val=mka;
pwmKi_val=mki;
err4_0=err4;
err5_0=err5;
}


void rule_cepat(unsigned char speed, unsigned char max, unsigned char deriv, unsigned char left, unsigned char min)
{
int data_us0, data_us1, data_us4, data_us5;
int x_tot, x_ceq, steer, val_bka, val_bki, val_ska, val_ski,val_lurus;
unsigned char jauh_pol5, jauh5, pas5,dkt5,jauh_pol4,jauh4,pas4,dkt4;
unsigned char val_jauhpol4,val_jauh4,val_pas4,val_dkt4;
unsigned char val_jauhpol5,val_jauh5,val_pas5,val_dkt5,rs;
int err4,derr4,err5,derr5,mka,mki,x1,x2;

data_us5 = dataPing[5];
data_us4 = dataPing[4];
data_us0 = dataPing[0];
data_us1 = dataPing[1];

if(left==0)
{
err5=set1-data_us5;
err4=set2-data_us4;
}
else
{

err5=set1-data_us0;
err4=set2-data_us1;
}

//error5
derr5=err5-err5_0;
//jauh sekali
if (err5<=jauh_sekali_MAX)
{
jauh_pol5=1;
val_jauhpol5 = 100;
} else if (err5 {
jauh_pol5=1;
val_jauhpol5=100-(10*(err5-(jauh_sekali_MAX)));
}
else jauh_pol5 = 0;
//jauh
if (err5<=jauh_sekali_MAX) jauh5=0;
else if (err5<=jauh_MAX) {
jauh5=1;
val_jauh5=10*(err5-(jauh_sekali_MAX));
}
else if (err5 jauh5=1;
val_jauh5=100-(10*(err5-(jauh_MAX)));
}
else jauh5=0;
//pas
if(err5<=jauh_MAX) pas5=0;
else if (err5 pas5=1;
val_pas5=10*(err5-(jauh_MAX));
}
else if (err5<=pas_ONE) {
pas5=1;
val_pas5=100;
}
else if(err5 pas5=1;
val_pas5=100-(10*(err5-pas_ONE));
}
else pas5=0;
//dekat
if (err5<=pas_ONE) dkt5=0;
else if(err5 {
dkt5=1;
val_dkt5=(10*(err5-pas_ONE));
}
else
{
dkt5=1;
val_dkt5=100;
}

//error4
derr4=err4-err4_0;
//jauh sekali
if (err4<=_jauh_sekali_MAX)
{
jauh_pol4=1;
val_jauhpol4 = 100;
} else if (err4<_jauh_sekali_MIN)
{
jauh_pol4=1;
val_jauhpol4=100-(10*(err4-(_jauh_sekali_MAX)));
}
else jauh_pol4 = 0;
//jauh
if (err4<=_jauh_sekali_MAX) jauh4=0;
else if (err4<=_jauh_MAX) {
jauh4=1;
val_jauh4=10*(err4-(_jauh_sekali_MAX));
}
else if (err4<_jauh_MIN) {
jauh4=1;
val_jauh4=100-(10*(err4-(_jauh_MAX)));
}
else jauh4=0;
//pas
if(err4<=_jauh_MAX) pas4=0;
else if (err4<_pas_MAX) {
pas4=1;
val_pas4=10*(err4-(_jauh_MAX));
}
else if (err4<=_pas_ONE) {
pas4=1;
val_pas4=100;
}
else if(err4<_pas_MIN) {
pas4=1;
val_pas4=100-(10*(err4-_pas_ONE));
}
else pas4=0;
//dekat
if (err4<=_pas_ONE) dkt4=0;
else if(err4<_dekat_MIN)
{
dkt4=1;
val_dkt4=(10*(err4-_pas_ONE));
}
else
{
dkt4=1;
val_dkt4=100;
}

bki=0;
ski=0;
lurus=0;
ska=0;
bka=0;
val_bki=0;
val_ski=0;
val_lurus=0;
val_ska=0;
val_bka=0;

//rule
if (jauh_pol4!=0){
if (jauh_pol5!=0)
{ bka=1;
if (val_jauhpol4 else rs=val_jauhpol5;
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauhpol4 else rs=val_jauh5;
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauhpol4 else rs=val_pas5;
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ ska=1;
if (val_jauhpol4 else rs=val_dkt5;
if (rs>val_ska) val_ska=rs;
}
}
if (jauh4!=0){
if (jauh_pol5!=0)
{ bka=1;
if (val_jauh4 else rs=val_jauhpol5;
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauh4 else rs=val_jauh5;
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauh4 else rs=val_pas5;
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_jauh4 else rs=val_dkt5;
if (rs>val_lurus) val_lurus=rs;
}
}
if (pas4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_pas4 else rs=val_jauhpol5;
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_pas4 else rs=val_jauh5;
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ lurus=1;
if (val_pas4 else rs=val_pas5;
if (rs>val_lurus) val_lurus=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_pas4 else rs=val_dkt5;
if (rs>val_lurus) val_lurus=rs;
}
}
if (dkt4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_dkt4 else rs=val_jauhpol5;
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_dkt4 else rs=val_jauh5;
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ bki=1;
if (val_dkt4 else rs=val_pas5;
if (rs>val_bki) val_bki=rs;
}
if (dkt5!=0)
{ bki=1;
if (val_dkt4 else rs=val_dkt5;
if (rs>val_bki) val_bki=rs;
}
}
//defuzzyfikasi motor:
x_tot=0;
x1=speed/4;
x2=speed/2;
if (bki!=0)
{ x_ceq = (x2*(val_bki+100)*(-1))/100;
x_tot += (x_ceq*val_bki);
}
if (ski!=0)
{ x_tot += (-1) * x1 * val_ski;
}
if (ska!=0)
{ x_tot += x1 * val_ska;
}
if (bka!=0)
{
x_ceq = (x2*(val_bka+100))/100;
x_tot += (x_ceq * val_bka);
}


steer = x_tot / (val_bki + val_ski + val_lurus + val_ska + val_bka);
steer -= (derr4*deriv);

if (left==0)
{
mka=speed-steer;
mki=speed+steer;
}
else
{ mki=speed-steer;
mka=speed+steer;
}
/*
if (mka<0) mka=0;
if (mki<0) mki=0;
*/

if (mka if (mki

if (mka>max) mka=max;
if (mki>max) mki=max;

pwmKa_val=mka-2;
pwmKi_val=mki;

err4_0=err4;
err5_0=err5;

}

//mf kecepatan
#define mf_dekat0 30
#define mf_dekat1 40

//range 15
#define mf_agak_dekat_1 35
#define mf_agak_dekat0 50
#define mf_agak_dekat1 65

#define mf_sedang_1 50
#define mf_sedang0 65
#define mf_sedang1 80

#define mf_agak_jauh_1 70
#define mf_agak_jauh0 85
#define mf_agak_jauh1 100

#define mf_jauh0 90
#define mf_jauh1 100

//mf haluan
#define mf_min_besar0 -30
#define mf_min_besar1 -20

//range 10,14,10
#define mf_min_kecil_1 -27
#define mf_min_kecil0 -17
#define mf_min_kecil1 -7

#define mf_zero_1 -14
#define mf_zero0 0
#define mf_zero1 14

#define mf_plus_kecil_1 7
#define mf_plus_kecil0 17
#define mf_plus_kecil1 27

#define mf_plus_besar0 20
#define mf_plus_besar1 30

#define s_diam 0
#define s_maju_pelan 5
#define s_maju 18
#define s_maju_cepat 24
#define s_mundur 18
#define s_mundur_pelan 5
bit ccw_mki,ccw_mka;
char str1[7];
char str2[7];
//bit dekat, agak_dekat, sedang, agak_jauh, jauh;
//bit min_besar,min_kecil,zero,plus_kecil,plus_besar;
//nando's rule

void obstacle_avoid()
{
int kecepatan, haluan;
unsigned char dekat, agak_dekat, sedang, agak_jauh, jauh;
int val_dekat, val_agak_dekat,val_sedang, val_agak_jauh,
val_jauh=0;
unsigned char min_besar,min_kecil,zero,plus_kecil,plus_besar;
int val_min_besar,val_min_kecil,val_zero,val_plus_kecil,val_plus_besar=0;
int fuzz_ki_diam,fuzz_ki_maju_pelan,fuzz_ki_maju,fuzz_ki_maju_cepat=0;
int fuzz_ki_mundur,fuzz_ki_mundur_pelan=0;
int fuzz_ka_diam,fuzz_ka_maju_pelan,fuzz_ka_maju,fuzz_ka_maju_cepat=0;
int fuzz_ka_mundur,fuzz_ka_mundur_pelan=0;
int mki,mka;



kecepatan=((int)dataPing[5]+dataPing[0])+(((int)dataPing[2]+(int)dataPing[4])*2)+
(((int)dataPing[2]+(int)dataPing[4])*4);
haluan=((int)dataPing[0]+((int)dataPing[1]*2)+((int)dataPing[2]*4))-
((int)dataPing[5]+((int)dataPing[4]*2)+((int)dataPing[3]*4));
//lcd_gotoxy(0,0);
//itob(kecepatan,str1,10);
/*itoa(kecepatan,str1);
for (i=0;i lcd_putchar(i);*/
// lcd_gotoxy(8,0);
// itob(haluan,str2,10);


//fuzzifikasi kecepatan
//dekat
if (kecepatan <= mf_dekat0)
{
dekat = 1;
val_dekat = 100;
}
else if (kecepatan <= mf_dekat1)
{
dekat = 1;
val_dekat = 100-(10*(kecepatan-mf_dekat0));
}
else
{
dekat = 0;
val_dekat = 0;
}
//agak dekat
if (kecepatan <= mf_agak_dekat_1)
{
agak_dekat=0;
val_agak_dekat = 0;
}
else if (kecepatan <= mf_agak_dekat0)
{
agak_dekat=1;
val_agak_dekat = 15 - (mf_agak_dekat0-kecepatan);
val_agak_dekat = (val_agak_dekat/15) * 100;
}
else if (kecepatan < mf_agak_dekat1)
{
agak_dekat=1;
val_agak_dekat=mf_agak_dekat1-kecepatan;
val_agak_dekat = (val_agak_dekat/15)*100;
}
else
{
agak_dekat=0;
val_agak_dekat = 0;
}
//sedang
if (kecepatan <= mf_sedang_1)
{
sedang=0;
val_sedang = 0;
}
else if (kecepatan <= mf_sedang0)
{
sedang=1;
val_sedang = 15 - (mf_sedang0-kecepatan);
val_sedang = (val_sedang / 15) * 100;
}
else if (kecepatan < mf_sedang1)
{
sedang=1;
val_sedang=mf_sedang1-kecepatan;
val_sedang = (val_sedang/15)*100;
}
else
{
sedang=0;
val_sedang = 0;
}
//agak jauh
if (kecepatan <= mf_agak_jauh_1)
{
agak_jauh=0;
val_agak_jauh = 0;
}
else if (kecepatan <= mf_agak_jauh0)
{
agak_jauh=1;
val_agak_jauh = 15 - (mf_agak_jauh0-kecepatan);
val_agak_jauh = (val_agak_jauh / 15) * 100;
}
else if (kecepatan < mf_agak_jauh1)
{
agak_jauh=1; val_agak_jauh=mf_agak_jauh1-kecepatan;
val_agak_jauh = (val_agak_jauh/15)*100;
}
else
{
agak_jauh=0;
val_agak_jauh = 0;
}
//jauh
if (kecepatan <= mf_jauh0)
{
jauh=0;val_jauh=0;
}
else if (kecepatan < mf_jauh1)
{
jauh=1;val_jauh=100-(10*(mf_jauh1-kecepatan));
}
else
{
jauh=1;val_jauh=100;
}

//fuzzifikasi haluan
//min_besar
if (haluan <= mf_min_besar0)
{
min_besar=1;
val_min_besar=100;
}
else if (haluan < mf_min_besar1)
{
min_besar=1;
val_min_besar=100-(10*(haluan-(mf_min_besar0)));
}
else
{
min_besar=0;
val_min_besar=0;
}
//min_kecil
if(haluan <= mf_min_kecil_1)
{
min_kecil=0;
val_min_kecil=0;
}
else if (haluan <= mf_min_kecil0)
{
min_kecil=1;
val_min_kecil=100-(10*(mf_min_kecil0-haluan));
}
else if (haluan < mf_min_kecil1)
{
min_kecil=1;
val_min_kecil=10*(mf_min_kecil1-haluan);
}
else
{
min_kecil=0;
val_min_kecil=0;
}
//zero
if(haluan <= mf_zero_1)
{
zero=0;
val_zero=0;
}
else if (haluan <= mf_zero0)
{
zero=1;
val_zero=((14-(mf_zero0-haluan))/14)*100;
}
else if (haluan < mf_zero1)
{
zero=1;
val_zero=((mf_zero1-haluan)/14)*100;
}
else
{
zero=0;
val_zero=0;
}
//plus kecil
if(haluan <= mf_plus_kecil_1)
{
plus_kecil=0;
val_plus_kecil=0;
}
else if (haluan <= mf_plus_kecil0)
{
plus_kecil=1;
val_plus_kecil=100-(10*(mf_plus_kecil0-haluan));
}
else if (haluan < mf_plus_kecil1)
{
plus_kecil=1;
val_plus_kecil=10*(mf_plus_kecil1-haluan);
}
else
{
plus_kecil=0;
val_plus_kecil=0;
}
//plus besar
if (haluan <= mf_plus_besar0)
{
plus_besar=0;val_plus_besar=0;
}
else if (haluan < mf_plus_besar1)
{
plus_besar=1;val_plus_besar=100-(10*(mf_plus_besar1-(int)haluan));
}
else
{
plus_besar=1;val_plus_besar=100;
}

//rule motor
if(dekat!=0)
{
if(min_besar!=0)
{
if (val_dekat > val_min_besar)
{
fuzz_ki_diam = val_min_besar;
fuzz_ka_mundur = val_min_besar;
}
else
{
fuzz_ki_diam = val_dekat;
fuzz_ka_mundur = val_dekat;
}
}
if(min_kecil!=0)
{
if (val_dekat > val_min_kecil)
{
if (val_min_kecil > fuzz_ki_diam) fuzz_ki_diam = val_min_kecil;
if (val_min_kecil > fuzz_ka_mundur) fuzz_ka_mundur = val_min_kecil;
}
else
{
if (val_dekat > fuzz_ki_diam) fuzz_ki_diam = val_dekat;
if (val_dekat > fuzz_ka_mundur) fuzz_ka_mundur = val_dekat;
}

}
if(zero!=0)
{
if (val_dekat > val_zero)
{
if (val_zero > fuzz_ki_diam) fuzz_ki_diam = val_zero;
fuzz_ka_diam = val_zero;
}
else
{
if (val_dekat > fuzz_ki_diam) fuzz_ki_diam = val_dekat;
fuzz_ka_diam = val_dekat;
}
}
if(plus_kecil!=0)
{
if (val_dekat > val_plus_kecil)
{
fuzz_ki_mundur = val_plus_kecil;
if (val_plus_kecil > fuzz_ka_diam) fuzz_ka_diam = val_plus_kecil;
}
else
{
fuzz_ki_mundur = val_dekat;
if (val_dekat > fuzz_ka_diam) fuzz_ka_diam = val_dekat;
}
}
if(plus_besar!=0)
{
if (val_dekat > val_plus_besar)
{
if (val_plus_besar > fuzz_ki_mundur) fuzz_ki_mundur = val_plus_besar;
if (val_plus_besar > fuzz_ka_diam) fuzz_ka_diam = val_plus_besar;
}
else
{
if (val_dekat > fuzz_ki_mundur) fuzz_ki_mundur = val_dekat;
if (val_dekat > fuzz_ka_diam) fuzz_ka_diam = val_dekat;
}
}
}
if(agak_dekat!=0)
{
if (min_besar!=0)
{
if (val_agak_dekat > val_min_besar)
{
fuzz_ki_maju_pelan = val_min_besar;
fuzz_ka_mundur_pelan = val_min_besar;
}
else
{
fuzz_ki_maju_pelan = val_agak_dekat;
fuzz_ka_mundur_pelan = val_agak_dekat;
}
}
if (min_kecil!=0)
{
if (val_agak_dekat > val_min_kecil)
{
if (val_min_kecil > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_min_kecil;
if (val_min_kecil > fuzz_ka_mundur_pelan) fuzz_ka_mundur_pelan = val_min_kecil;
}
else
{
if (val_agak_dekat > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_agak_dekat;
if (val_agak_dekat > fuzz_ka_mundur_pelan) fuzz_ka_mundur_pelan = val_agak_dekat;
}
}
if (zero!=0)
{
if (val_agak_dekat > val_zero)
{
if (val_zero > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_zero;
fuzz_ka_maju_pelan = val_zero;
}
else
{
if (val_agak_dekat > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_agak_dekat;
fuzz_ka_maju_pelan = val_agak_dekat;
}
}
if (plus_kecil!=0)
{
if (val_agak_dekat > val_plus_kecil)
{
fuzz_ki_mundur_pelan = val_plus_kecil;
if (val_plus_kecil > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_plus_kecil;
}
else
{
fuzz_ki_mundur_pelan = val_agak_dekat;
if (val_agak_dekat > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_agak_dekat;
}

}
if (plus_besar!=0)
{
if (val_agak_dekat > val_plus_besar)
{
if (val_plus_besar > fuzz_ki_mundur_pelan) fuzz_ki_mundur_pelan = val_plus_besar;
if (val_plus_besar > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_plus_besar;
}
else
{
if (val_agak_dekat > fuzz_ki_mundur_pelan) fuzz_ki_mundur_pelan = val_agak_dekat;
if (val_agak_dekat > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_agak_dekat;
}
}
}
if(sedang!=0)
{
if (min_besar!=0)
{
if (val_sedang > val_min_besar)
{
fuzz_ki_maju = val_min_besar;
if (val_min_besar > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_min_besar;
}
else
{
fuzz_ki_maju = val_sedang;
if (val_sedang > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_sedang;
}
}
if (min_kecil!=0)
{
if (val_sedang > val_min_kecil)
{
if (val_min_kecil > fuzz_ki_maju) fuzz_ki_maju = val_min_kecil;
if (val_min_kecil > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_min_kecil;
}
else
{
if (val_sedang > fuzz_ki_maju) fuzz_ki_maju = val_sedang;
if (val_sedang > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_sedang;
}
}
if(zero!=0)
{
if (val_sedang > val_zero)
{
if (val_zero > fuzz_ki_maju) fuzz_ki_maju = val_zero;
fuzz_ka_maju = val_zero;
}
else
{
if (val_sedang > fuzz_ki_maju) fuzz_ki_maju = val_sedang;
fuzz_ka_maju = val_sedang;
}
}
if(plus_kecil!=0)
{
if (val_sedang > val_plus_kecil)
{
if (val_plus_kecil > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_plus_kecil;
if (val_plus_kecil > fuzz_ka_maju) fuzz_ka_maju = val_plus_kecil;
}
else
{
if (val_sedang > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_sedang;
if (val_sedang > fuzz_ka_maju) fuzz_ka_maju = val_sedang;
}
}
if(plus_besar!=0)
{
if (val_sedang > val_plus_besar)
{
if (val_plus_besar > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_plus_besar;
if (val_plus_besar > fuzz_ka_maju) fuzz_ka_maju = val_plus_besar;
}
else
{
if (val_sedang > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_sedang;
if (val_sedang > fuzz_ka_maju) fuzz_ka_maju = val_sedang;
}
}
}
if(agak_jauh!=0)
{
if(min_besar!=0)
{
if (val_agak_jauh > val_min_besar)
{
fuzz_ki_maju_cepat = val_min_besar;
if (val_min_besar > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_min_besar;
}
else
{
fuzz_ki_maju_cepat = val_agak_jauh;
if (val_agak_jauh > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_agak_jauh;
}
}
if(min_kecil!=0)
{
if (val_agak_jauh > val_min_kecil)
{
if (val_min_kecil > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_min_kecil;
if (val_min_kecil > fuzz_ka_maju) fuzz_ka_maju = val_min_kecil;
}
else
{
if (val_agak_jauh > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_agak_jauh;
if (val_agak_jauh > fuzz_ka_maju) fuzz_ka_maju = val_agak_jauh;
}
}
if(zero!=0)
{
if (val_agak_jauh > val_zero)
{
if (val_zero > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_zero;
fuzz_ka_maju_cepat = val_zero;
}
else
{
if (val_agak_jauh > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_agak_jauh;
fuzz_ka_maju_cepat = val_agak_jauh;
}
}
if(plus_kecil!=0)
{
if (val_agak_jauh > val_plus_kecil)
{
if (val_plus_kecil > fuzz_ki_maju) fuzz_ki_maju = val_plus_kecil;
if (val_plus_kecil > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_plus_kecil;
}
else
{
if (val_agak_jauh > fuzz_ki_maju) fuzz_ki_maju = val_agak_jauh;
if (val_agak_jauh > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_agak_jauh;
}
}
if(plus_besar!=0)
{
if (val_agak_jauh > val_plus_besar)
{
if (val_plus_besar > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_plus_besar;
if (val_plus_besar > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_plus_besar;
}
else
{
if (val_agak_jauh > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_agak_jauh;
if (val_agak_jauh > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_agak_jauh;
}
}
}
if(jauh!=0)
{
if (min_besar!=0)
{
if (val_jauh > val_min_besar)
{
if (val_min_besar > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_min_besar;
if (val_min_besar > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_min_besar;
}
else
{
if (val_jauh > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_jauh;
if (val_jauh > fuzz_ka_maju_pelan) fuzz_ka_maju_pelan = val_jauh;
}
}
if (min_kecil!=0)
{
if (val_jauh > val_min_kecil)
{
if (val_min_kecil > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_min_kecil;
if (val_min_kecil > fuzz_ka_maju) fuzz_ka_maju = val_min_kecil;
}
else
{
if (val_jauh > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_jauh;
if (val_jauh > fuzz_ka_maju) fuzz_ka_maju = val_jauh;
}
}
if (zero!=0)
{
if (val_jauh > val_zero)
{
if (val_zero > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_zero;
if (val_zero > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_zero;
}
else
{
if (val_jauh > fuzz_ki_maju_cepat) fuzz_ki_maju_cepat = val_jauh;
if (val_jauh > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_jauh;
}
}
if (plus_kecil!=0)
{
if (val_jauh > val_plus_kecil)
{
if (val_plus_kecil > fuzz_ki_maju) fuzz_ki_maju = val_plus_kecil;
if (val_plus_kecil > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_plus_kecil;
}
else
{
if (val_jauh > fuzz_ki_maju) fuzz_ki_maju = val_jauh;
if (val_jauh > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_jauh;
}
}
if (plus_besar!=0)
{
if (val_jauh > val_plus_besar)
{
if (val_plus_besar > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_plus_besar;
if (val_plus_besar > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_plus_besar;
}
else
{
if (val_jauh > fuzz_ki_maju_pelan) fuzz_ki_maju_pelan = val_jauh;
if (val_jauh > fuzz_ka_maju_cepat) fuzz_ka_maju_cepat = val_jauh;
}
}
}

//deffuzifikasi motor kiri
if ((fuzz_ki_mundur!=0)||(fuzz_ki_mundur_pelan!=0)||(fuzz_ki_diam!=0))
{
mki = ((s_diam*fuzz_ki_diam)+(s_mundur_pelan*fuzz_ki_mundur_pelan)+(s_mundur*fuzz_ki_mundur))/(fuzz_ki_diam+fuzz_ki_mundur_pelan+fuzz_ki_mundur);
mki = mki;
ccw_mki=1;
}
else if ((fuzz_ki_maju_pelan!=0)||(fuzz_ki_maju!=0)||(fuzz_ki_maju_cepat!=0))
{
mki = ((s_maju_pelan*fuzz_ki_maju_pelan)+(s_maju*fuzz_ki_maju)+(s_maju_cepat*fuzz_ki_maju_cepat))/(fuzz_ki_maju_pelan+fuzz_ki_maju+fuzz_ki_maju_cepat);
mki = mki;
ccw_mki=0;
}

//deffuzifikasi motor kanan
if ((fuzz_ka_mundur!=0)||(fuzz_ka_mundur_pelan!=0)||(fuzz_ka_diam!=0))
{
mka = ((s_diam*fuzz_ka_diam)+(s_mundur_pelan*fuzz_ka_mundur_pelan)+(s_mundur*fuzz_ka_mundur))/(fuzz_ka_diam+fuzz_ka_mundur_pelan+fuzz_ka_mundur);
mka = mka;
ccw_mka=1;
}
else if ((fuzz_ka_maju_pelan!=0)||(fuzz_ka_maju!=0)||(fuzz_ka_maju_cepat!=0))
{
mka = ((s_maju_pelan*fuzz_ka_maju_pelan)+(s_maju*fuzz_ka_maju)+(s_maju_cepat*fuzz_ka_maju_cepat))/(fuzz_ka_maju_pelan+fuzz_ka_maju+fuzz_ka_maju_cepat);
mka = mka;
ccw_mka=0;
}
/* lcd_gotoxy(0,1);

// tampil(mki);
// lcd_putchar(':');
if (ccw_mki==1) lcd_putchar('0');
else lcd_putchar('1');

lcd_putchar(spasi);
lcd_putchar(spasi);

tampil(mka);
lcd_putchar(':');
if (ccw_mka==1) lcd_putchar('0');
else lcd_putchar('1'); */
}



void putar_servo (unsigned char input)
{
unsigned char i,j;
//Kirim pulsa sebanyak nilai k
for(i=1; i<=75; i++)
{
servo=1;
for (j=1; j<=input; j++)
delay_us(10);
servo=0;
delay_ms(20);
}
}


void matikan_lilin (unsigned char times)
{

unsigned char j;
#asm("cli");
kipas=0;
for (j=0; j < times; j++) {
putar_servo(70); //tengah
putar_servo(50); //kiri
putar_servo(70); //tengah
putar_servo(90); //kanan
putar_servo(70); //tengah
}
kipas=1;
#asm("sei");
}

void main(void)
{
// Declare your local variables here
//unsigned char dataPing_[6];

unsigned char count_hal_dpn;
char strRotary[4];


//sensor
//kompas, lcd
PORTA=0xff; //p
DDRA=0x00;

//b0,b1 : sw (in) p
//b2 : rotary (in) p
//b3 : servo (out) x
//b4 : kipas (out) 1
PORTB=0x17;
DDRB=0x18;

//motor
//6 uv, p
PORTC=0x40;
DDRC=0xbf;

//PD0,1 : serial, p
//pD2,3 : bumper, p
//PD4,5 : IR, ts
//PD6 : Q UV, ts
//PD7 : Q bar UV, p
PORTD=0x8f;
DDRD=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x01;
TCNT0=0xC0;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer 1 Stopped
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: 31.250 kHz
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x04;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: On, Falling Edge
// INT1: On, Falling Edge
// INT2: On
// INT2 Mode: Falling Edge
GICR|=0xe0;
//MCUCR=0x00;
MCUCR=0x0A;
MCUCSR=0x00;
//GIFR=0x20;
GIFR=0xe0;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x01;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: Off
// USART Mode: Asynchronous
// USART Baud rate: 9600 (Double Speed Mode)
UCSRA=0x02;
UCSRB=0x90;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x33;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
// Analog Comparator Output: Off
ACSR=0x80;
SFIOR=0x00;

//lcd_init(16);

// Global enable interrupts
#asm("sei")

rem();
disable_bumper();
set1 = 38;
set2 = 52;
rotary = 0;
maju();
gas();
rst_rotary();
while (1)
{
while (sw1==1);

//from Home to room 1, no stair
while(rotary < 40 || IR_depan!=1)
{
rule_cepat(120+(40-rotary),180-rotary,10,1,60);
}
rem();
mundur();
pwmKi_val=70;
pwmKa_val=70;
gas();
delay_ms(50);
rem();
putar(p_utara,70,0);
maju();
gas();
set1=45;
set2=63;
rst_rotary();
while(rotary < 65 || IR_depan!=1)
{
rule_cepat(70+rotary,100+rotary,10,1,40);
}
rem();
mundur();
pwmKi_val=70;
pwmKa_val=70;
gas();
delay_ms(50);
rem();
putar(p_timur,70,0);
maju();
gas();
rst_rotary();
while(rotary < 30) {
rule_cepat(68,98,10,1,40);
}
rem();
putar(selatan2,65,0);
maju();
pwmKi_val=72;
pwmKa_val=70;
gas();
rst_rotary();
while (rotary < 15);
rem();
delay_ms(1500);
//cek lilin room1

mundur();
pwmKi_val=85;
pwmKa_val=80;
delay_ms(20);
gas();
while(IR_blkg!=1);
rem();

putar(timur+18,68,1);
set1=45;
set2=65;
maju();
gas();
rst_rotary();
while (!(kompas > 142 && kompas < 159)){
rule_cepat(70,90,10,1,30);
}
rem();
delay_ms(1500);
putar(timur+10,68,0);
maju();
pwmKi_val=82;
pwmKa_val=80;
gas();
while (IR_depan!=1);
rem();
putar(p_utara,70,1);
maju();
gas();
while (IR_depan!=1) {
rule_cepat(80,100,10,0,65);
}
rem();
loop3:
goto loop3;

//from room 1 goto room 2
mundur();
pwmKi_val = 100;
pwmKa_val = 96;
gas();
while (IR_blkg!=1);
rem();
maju();
pwmKi_val=80;
pwmKa_val=80;
gas();
delay_ms(50);
rem();
putar(timur+14,100,1);
rem();
maju();
pwmKi_val=112;
pwmKa_val=110;
gas();
rst_rotary();
while (rotary < 15);
rem();
putar(utara+20,100,1);
maju();
pwmKi_val=112;
pwmKa_val=100;
gas();
rst_rotary();
while (rotary < 15);
rem();
putar(p_barat+20,100,1);
rem();
loop:
goto loop;
};

}