#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<
// 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
val_jauh5=100-(10*(err5-(jauh_MAX)));
}
else jauh5=0;
//pas
if(err5<=jauh_MAX) pas5=0;
else if (err5
val_pas5=10*(err5-(jauh_MAX));
}
else if (err5<=pas_ONE) {
pas5=1;
val_pas5=100;
}
else if(err5
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
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauhpol4
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauhpol4
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ ska=1;
if (val_jauhpol4
if (rs>val_ska) val_ska=rs;
}
}
if (jauh4!=0){
if (jauh_pol5!=0)
{ bka=1;
if (val_jauh4
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauh4
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauh4
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_jauh4
if (rs>val_lurus) val_lurus=rs;
}
}
if (pas4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_pas4
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_pas4
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ lurus=1;
if (val_pas4
if (rs>val_lurus) val_lurus=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_pas4
if (rs>val_lurus) val_lurus=rs;
}
}
if (dkt4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_dkt4
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_dkt4
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ bki=1;
if (val_dkt4
if (rs>val_bki) val_bki=rs;
}
if (dkt5!=0)
{ bki=1;
if (val_dkt4
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
val_jauh5=100-(10*(err5-(jauh_MAX)));
}
else jauh5=0;
//pas
if(err5<=jauh_MAX) pas5=0;
else if (err5
val_pas5=10*(err5-(jauh_MAX));
}
else if (err5<=pas_ONE) {
pas5=1;
val_pas5=100;
}
else if(err5
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
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauhpol4
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauhpol4
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ ska=1;
if (val_jauhpol4
if (rs>val_ska) val_ska=rs;
}
}
if (jauh4!=0){
if (jauh_pol5!=0)
{ bka=1;
if (val_jauh4
if (rs>val_bka) val_bka=rs;
}
if (jauh5!=0)
{ bka=1;
if (val_jauh4
if (rs>val_bka) val_bka=rs;
}
if (pas5!=0)
{ bka=1;
if (val_jauh4
if (rs>val_bka) val_bka=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_jauh4
if (rs>val_lurus) val_lurus=rs;
}
}
if (pas4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_pas4
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_pas4
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ lurus=1;
if (val_pas4
if (rs>val_lurus) val_lurus=rs;
}
if (dkt5!=0)
{ lurus=1;
if (val_pas4
if (rs>val_lurus) val_lurus=rs;
}
}
if (dkt4!=0){
if (jauh_pol5!=0)
{ bki=1;
if (val_dkt4
if (rs>val_bki) val_bki=rs;
}
if (jauh5!=0)
{ bki=1;
if (val_dkt4
if (rs>val_bki) val_bki=rs;
}
if (pas5!=0)
{ bki=1;
if (val_dkt4
if (rs>val_bki) val_bki=rs;
}
if (dkt5!=0)
{ bki=1;
if (val_dkt4
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 (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_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;
};
}
Selengkapnya...
Selasa, 24 Maret 2009
FIre fighting source code codevision avr
obo-Magellan Robot Project
Robo-Magellan Robot Project - "Odyssey"
 |  |
After many months of effort, here is Odyssey, our SRS/Servo Robo-Magellan contest entry. What is Robo-Magellan you ask? It is a contest devised by the Seattle Robotics Society, inspired by the problems to be solved for the DARPA Grand Challenge. It is an outdoor, autonomous, robotic navigation contest. A Robo-Magellan robot must be able to autonomously drive around a 500ft. square area of the Seattle Center, avoiding trees, rocks, park benches, garbage cans, light posts, railings, sculptures, and many other obstacles, and find orange road cones placed at specific waypoints. The GPS coordinates of the cones are given out 30 minutes in advance of the contest, and the robot that navigates autonomously from the start to the finish cone in the shortest amount of time, wins the contest. The only remote control that is allowed is for a safety shut-off switch should the robot run into trouble. Each robot gets three tries to navigate to the final cone. The best time of three is used for their final score.
The Design:
Our robot is a monster. The contest rules state that the robot cannot weigh more than 50 pounds, Odyssey weighs in at 48 pounds even. Bob and I have worked hard and long on this robot, and inside, it contains a culmination of all our electronics efforts to-date. Of course, this robot would never have come together, if it weren't for our sponsor, NPC Robotics (plug-plug). They provided the excellent motors, and a motor controller that can easily move this beast over almost any terrain with ease, without any shortage of power or torque.
Bob shot the initial snapshot above during the final assembly phase of the robot. Notice once again, Bob's excellent CAD design, and how close the final product closely resembles the drawing. I think the only major changes that were made was the tail-wheel assembly, and the location of the GPS dome. Since weight was such an issue, Bob literally weighed every nut and bolt on the robot, and had the CAD program calculate the final weight, so we knew if we built the design, we would be under the 50 pound weight limit.
 |  |
Here is a laundry list of technologies we have on-board:
 | Almost every single board in our BotStack robot bus, main, sensor, CPU, navigation, camera, radio, and motor control. |
| Environmental Grade Sonar Array |
| GPS System with WAAS correction |
| 6-degree of freedom inertial guidance system (The grey box on top of the robot) |
| 2-axis Magneto-Inductive Compass |
| 20fps Camera & Computer Vision System |
| R/C Receiver for Remote Safety Fail-Safe. |
| 12v Permanent Magnet Motors with optical-encoder feedback for closed loop control and odometery. |
| 100amp Motor Controller |
Here is another photo of it from the front, and the inside:
Navigation:
  | Navigating the Robo-Magellan course could be accomplished in a number of different ways. We chose to use GPS, combined with a 2-axis compass, and a home-brew inertial navigation system. We chose a WAAS enabled, OEM model of GPS unit made by Garmin. The GPS-18 LVC. Since it is an OEM unit it needs a mechanism for reading and storing waypoints, off of the robot. We built this nifty handheld unit to walk the course and store the waypoints, and then download them from the handheld device into the Robot just before the competition. |
 | Here is our home-made inertial navigation unit. It has three ceramic gyro's, and two accelerometers to yield 6-degrees of freedom of inertial measurement. The compass board mounts just below the gyro board, with the same footprint, and all of this is housed in the square box on the top of the robot. All of these measurement instruments, combined with the GPS should give us fairly accurate position and heading computations as the robot moves through the course, with or without a consistent GPS signal. |
Fire-Fighting Robot Project
Home -> Robots - > Fire-Fighting Robot Fire-Fighting Robot Project - "FlameOut"
Latest Updates: (03/27/05) - Flameout takes the Silver medal in firefighting at the 2005 RoboGames (03/21/04) - Flameout takes the Bronze medal in the firefighting event at the Robolympics! (10/29/03) - Flameout takes 2nd place at the Seattle Robothon I built the first version of this fire-fighting robot, by taking Bob's well designed base for the tabletop challenge, and bolting on a top platform with all the necessary fire-fighting gear. It's first debut was at the 2003 Seattle Robothon, sponsored by the Seattle Robotics Society. All the additional sensors were a challenge to get debugged in-time for the event.
Robot Design:
RoboGames 2005 Firefighting Competition: Here are video clips of many of the firefighting runs of several of the competitors, including my robot FlameOut.
|
Here is "FlameOut" looking for the fire at the event. I figured it was either going to put the flame out....or Flame-Out itself. I have to mention that only one robot...the one that got first place actually put the candle out. There was a good-sized crowd of spectators on-hand to see the fire-fighting robots in Seattle.
Langganan:
Postingan (Atom)
