You are on page 1of 8

/***************************************************** This program was produced by the CodeWizardAVR V2.04.

4a Advanced Automatic Program Generator Copyright 1998-2009 Pavel Haiduc, HP InfoTech s.r.l. http://www.hpinfotech.com Project : Version : Date : 05/10/2012 Author : NeVaDa Company : Comments: Chip type : ATmega128 Program type : Application AVR Core Clock frequency: 16,000000 MHz Memory model : Small External RAM size : 0 Data Stack size : 1024 *****************************************************/ #include #include #include #include #include #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define <mega128.h> <delay.h> <stdio.h> <stdlib.h> <math.h> MOTOR_1_JALAN MOTOR_1_STOP MOTOR_1_CW MOTOR_1_CCW MOTOR_1_EXT MOTOR_1_SPEED MOTOR_2_JALAN MOTOR_2_STOP MOTOR_2_CW MOTOR_2_CCW MOTOR_2_EXT MOTOR_2_SPEED MOTOR_3_JALAN MOTOR_3_STOP MOTOR_3_CW MOTOR_3_CCW MOTOR_3_EXT MOTOR_3_SPEED MOTOR_4_JALAN MOTOR_4_STOP MOTOR_4_CW MOTOR_4_CCW MOTOR_4_EXT MOTOR_4_SPEED PORTD.0=PORTD.1=0 PORTD.0=PORTD.1=1 PORTD.2=1 PORTD.2=0 PORTD.3=1 OCR1A PORTD.4=PORTD.5=0 PORTD.4=PORTD.5=1 PORTD.6=1 PORTD.6=0 PORTD.7=1 OCR1B PORTC.0=PORTC.1=0 PORTC.0=PORTC.1=1 PORTC.2=1 PORTC.2=0 PORTD.3=1 OCR1CL PORTC.4=PORTC.5=0 PORTC.4=PORTC.5=1 PORTC.6=1 PORTC.6=0 PORTD.7=1 OCR3AL

char lcd[20];

//*********** Definisi Tombol Menu *************// #define BUTTON_0 !PINF.6 #define BUTTON_1 !PINF.5 #define BUTTON_2 !PINF.7 #define BUTTON_3 !PINF.4 //#define BUTTON_4 !PINC.6 //#define BUTTON_5 !PINC.1 //#define BUTTON_6 !PINC.7 //#define BUTTON_7 !PINC.0 unsigned char menu_loop; #define BUTTON(M_0,M_1,M_2,M_3) \ //,M_4,M_5,M_6,M_7) \ delay_ms(20); \ menu_loop=0; \ do{ \ if(BUTTON_0) \ menu_loop++; \ delay_ms(20); \ if(menu_loop>15) goto MENU_UTAMA; \ }while(BUTTON_0||BUTTON_1||BUTTON_2||BUTTON_3); \ //||BUTTON_4||BUTTON_5||BUTT ON_6||BUTTON_7); \ do { \ if(BUTTON_0) goto M_0; \ if(BUTTON_1) goto M_1; \ if(BUTTON_2) goto M_2; \ if(BUTTON_3) goto M_3; \ \ //if(BUTTON_4) goto M_4; \ \ //if(BUTTON_5) goto M_5; \ \ //if(BUTTON_6) goto M_6; \ \ //if(BUTTON_7) goto M_7; \ delay_ms(20); \ } \ while (1); #define DISPLAY(DISPLAY1,DISPLAY2, DISPLAY3, DISPLAY4) do { lcd_gotoxy(0,0); lcd_putsf(""#DISPLAY1""); lcd_gotoxy(0,1); lcd_putsf(""#DISPLAY2""); lcd_gotoxy(0,2); lcd_putsf(""#DISPLAY3""); lcd_gotoxy(0,3); lcd_putsf(""#DISPLAY4""); }while(0) // Alphanumeric LCD Module functions #asm .equ __lcd_port=0x1B ;PORTA #endasm #include <lcd.h> // Declare your global variables here void jalan(int kecepatan_x, int kecepatan_y, int kecepatan_sudut); void jalan_putar(int kecepatan_x, int kecepatan_y, int kec_putar); \ \ \ \ \ \ \ \ \ \

void main(void) { // Declare your local variables here // Input/Output Ports initialization // Port A initialization // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T PORTA=0x00; DDRA=0x00; // Port B initialization // Func7=Out Func6=Out Func5=Out Func4=In Func3=In Func2=In Func1=In Func0=In // State7=0 State6=0 State5=0 State4=T State3=T State2=T State1=T State0=T PORTB=0x00; DDRB=0xE0; // Port C initialization // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T PORTC=0xFF; DDRC=0xFF; // Port D initialization // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T PORTD=0xFF; DDRD=0xFF; // Port E initialization // Func7=In Func6=In Func5=In Func4=In Func3=Out Func2=In Func1=In Func0=In // State7=T State6=T State5=T State4=T State3=0 State2=T State1=T State0=T PORTE=0x00; DDRE=0x08; // Port F initialization // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T PORTF=0xFF; DDRF=0x00; // Port G initialization // Func4=In Func3=In Func2=In Func1=In Func0=In // State4=T State3=T State2=T State1=T State0=T PORTG=0x00; DDRG=0x00; // Timer/Counter 0 initialization // Clock source: System Clock // Clock value: Timer 0 Stopped // Mode: Normal top=FFh // OC0 output: Disconnected ASSR=0x00; TCCR0=0x00; TCNT0=0x00; OCR0=0x00; // Timer/Counter 1 initialization // Clock source: System Clock

// Clock value: 250,000 kHz // Mode: Ph. correct PWM top=00FFh // OC1A output: Non-Inv. // OC1B output: Non-Inv. // OC1C output: Non-Inv. // Noise Canceler: Off // Input Capture on Falling Edge // Timer1 Overflow Interrupt: Off // Input Capture Interrupt: Off // Compare A Match Interrupt: Off // Compare B Match Interrupt: Off // Compare C Match Interrupt: Off TCCR1A=0xA9; TCCR1B=0x03; TCNT1H=0x00; TCNT1L=0x00; ICR1H=0x00; ICR1L=0x00; OCR1AH=0x00; OCR1AL=0x00; OCR1BH=0x00; OCR1BL=0x00; OCR1CH=0x00; OCR1CL=0x00; // Timer/Counter 2 initialization // Clock source: System Clock // Clock value: Timer2 Stopped // Mode: Normal top=FFh // OC2 output: Disconnected TCCR2=0x00; TCNT2=0x00; OCR2=0x00; // Timer/Counter 3 initialization // Clock source: System Clock // Clock value: 250,000 kHz // Mode: Ph. correct PWM top=00FFh // OC3A output: Non-Inv. // OC3B output: Discon. // OC3C output: Discon. // Noise Canceler: Off // Input Capture on Falling Edge // Timer3 Overflow Interrupt: Off // Input Capture Interrupt: Off // Compare A Match Interrupt: Off // Compare B Match Interrupt: Off // Compare C Match Interrupt: Off TCCR3A=0x81; TCCR3B=0x03; TCNT3H=0x00; TCNT3L=0x00; ICR3H=0x00; ICR3L=0x00; OCR3AH=0x00; OCR3AL=0x00; OCR3BH=0x00; OCR3BL=0x00; OCR3CH=0x00; OCR3CL=0x00;

// External Interrupt(s) initialization // INT0: Off // INT1: Off // INT2: Off // INT3: Off // INT4: Off // INT5: Off // INT6: Off // INT7: Off EICRA=0x00; EICRB=0x00; EIMSK=0x00; // Timer(s)/Counter(s) Interrupt(s) initialization TIMSK=0x00; ETIMSK=0x00; // Analog Comparator initialization // Analog Comparator: Off // Analog Comparator Input Capture by Timer/Counter 1: Off ACSR=0x80; SFIOR=0x00; // LCD module initialization lcd_init(20); MOTOR_1_EXT; MOTOR_2_EXT; MOTOR_3_EXT; MOTOR_4_EXT; lcd_gotoxy(0,0); lcd_puts("Bismillah"); delay_ms(200); while (1) { // Place your code here MENU_UTAMA: DISPLAY(<JALAN >); // <START START B> // // // // jalan(0,0,0); MOTOR_1_STOP; MOTOR_2_STOP; MOTOR_3_STOP; MOTOR_4_STOP; BUTTON(JALAN,MENU_UTAMA,JALAN_2,JALAN_3); JALAN: DISPLAY(< // >); <START BACK>,<JALAN2 B> <START JALAN3>,< B> <START >,< B> <

BACK>,< B> <START

>,< B> <START

>,< B> <

START

B> //jalan(0,-70,0); delay_ms(500); jalan_putar(140,140,40); jalan(0,70,0); MOTOR_1_STOP; MOTOR_2_STOP; MOTOR_3_STOP; MOTOR_4_STOP; delay_ms(700); goto MENU_UTAMA; JALAN_2: DISPLAY(< // BACK>,< B> <START >,< B> <START >,< B> <

START

>); <START B>

//jalan(0,0,20); //jalan(0,-70,0); delay_ms(500); jalan_putar(100,100,40); jalan(0,70,0); MOTOR_1_STOP; MOTOR_2_STOP; MOTOR_3_STOP; MOTOR_4_STOP; delay_ms(700); goto MENU_UTAMA; JALAN_3: DISPLAY(< // START >); <START B> BACK>,< B> <START >,< B> <START >,< B> <

//jalan(0,-70,0); delay_ms(500); jalan_putar(70,70,40); jalan(0,70,0); MOTOR_1_STOP; MOTOR_2_STOP; MOTOR_3_STOP; MOTOR_4_STOP; delay_ms(700); goto MENU_UTAMA; }; } void jalan(int kecepatan_x, int kecepatan_y, int kecepatan_sudut) { int kec_motor_1, kec_motor_2, kec_motor_3, kec_motor_4; kecepatan_y = -kecepatan_y;

kec_motor_1 kec_motor_2 kec_motor_3 kec_motor_4

= = = =

(float) (float) (float) (float)

kecepatan_x*0.7071+kecepatan_y*0.7071+kecepatan_sudut; kecepatan_x*0.7071-kecepatan_y*0.7071+kecepatan_sudut; -kecepatan_x*0.7071-kecepatan_y*0.7071+kecepatan_sudut; -kecepatan_x*0.7071+kecepatan_y*0.7071+kecepatan_sudut;

if(kec_motor_1>0) MOTOR_1_CW; else MOTOR_1_CCW; if(kec_motor_2>0) MOTOR_2_CW; else MOTOR_2_CCW; if(kec_motor_3>0) MOTOR_3_CW; else MOTOR_3_CCW; if(kec_motor_4>0) MOTOR_4_CW; else MOTOR_4_CCW; kec_motor_1 kec_motor_2 kec_motor_3 kec_motor_4 = = = = abs(kec_motor_1); abs(kec_motor_2); abs(kec_motor_3); abs(kec_motor_4); kec_motor_1=255; kec_motor_2=255; kec_motor_3=255; kec_motor_4=255;

if(kec_motor_1>255) if(kec_motor_2>255) if(kec_motor_3>255) if(kec_motor_4>255) MOTOR_1_JALAN; MOTOR_2_JALAN; MOTOR_3_JALAN; MOTOR_4_JALAN; MOTOR_1_SPEED MOTOR_2_SPEED MOTOR_3_SPEED MOTOR_4_SPEED } = = = =

kec_motor_1; kec_motor_2; kec_motor_3; kec_motor_4;

void jalan_putar(int kecepatan_x, int kecepatan_y, int kec_putar) { int var_tambah=0; int kec_x, kec_y; for(var_tambah=0; var_tambah<2000; var_tambah+=20) { kec_x = ((float) -0.000001*kecepatan_x*((long)var_tambah*var_tambah)+0.002*k ecepatan_x*var_tambah); kec_y = (float) 0.001*kecepatan_y*var_tambah-kecepatan_y; jalan(kec_x, kec_y, kec_putar); sprintf(lcd, "%d %d", kec_x, kec_y); lcd_gotoxy(2,2); lcd_puts(" lcd_gotoxy(2,2); ");

lcd_puts(lcd); delay_ms(20); } jalan(0,0,0); }

You might also like