Buat Blajar

1163644
/*******************************************************************************
*************** //
Project : G - Robo Line Follower Robot
//
Version : 1.0.0
//
Date
: 1/10/2010
//
Author : G - Robo Team
//
Company : Divisi Workshop Himatektro ITS
//
//
//
Chip type
: ATmega16
//
Program type
: Application
//
Clock frequency
: 16.00 MHz
//
Memory model
: Small
//
External RAM size
: 0
//
Data Stack size
: 256
//
********************************************************************************
**************/ //
#include
#include
#include
#include
<mega16.h>
<stdio.h>
<delay.h>
<lcd.h>
#asm
.equ __lcd_port=0x15 ;PORTC
#endasm
#define
#define
#define
#define
#define
#define
#define
lpwm OCR1B // PWM 10-Bit Maka Nilai Maksimal 1024

rpwm OCR1A // PWM 10-Bit Maka Nilai MAksimal 1024
atas PINB.6
bawah PINB.7
batal PIND.0
ok PIND.1
ki_a PINB.0
#define
#define
#define
#define
#define
ki_b PINB.1
teng_d PINB.2
teng_b PINB.4
ka_b PINB.3
ka_a PINB.5
//---------------Dekalarsi Variabel Global------------------------------------------------------//

void kanan_maju();
//
void kiri_maju();
//
void kanan_mundur();
//
void kiri_mundur();
//
void lcd_sukses();
//void kanan_stop();
//void kiri_stop();
void maping_darurat();
//void deteksi_perempatan();
void kanan_r();
void kiri_r();
//void stop();
void perempatan_kiri();
void perempatan_kanan();
//void pertigaan();
//void cek_b2();
//void cek_b3();
//void cek_b4();
//void cek_b5();
//void cek_b6();
//void cek_b7();
//void cek_b8();
//void cek_b9();
//void cek_b10();
//void cek_b11();
bit b0, b1, b2, b3, b4, b5, b6, b7,b8,b9,b10,b11,b12,b13,x,g;
unsigned char yb3=0;//,yb31=0,yb4=0,yb5=0,yb6=0,yb7=0,yb8=0,yb9=0,yb10=0,yb11=0;
/
/
unsigned char buff[30];
//
unsigned int adc_auto_scan();
unsigned int adc_auto_scan2();
//
unsigned char read_adc();
//
unsigned char menu, kp, ki,setting_sensor = 0, bitx, count = 0,y;
//
unsigned int kd, sampling_rate = 0, starting_speed, maximum_speed, minimum_speed
, extra_speed, proportional, debug = 0, bit_sensor,i = 0;
unsigned int ki_save,kd_save,kp_save,a;
//unsigned char sense_kiri=0, sense_kanan=0, mb2,mb3,mb4,mb5,mb6,mb7,mb8,mb9,mb1
0,mb11;
int integratif, derivatif, error_acc, error_rate, control_signal, pwm, pv, error
, last_error;
char sp = 0;
eeprom unsigned char kp_eeprom = 106;
//
eeprom unsigned char ki_eeprom = 10;

//
eeprom unsigned int kd_eeprom = 1000;
//
eeprom unsigned int sampling_rate_eeprom = 1000;
//
eeprom unsigned int starting_speed_eeprom = 1000;
//
eeprom unsigned int maximum_speed_eeprom = 1000;
eeprom unsigned int minimum_speed_eeprom = 9;
//
eeprom unsigned int extra_speed_eeprom = 999;
eeprom unsigned char setting_sensor_eeprom = 1;
//
//
//----------------------------------------------------------------------------------------------//
void main(void)
{
//-------------------Deklarasi Initial----------------------------------------------------------//
//
= 0x00; /* Fungsi untuk : Port A Sebagai Input ADC */
//
PORTA = 0x00; /* Fungsi untuk : Tristate */
//
DDRA
//
= 0x00; /* Fungsi untuk : Port B Sebagai Input Saklar Ground */
//
PORTB = 0xFF; /* Fungsi untuk : Pulp UP ON */
//
DDRB
//
= 0x00; /* Fungsi untuk : Port C Sebagai Port Interaksi dengan LCD */
//
PORTC = 0x00; /* Fungsi untuk : Tristate */
//
DDRC
//
= 0xFC; /* Fungsi untuk : Port D Sebagai Port Interaksi dengan Driver */
//
PORTD = 0x03; /* Fungsi untuk : Output Low */
//
DDRD
//
//
TCCR1A = 0xA3; /* Fungsi untuk : 1. Bit COM1A[7:6]=10 (Mode Non-Inverting PWM)
//
2. Bit COM1B[5:4]=10 (Mode Non-Inverting PWM)
//
3. Bit WGM11-WGM10[1:0]=11 (Mode Phase Correct
PWM 10-Bit)*/
//
//
TCCR1B = 0x02; /* Fungsi untuk : 1. Bit WGM13-WGM12[4:3]=00 (Mode Phase Correct
PWM 10-Bit)
//
2. Bit CS12-CS10[2:1:0]=010 (Prescalers 1024)*/
//
//
ADMUX = 0x27; /* Fungsi untuk : 1. Bit REFS[7:6]=00 (AREF used for ADC vol ref)
//
2. Bit ADLAR[5]=1 (Left Ajusted)
//
3. Bit MUX[4:0]=00111 (Channel 7 dipilih)*/
//
//
ADCSRA = 0x87; /* Fungsi untuk : 1.
//
2.
//
3.
//
4.
//
5.
//
6.
//
Bit ADEN[7]=1 (Enabling ADC)

Bit ADSC[6]=0 (Conversion Not Started Yet)
Bit ADATE[5]=0 (Wes, poko'e OFF)
Bit ADIF[4]=XX (conversion interupt Flag)
Bit ADIE[3]=0 (ADC Interupt OFF)
Bit ADPS[2:0]=111 (Division Factor 128)*/
//
lcd_init(16);
//
//
//----------------------------------------------------------------------------------------------//
//--------------Program Menu Line Follower------------------------------------------------------//
/*
lcd_clear();
lcd_gotoxy(0,0);
lcd_putsf("==G - ROBO V.5==");
delay_ms(1500);
lcd_gotoxy(0,1);
lcd_putsf("PRESENT.....");
delay_ms(1000);
*/
menu_utama:
menu = 0;
lcd_clear();
while (1)
{
lcd_gotoxy(0,0);
lcd_putsf("...BISMILLAH....");
if(atas==0)
{
delay_ms(250);
menu++;
if(menu>6)
{
menu=0;
};
};
if(bawah==0)
{
delay_ms(250);
menu--;
if(menu>6)
{
menu=6;
};
};
if(menu==0)
{
lcd_gotoxy(0,1);
lcd_putsf("SIAP MAJU KANAN?");
};
if(menu==1)
{
lcd_gotoxy(0,1);
lcd_putsf("SIAP MAJU KIRI?");
};
if(menu==2)
{
lcd_gotoxy(0,1);
lcd_putsf("Maximum Speed?");
};
if(menu==3)
{
lcd_gotoxy(0,1);
lcd_putsf("Extra Speed?");
};
if(menu==4)
{
lcd_gotoxy(0,1);
lcd_putsf("SETTING KP ?
};
if(menu==5)
{
lcd_gotoxy(0,1);
lcd_putsf("SETTING KI ?
};
if(menu==6)
{
lcd_gotoxy(0,1);
lcd_putsf("SETTING KD ?
};
if ((ok==0)&(menu==0))
");
");
");
{
delay_ms(250);
menu=0;
goto setting_kanan;
};
if ((ok==0)&(menu==1))
{
delay_ms(250);
menu=0;
goto setting_kiri;
};
if ((ok==0)&(menu==2))
{
delay_ms(250);
menu=0;
goto setting_maximum_speed;
};
if ((ok==0)&(menu==3))
{
delay_ms(250);
menu=0;
goto setting_extra_speed;
};
if((menu==4) && (ok==0))
{
delay_ms(250);
goto setting_kp;
};
if((menu==5) && (ok==0))
{
delay_ms(250);
goto setting_ki;
};
if((menu==6) && (ok==0))
{
delay_ms(250);
goto setting_kd;
};
if(batal==0)
{
delay_ms(250);
lcd_clear();
lcd_gotoxy(0,0);
lcd_putsf("IKI MENU UTAMA ");
lcd_gotoxy(0,1);
lcd_putsf("OJO GUYON!!!!! ");
delay_ms(1500);
lcd_clear();
};
};
setting_kp:
lcd_clear();
kp = kp_eeprom;
while (1)
{
if(atas==0)
{
delay_ms(250);
lcd_clear();
kp = kp + 2 ;
if(kp > 250)
kp = 0;
};
if(bawah==0)
{
delay_ms(250);
lcd_clear();
kp = kp - 2;
if(kp > 250)
kp = 250;
};
lcd_gotoxy(0,0);
sprintf(buff,"KP = %d",kp);
lcd_puts(buff);
if(ok==0)
{
delay_ms(250);
kp_eeprom = kp;
lcd_sukses();
lcd_clear();
};
if(batal==0)
{
delay_ms(250);
goto menu_utama;
};
};
setting_ki:
lcd_clear();
ki = ki_eeprom;
while (1)
{
if(atas==0)
{
delay_ms(250);
ki++;
lcd_clear();
};
if(bawah==0)
{
delay_ms(250);
ki--;
lcd_clear();
};
lcd_gotoxy(0,0);
sprintf(buff,"KI = %d",ki);
lcd_puts(buff);
if(ok==0)
{
delay_ms(250);
ki_eeprom = ki;
lcd_sukses();
lcd_clear();
};
if(batal==0)
{
delay_ms(250);
goto menu_utama;
};
};
setting_kd:
lcd_clear();
kd = kd_eeprom;
while (1)
{
if(atas==0)
{
delay_ms(250);
kd = kd + 100;
if(kd > 10000)
kd = 0;
lcd_clear();
};
if(bawah==0)
{
delay_ms(250);
kd = kd - 100;
if(kd > 10000)
kd = 10000;
lcd_clear();
};
lcd_gotoxy(0,0);
sprintf(buff,"KD = %d",kd);
lcd_puts(buff);
if(ok==0)
{
delay_ms(250);
kd_eeprom = kd;
lcd_sukses();
lcd_clear();
};
if(batal==0)
{
delay_ms(250);
goto menu_utama;
};
};
setting_maximum_speed:
lcd_clear();
maximum_speed = maximum_speed_eeprom;
while (1)
{
if(atas==0)
{
delay_ms(250);
maximum_speed = maximum_speed + 50;
if(maximum_speed > 1000)
maximum_speed = 200;
lcd_clear();
};
if(bawah==0)
{
delay_ms(250);
maximum_speed = maximum_speed - 50;
if(maximum_speed < 200)
maximum_speed = 1000;
lcd_clear();
};
lcd_gotoxy(0,0);
sprintf(buff,"MAX SPEED = %d",maximum_speed);
lcd_puts(buff);
if(ok==0)
{
delay_ms(250);
maximum_speed_eeprom = maximum_speed;
lcd_sukses();
lcd_clear();
};
if(batal==0)
{
delay_ms(250);
goto menu_utama;
};
};
setting_extra_speed:
lcd_clear();
extra_speed = extra_speed_eeprom;
while (1)
{
if(atas==0)
{
delay_ms(250);
extra_speed = extra_speed + 50;
lcd_clear();
if(extra_speed > 1000)
extra_speed = 600;
};
if(bawah==0)
{
delay_ms(250);
extra_speed = extra_speed - 50;
lcd_clear();
if(extra_speed < 600)
extra_speed = 1000;
};
lcd_gotoxy(0,0);
sprintf(buff,"EX SPEED = %d",extra_speed);
lcd_puts(buff);
if(ok==0)
{
delay_ms(250);
extra_speed_eeprom = extra_speed;
lcd_sukses();
lcd_clear();
};
if(batal==0)
{
delay_ms(250);
goto menu_utama;
};
};
setting_kanan:
lcd_clear();
while (1)
{
lcd_gotoxy(0,0);
lcd_putsf("CEK P KANAN");
if(atas==0)
{
delay_ms(250);
menu++;
if(menu>6) menu=0;
};
if(bawah==0)
{
delay_ms(250);
menu--;
if(menu>6) menu=6;
};
if(menu==0)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 1");
};
if(menu==1)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 2");
};
if(menu==2)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 3");
};
if(menu==3)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 4");
};
if(menu==4)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 5");
};
if(menu==5)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 6");
};
if(menu==6)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 0");
};
if((menu==0) && (ok==0))
{
delay_ms(250);
count=0;
goto siap_maju_kanan;
};
if((menu==1) && (ok==0))
{
delay_ms(250);
count=1;
y=1;
};
if((menu==2) && (ok==0))
{
delay_ms(250);
count=3;
};
if((menu==3) && (ok==0))
{
delay_ms(250);
count=4;
};
if((menu==4) && (ok==0))
{
delay_ms(250);
count=8;
};
if((menu==5) && (ok==0))
{
delay_ms(250);
count=10;
};
if((menu==6) && (ok==0))
{
delay_ms(250);
count=0;
kp = kp_eeprom ;
//
ki = ki_eeprom ;
//
kd = kd_eeprom ;
last_error = 0 ;
error_acc = 0 ;
//
sampling_rate = 1000 / sampling_rate_eeprom;
//
starting_speed = starting_speed_eeprom;
//
minimum_speed = minimum_speed_eeprom;
//
setting_sensor = setting_sensor_eeprom;
for(i=0;i<100;i++)
{
maping_darurat();
}
};
if(batal==0)
{
delay_ms(250);
goto menu_utama;
};
};
setting_kiri:
lcd_clear();
while (1)
{
lcd_gotoxy(0,0);
lcd_putsf("CEK P KIRI");
if(atas==0)
{
delay_ms(250);
menu++;
if(menu>6) menu=0;
};
if(bawah==0)
{
delay_ms(250);
menu--;
if(menu>6) menu=6;
};
if(menu==0)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 1");
};
if(menu==1)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 2");
};
if(menu==2)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 3");
};
if(menu==3)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 4");
};
if(menu==4)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 5");
};
if(menu==5)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 6");
};
if(menu==6)
{
lcd_gotoxy(0,1);
lcd_putsf("CP 0");
};
if((menu==0) && (ok==0))
{
delay_ms(250);
count=0;
goto siap_maju_kiri;
};
if((menu==1) && (ok==0))
{
delay_ms(250);
count=0;
y=1;
};
if((menu==2) && (ok==0))
{
delay_ms(250);
count=4;
};
if((menu==3) && (ok==0))
{
delay_ms(250);
count=6;
};
if((menu==4) && (ok==0))
{
delay_ms(250);
count=8;
};
if((menu==5) && (ok==0))
{
delay_ms(250);
count=10;
};
if((menu==6) && (ok==0))
{
delay_ms(250);
count=0;
kp = kp_eeprom ;
//
ki = ki_eeprom ;
//
kd = kd_eeprom ;
last_error = 0 ;
error_acc = 0 ;
//
//
//
//
for(i=0;i<100;i++)
{
maping_darurat();
}
};
if(batal==0)
{
delay_ms(250);
goto menu_utama;
};
};
//----------------------------------------------------------------------------------------------//
//--------------Program Inti Line Follower------------------------------------------------------//

siap_maju_kiri:
lcd_clear();
kp = kp_eeprom ;
//
ki = ki_eeprom ;
//
kd = kd_eeprom ;
last_error = 0 ;
error_acc = 0 ;
//
//
//
//
count=0;
while (1)
{
if(ka_a==1 && ki_a==1)
{
perempatan_kanan();
}
if (ka_a==1)
{
bitx = 1;
}
if (ki_a==1)
{
bitx = 2;
}
bit_sensor = adc_auto_scan();
switch(bit_sensor)
{
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
0b0000000000001000
0b0000000000011000
0b0000000000111000
0b0000000001111000
0b0000000000010000
0b0000000000110000
0b0000000001110000
0b0000000011110000
0b0000000000100000
0b0000000001100000
0b0000000111110000
0b0000000011100000
0b0000000001000000
0b0000000111100000
0b0000000011000000
0b0000001111100000
0b0000000010000000
0b0000000111000000
0b0000001111000000
0b0000000110000000
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
-20 ; x = 1;break;
-17 ; x = 1;break;
-15 ; x = 1;break;
-13 ; x = 1;break;
-15 ; x = 1;break;
-13 ; x = 1;break;
-9 ; x = 1;break;
-5 ; x = 1;break;
-9 ; x = 1;break;
-5 ; x = 1;break;
-3 ; x = 1;break;
-3 ; x = 1;break;
-3 ; x = 1;break;
0 ; x = 1;break;
0 ; x = 1;break;
3 ; x = 1;break;
3 ; x = 1;break;
3 ; x = 1;break;
5 ; x = 1;break;
5 ; x = 1;break;
case
case
case
case
case
case
case
case
0b0000000100000000
0b0000001110000000
0b0000011110000000
0b0000001100000000
0b0000001000000000
0b0000011100000000
0b0000011000000000
0b0000010000000000
:
:
:
:
:
:
:
:
pv
pv
pv
pv
pv
pv
pv
pv
=
=
=
=
=
=
=
=
9 ; x = 1;break;
9 ; x = 1;break;
13 ; x = 1;break;
13 ; x = 1;break;
15 ; x = 1;break;
15 ; x = 1;break;
17 ; x = 1;break;
20 ; x = 1;break;
case 0b0000001100110000 : pv = 0 ; perempatan_kanan(); break;

case
case
case
case
0b0000000011011000
0b0000000011001000
0b0000000010011000
0b0000000010001000
:
:
:
:
pv
pv
pv
pv
=
=
=
=
0
0
0
0
;
;
;
;
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
break;
break;
break;
break;

case
case
case
case
0b0000011011000000
0b0000010011000000
0b0000011010000000
0b0000010010000000
:
:
:
:
pv
pv
pv
pv
=
=
=
=
0
0
0
0
;
;
;
;
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
break;
break;
break;
break;

case
case
case
case
case
case
case
case
case
0b0000011011011000
0b0000010011011000
0b0000011011001000
0b0000011010011000
0b0000010010011000
0b0000011010001000
0b0000011001011000
0b0000010001011000
0b0000011001001000
:
:
:
:
:
:
:
:
:
pv
pv
pv
pv
pv
pv
pv
pv
pv
=
=
=
=
=
=
=
=
=
0
0
0
0
0
0
0
0
0
;
;
;
;
;
;
;
;
;
pv
pv
pv
pv
=
=
=
=
70
65
-70
-65
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
perempatan_kanan();
//
//
//
//
//
//
case 0b0000011110000000
case 0b0000001111000000
case 0b0000000001111000
case 0b0000000011110000
case 0b0000000000000000 :
:
:
:
:
; x = 1;break;
; x = 1;break;
; x = 1;break;
; x = 1;break;
if(bitx==0)
{
pv=3;
//
//
//
}
if(bitx==1)
{
if(x)
{
kiri_mundur();
break;
break;
break;
break;
break;
break;
break;
break;
break;
//
//
//
//
//
//
lpwm = 1000;
kanan_mundur();
rpwm = 1000;
delay_ms(30);
x=0;
}
pv =-80 ;
}
if(bitx==2)
{
if(x)
{
kiri_mundur();
lpwm = 1000;
kanan_mundur();
rpwm = 1000;
delay_ms(30);
x=0;
}
pv =80 ;
}
//
//
//
//
//
//
//
//
//
break;
//
}
lcd_gotoxy(0,0);
sprintf(buff,"%d%d %d%d%d%d%d%d%d%d %d%d",ka_b,ka_a,b3,b4,b5,b6,b7,b8,
b9,b10,ki_a,ki_b);
lcd_puts(buff);
lcd_gotoxy(0,1);
sprintf(buff,"%d %d %d",lpwm,debug,rpwm);
lcd_puts(buff);
error = sp - pv ;
proportional = (kp * error) ;
error_acc = error_acc + error ;
integratif = (ki * error_acc) / 100 ;
error_rate = error - last_error ;
derivatif
= (kd * error_rate) ;
last_error = error ;
control_signal = proportional + integratif + derivatif ;
pwm = starting_speed + control_signal;

if (pwm > 0)
kiri_maju();
if (pwm < 0)
{
pwm = 0 - pwm;
kiri_mundur();
}
if (pwm > maximum_speed)
pwm = maximum_speed;
//
if ((pwm > 0)&&(pwm < minimum_speed))
//
pwm = minimum_speed;
if (pv == 0 )
{
//pwm = extra_speed;
error_acc = 0;
}
if ( pv == 0 | pv == -3 | pv == 3 )
{
error_acc = 0;
}
lpwm = pwm ;
pwm = starting_speed - control_signal;
if (pwm > 0)
kanan_maju();
if (pwm < 0)
{
pwm = 0 - pwm;
kanan_mundur();
}
//
//
if (pv == 0)
{
error_acc = 0;
}
if ( pv == 0 | pv == -3 | pv == 3 )
{
error_acc = 0;
}
rpwm = pwm ;
debug++;
delay_ms(sampling_rate);
};
//----------------------------------------------------------------------------------------------//
siap_maju_kanan:
lcd_clear();
kp = kp_eeprom ;
//
ki = ki_eeprom ;
//
kd = kd_eeprom ;
last_error = 0 ;
error_acc = 0 ;
//
//
//
//
while (1)
{
lcd_gotoxy(0,0);
lcd_putsf("...BISMILLAH....");
lpwm=512;
rpwm=512;
};
}
unsigned int adc_auto_scan()

{
unsigned char variabel1=0;
//
unsigned int variabel2=0;
//
yb3=0;
//if(PINB.5 == 1)
//
{
//variabel2 |= 0b0000000000000001;
//
b0 = 1;
//
}
//else
//
{
//
b0 = 0;
//
};
//if(PINB.4 == 1)
//
{
//
variabel2 |= 0b0000000000000010;
//
b1 = 1;
//
}
//
//
//
//
//
//else
//
{
//
b1 = 0;
//
};
//
//if(PINB.3 == 1)
//
{
//
variabel2 |= 0b0000000000000100;
//
b2 = 1;
//
}
//else
//
{
//
b2 = 0;
//
};
variabel1 = read_adc();
if(variabel1 > variabel3)
{
variabel2 |= 0b0000000000001000;
b3 = 1;
yb3++;
}
else
{
b3 = 0;
}
//
//
//
//
//
//
ADMUX = 0x26;
{
variabel2 |= 0b0000000000010000;
b4 = 1;
yb3++;
}
else
{
b4 = 0;
}
//
ADMUX = 0x25;
//
{
variabel2 |= 0b0000000000100000;
b5 = 1;
yb3++;
}
else
{
b5 = 0;
}
ADMUX = 0x24;
//
//
//
//
{
variabel2 |= 0b0000000001000000;
b6 = 1;
yb3++;
}
else
{
b6 = 0;
}
//
//
//
ADMUX = 0x23;
//
{
variabel2 |= 0b0000000010000000;
b7 = 1;
yb3++;
}
else
{
b7 = 0;
}
//
//
//
ADMUX = 0x22;
//
{
variabel2 |= 0b0000000100000000;
b8 = 1;
yb3++;
}
else
{
b8 = 0;
}
//
//
//
ADMUX = 0x21;
//
{
variabel2 |= 0b0000001000000000;
b9 = 1;
yb3++;
}
else
{
b9 = 0;
}
ADMUX = 0x20;
//
//
//
//
{
variabel2 |= 0b0000010000000000;
b10 = 1;
yb3++;
}
else
{
b10 = 0;
}
//
//
//
ADMUX = 0x27;
return variabel2;
}
unsigned int adc_auto_scan2()
{
//
unsigned int variabel2=0;
//
/*variabel1 = read_adc();
{
variabel2 |= 0b0000000000001000;
b3 = 1;
}
else
{
b3 = 0;
}
*/
ADMUX--;
{
variabel2 |= 0b0000000000010000;
b4 = 1;
}
else
{
b4 = 0;
}
ADMUX--;
//
//
//
//
//
//
variabel2 |= 0b0000000000100000;
b5 = 1;
//
else
{
//
b5 = 0;
}
ADMUX--;
//
{
variabel2 |= 0b0000000001000000;
b6 = 1;
}
else
{
b6 = 0;
}
//
//
//
ADMUX--;
//
{
variabel2 |= 0b0000000010000000;
b7 = 1;
}
else
{
b7 = 0;
}
//
//
//
ADMUX--;
//
{
variabel2 |= 0b0000000100000000;
b8 = 1;
}
else
{
b8 = 0;
}
//
//
//
ADMUX--;
//
{
variabel2 |= 0b0000001000000000;
b9 = 1;
//
/
/
}
else
{
//
b9 = 0;
}
/*
ADMUX--;
//
{
variabel2 |= 0b0000010000000000;
b10 = 1;
}
else
{
b10 = 0;
}
*/
ADMUX = 0x27;
//
//
//
return variabel2;
}
unsigned char read_adc()
{
unsigned char data_adc;
ADCSRA |= 0x40;
/*Bit ADSC[6] diaktifkan, Conversion Start*/
while ((ADCSRA & 0x10)==0); /*Selama ADIF[5]= 0 maka InfiniteLoop. intinya

Kita menunggu sampai nilai ADIF bernilai 1,
(yang berarti Konversi telah selesai)*/
data_adc = ADCH;
/*Di program ini ketelitian 10 bit

tidak dibutuhkan*/
ADCSRA |= 0x10;
/*ADIF bit kita reset dengan memberi nilai 1 */
return data_adc;
}
//
//
//----------------------------------------------------------------------------------------------//
//----------------Subrutin Arah Mobil-----------------------------------------------------------//
//
void kiri_maju()
//
{
//
//
/* Motor Kanan Maju */
PORTD.2 = 1;
//
PORTD.3 = 0;
//
}
//
//
void kanan_maju()
//
{
//
PORTD.7 = 1;
/* Motor Kiri Maju */

//
PORTD.6 = 0;
//
}
//
//
void kiri_mundur()
//
{
//
//
/* Motor Kanan Mundur */
PORTD.2 = 0;
//
PORTD.3 = 1;
//
}
//
//
void kanan_mundur()
//
{
//
PORTD.7 = 0;
/* Motor Kiri Mundur */
//
PORTD.6 = 1;
//
}
void kiri_r()
{
PORTD.7 = 0;
/* Motor Kiri Mundur Pelan*/

//
PORTD.6 = 1;
PORTD.2 = 1;
/* Motor Kanan Maju */

//
PORTD.3 = 0;
}
void kanan_r()
{
PORTD.7 = 1;
//
PORTD.6 = 0;
/* Motor Kiri Maju */
PORTD.2 = 0;
/* Motor Kanan Mundur*/

//
PORTD.3 = 1;
}
/*
void pertigaan()
{
kanan_r();
lpwm = 1000;
rpwm = 1000;
while ( !PINB.2 );
while ( PINB.2 );
}
*/
void perempatan_kanan()
{
count++;
/*
yb31=0;
yb4=0;
yb5=0;
yb6=0;
yb7=0;
yb8=0;
yb9=0;
yb10=0;
*/
if ( count == 1 )
{
for(i=0;i<75;i++)
{
maping_darurat();
}
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
kanan_maju();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ka_b);
kanan_maju();
kiri_mundur();
lpwm=1000;
rpwm=1000;
while(!ka_b);
bitx=2;
for(i=0;i<200;i++)
{
maping_darurat();
}
if ( count == 2 )
{
for(i=0;i<100;i++)
{
maping_darurat();
}
//
//
//
//
//
//
//
}
kanan_maju();
kiri_mundur();
lpwm=1000;
rpwm=1000;
while(!ka_b);
while(ka_b);
bitx=2;
if ( count == 3 )
{
for(i=0;i<100;i++)
{
maping_darurat();
}
//
//
//
//
//
//
//
//
//
//
//
//
kanan_maju();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ka_b);
kanan_mundur();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ki_a);
while(ki_a);
bitx=1;
if ( count == 4 )
{
for(i=0;i<300;i++)
{
maping_darurat();
}
if ( count == 5 )
{
for(i=0;i<200;i++)
{
maping_darurat();
}
}
if ( count == 6 )
{
//
//
//
//
//
//
//
//
//
//
//
kanan_maju();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ka_b);
while(ka_b);
kanan_maju();
kiri_mundur();
lpwm=1000;
rpwm=1000;
bitx=2;
for(i=0;i<200;i++)
{
maping_darurat();
}
}
if ( count == 7 )
{
kanan_maju();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ka_b);
while(ka_b);
kanan_maju();
kiri_mundur();
lpwm=1000;
rpwm=1000;
bitx=2;
if ( count == 8 )
{
kanan_maju();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ka_b);
kanan_mundur();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ki_a);
while(ki_a);
bitx=1;
}
if ( count == 9 )
{
for(i=0;i<105;i++)
{
maping_darurat();
}
}
if ( count == 10 )
{
for(i=0;i<200;i++)
{
maping_darurat();
}
}
if ( count == 11 )
{
kanan_maju();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ka_b);
while(ka_b);
kanan_maju();
kiri_mundur();
lpwm=1000;
rpwm=1000;
bitx=2;
}
if ( count == 12 )
{
kanan_maju();
kiri_maju();
lpwm=1000;
rpwm=1000;
while(!ka_b);
while(ka_b);
kanan_mundur();
kiri_maju();
lpwm=1000;
rpwm=1000;
bitx=1;
}
//
//
//
//
//
//
//
}
kiri_r();
lpwm = 1000;
rpwm = 1000;
while ( !PINB.5 );
while ( PINB.5 );
while ( !PINB.5 );
while ( PINB.5 );
void perempatan_kiri()
{
count++;
/*
yb31=0;
yb4=0;
yb5=0;
yb6=0;
yb7=0;
yb8=0;
yb9=0;
yb10=0;
*/
if ( count == 1 )
{
for(i=0;i<75;i++)
{
maping_darurat();
}
}
if ( count == 2 )
{
for(i=0;i<75;i++)
{
maping_darurat();
}
}
if ( count == 3 )
{
/*
kiri_mundur();
lpwm = 1000;
kanan_mundur();
rpwm = 1000;
delay_ms(10);
*/
kiri_r();
lpwm = 1000;
rpwm = 1000;
while ( !PINB.5 );
while ( PINB.5 );
while ( !PINB.5 );
while ( PINB.5 );
while (!PINB.1);
}
if ( count == 4 )
{
for(i=0;i<100;i++)
{
maping_darurat();
}
}
/*
if ( count >= 4 )
{
kiri_stop();
kanan_stop();
delay_ms(500);
}
*/
if ( count == 5 )
{
for(i=0;i<75;i++)
{
maping_darurat();
}
if ( count == 6 )
{
for(i=0;i<75;i++)
{
maping_darurat();
}
}
if ( count == 7 )
{
for(i=0;i<100;i++)
{
maping_darurat();
}
//
//
//
//
//
//
//
kanan_r();
lpwm = 1000;
rpwm = 1000;
while ( !PINB.2 );
while ( PINB.2 );
while ( !PINB.2 );
while ( PINB.2 );
if ( count == 8 )
{
/*
kiri_r();
lpwm = 900;
rpwm = 1000;
while ( !PINB.5 );
while ( PINB.5 );
while ( !PINB.5 );
while ( PINB.5 );
*/
for(i=0;i<75;i++)
{
maping_darurat();
}
}
if ( count == 9 )
{
for(i=0;i<75;i++)
{
maping_darurat();
}
}
if ( count == 10 )
{
kanan_r();
lpwm = 900;
rpwm = 1000;
while ( !PINB.2 );
while ( PINB.2 );
while ( !PINB.2 );
while ( PINB.2 );
g=1;
for(i=0;i<300;i++)
{
maping_darurat();
}
}
if ( count >= 11 )
{
for(i=0;i<100;i++)
{
maping_darurat();
}
}
//
//
//
//
//
//
//
}
kiri_r();
lpwm = 1000;
rpwm = 1000;
while ( !PINB.5 );
while ( PINB.5 );
while ( !PINB.5 );
while ( PINB.5 );
/*
void deteksi_perempatan()
{
cek_b2();
cek_b3();
cek_b4();
cek_b5();
cek_b6();
cek_b7();
cek_b8();
cek_b9();
cek_b10();
cek_b11();
sense_kanan= yb2 + yb31 + yb4 + yb5 + yb6;
sense_kiri= yb7 + yb8 + yb9 + yb10 + yb11;

if((sense_kanan == 5) && (sense_kiri==5) )
{
perempatan_kiri();
}
}
void cek_b2()
{
if(PINB.3==1)
{
yb2=1;
mb2=1;
}
if(yb2==1 && PINB.3==0)
{
mb2++;
if(mb2>=30)
{
yb2=0;
}
}
}
void cek_b3()
{
if(b3==1)
{
yb31=1;
mb3=1;
}
if(yb31==1 && b3==0)
{
mb3++;
if(mb3>=30)
{
yb31=0;
}
}
}
void cek_b4()
{
if(b4==1)
{
yb4=1;
mb4=1;
}
if(yb4==1 && b4==0)
{
mb4++;
if(mb4>=30)
{
yb4=0;
}
}
}
void cek_b5()
{
if(b5==1)
{
yb5=1;
mb5=1;
}
if(yb5==1 && b5==0)
{
mb5++;
if(mb5>=30)
{
yb5=0;
}
}
}
void cek_b6()
{
if(b6==1)
{
yb6=1;
mb6=1;
}
if(yb6==1 && b6==0)
{
mb6++;
if(mb6>=30)
{
yb6=0;
}
}
}
void cek_b7()
{
if(b7==1)
{
yb7=1;
mb7=1;
}
if(yb7==1 && b7==0)
{
mb7++;
if(mb7>=30)
{
yb7=0;
}
}
}
void cek_b8()
{
if(b8==1)
{
yb8=1;
mb8=1;
}
if(yb8==1 && b8==0)
{
mb8++;
if(mb8>=30)
{
yb8=0;
}
}
}
void cek_b9()
{
if(b9==1)
{
yb9=1;
mb9=1;
}
if(yb9==1 && b9==0)
{
mb9++;
if(mb9>=30)
{
yb9=0;
}
}
}
void cek_b10()
{
if(b10==1)
{
yb10=1;
mb10=1;
}
if(yb10==1 && b10==0)
{
mb10++;
if(mb10>=30)
{
yb10=0;
}
}
}
void cek_b11()
{
if(PINB.0==1)
{
yb11=1;
mb11=1;
}
if(yb11==1 && PINB.0==0)
{
mb11++;
if(mb11>=30)
{
yb11=0;
}
}
}
*/
void maping_darurat()
{
if(ka_a==1 && ki_a==1)
{
perempatan_kanan();
}
if (ka_a==1)
{
bitx = 1;
}
if (ki_a==1)
{
bitx = 2;
}
bit_sensor = adc_auto_scan();
switch(bit_sensor)
{
case
case
case
case
0b0000000000001000
0b0000000000011000
0b0000000000111000
0b0000000001111000
:
:
:
:
pv
pv
pv
pv
=
=
=
=
-20
-17
-15
-13
; x = 1;break;
; x = 1;break;
; x = 1;break;
; x = 1;break;
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
0b0000000000010000
0b0000000000110000
0b0000000001110000
0b0000000011110000
0b0000000000100000
0b0000000001100000
0b0000000111110000
0b0000000011100000
0b0000000001000000
0b0000000111100000
0b0000000011000000
0b0000001111100000
0b0000000010000000
0b0000000111000000
0b0000001111000000
0b0000000110000000
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
:
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
pv
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
=
-15 ; x = 1;break;
-13 ; x = 1;break;
-9 ; x = 1;break;
-5 ; x = 1;break;
-9 ; x = 1;break;
-5 ; x = 1;break;
-3 ; x = 1;break;
-3 ; x = 1;break;
-3 ; x = 1;break;
0 ; x = 1;break;
0 ; x = 1;break;
3 ; x = 1;break;
3 ; x = 1;break;
3 ; x = 1;break;
5 ; x = 1;break;
5 ; x = 1;break;
case
case
case
case
case
case
case
case
0b0000000100000000
0b0000001110000000
0b0000011110000000
0b0000001100000000
0b0000001000000000
0b0000011100000000
0b0000011000000000
0b0000010000000000
:
:
:
:
:
:
:
:
pv
pv
pv
pv
pv
pv
pv
pv
=
=
=
=
=
=
=
=
9 ; x = 1;break;
9 ; x = 1;break;
13 ; x = 1;break;
13 ; x = 1;break;
15 ; x = 1;break;
15 ; x = 1;break;
17 ; x = 1;break;
20 ; x = 1;break;
case 0b0000000000000000 :
if(bitx==0)
{
pv=3;
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
}
if(bitx==1)
{
if(x)
{
kiri_mundur();
lpwm = 1000;
kanan_mundur();
rpwm = 1000;
delay_ms(30);
x=0;
}
pv =-80 ;
}
if(bitx==2)
{
if(x)
{
kiri_mundur();
lpwm = 1000;
kanan_mundur();
rpwm = 1000;
delay_ms(30);
x=0;
}
pv =80 ;
}
break;
//
}
lcd_gotoxy(0,0);
sprintf(buff,"%d%d %d%d%d%d%d%d%d%d %d%d",ka_b,ka_a,b3,b4,b5,b6,b7,b8,
b9,b10,ki_a,ki_b);
lcd_puts(buff);
lcd_gotoxy(0,1);
sprintf(buff,"%d %d %d",lpwm,debug,rpwm);
lcd_puts(buff);
error = sp - pv ;
proportional = (kp * error) ;
error_acc = error_acc + error ;
integratif = (ki * error_acc) / 100 ;
error_rate = error - last_error ;
derivatif
= (kd * error_rate) ;
last_error = error ;
control_signal = proportional + integratif + derivatif ;
pwm = starting_speed + control_signal;
if (pwm > 0)
kiri_maju();
if (pwm < 0)
{
pwm = 0 - pwm;
kiri_mundur();
}
//
//
if (pv == 0 )
{
error_acc = 0;
}
if ( pv == 0 | pv == -3 | pv == 3 )
{
error_acc = 0;
}
lpwm = pwm ;
pwm = starting_speed - control_signal;
if (pwm > 0)
kanan_maju();
if (pwm < 0)
{
pwm = 0 - pwm;
kanan_mundur();
}
//
//
if (pv == 0)
{
error_acc = 0;
}
if ( pv == 0 | pv == -3 | pv == 3 )
{
error_acc = 0;
}
rpwm = pwm ;
}
//
//
//----------------------------------------------------------------------------------------------//
/*
//----------------Sub Rutin Memasukkan EEPROM---------------------------------------------------//
//
void EEPROM_write(unsigned char uiaddress, unsigned char ucdata)
//
{
//
while((EECR & 0x02)!=0);
EEARH = 0;
//
EEARL = uiaddress;
//
EEDR = ucdata;
EECR |= 0x04;
//
EECR |= 0x02;
//
//
}
//
//
//----------------------------------------------------------------------------------------------//
//----------------Sub Rutin Membaca EEPROM------------------------------------------------------//

//
unsigned char EEPROM_read(unsigned char uiaddress)
//
{
//
while((EECR & 0x02)!=0);
EEARH = 0;
//
EEARL = uiaddress;
//
EECR |= 0x01;
//
//
return EEDR;
//
}
void clear_EEPROM()
{
EEAR |= 0xFF;
EEDR |= 0xFF;
EECR |= 0xFF;
}
//
//
//----------------------------------------------------------------------------------------------//
*/
//----------------Sub Rutin Menampilkan Variabel------------------------------------------------//
void lcd_sukses()
{
lcd_gotoxy(0,1);
lcd_putsf("=SIMPAN==SUKSES=");
delay_ms(250);
}
//----------------------------------------------------------------------------------------------//

Buat Blajar

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Buat Blajar

Uploaded by

Copyright:

Available Formats

1163644

External RAM size

Data Stack size

lpwm OCR1B // PWM 10-Bit Maka Nilai Maksimal 1024

//---------------Dekalarsi Variabel Global------------------------------------------------------//

eeprom unsigned char ki_eeprom = 10;

Bit ADEN[7]=1 (Enabling ADC)

//--------------Program Inti Line Follower------------------------------------------------------//

case 0b0000001100110000 : pv = 0 ; perempatan_kanan(); break;

case 0b0000000001001000 : pv = 0 ; perempatan_kanan(); break;

case 0b0000010001000000 : pv = 0 ; perempatan_kanan(); break;

pwm = starting_speed + control_signal;

unsigned int adc_auto_scan()

/*Bit ADSC[6] diaktifkan, Conversion Start*/

while ((ADCSRA & 0x10)==0); /*Selama ADIF[5]= 0 maka InfiniteLoop. intinya

/*Di program ini ketelitian 10 bit

/*ADIF bit kita reset dengan memberi nilai 1 */

/* Motor Kiri Maju */

/* Motor Kiri Mundur Pelan*/

/* Motor Kanan Maju */

/* Motor Kiri Maju */

/* Motor Kanan Mundur*/

sense_kiri= yb7 + yb8 + yb9 + yb10 + yb11;

//----------------Sub Rutin Membaca EEPROM------------------------------------------------------//

You might also like

/Bit ADSC[6] diaktifkan, Conversion Start/

/ADIF bit kita reset dengan memberi nilai 1 /