Grage Aeromodelling

Code: Select all

/*****************************************************
This program was produced by the
CodeWizardAVR V1.25.7 beta 5 Professional
Automatic Program Generator
© Copyright 1998-2007 Pavel Haiduc, HP InfoTech s.r.l.
http://www.hpinfotech.com

Project : 
Version : 
Date    : 7/21/2009
Author  : F4CG                            
Company : F4CG                            
Comments: 


Chip type           : ATmega8515
Program type        : Application
Clock frequency     : 11.059200 MHz
Memory model        : Small
External SRAM size  : 0
Data Stack size     : 128
*****************************************************/

#include <mega8515.h>
#include <math.h>
#include <delay.h>
#define aileron PINC.0
#define elevator PINC.1
#define pitch PINC.1
#define out1 PORTC.2
#define out2 PORTC.3
#define out3 PORTC.4

int ram_aileron,ram_elevator,ram_pitch;
// Declare your global variables here

long cc,a,b,c,pc,alpha;


void kirim_servo(long nilai,char port){

switch (port){
case 0: out1=1;out2=0;out3=0;
case 1: out1=0;out2=1;out3=0;
case 2: out1=0;out2=0;out3=1;
}                           
while (nilai--){
delay_us(1);
}                            
out1=0;out2=0;out3=0;
}
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=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 
PORTB=0x00;
DDRB=0x00;

// 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=0x00;
DDRC=0x00;

// 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=0x00;
DDRD=0x00;

// Port E initialization
// Func2=In Func1=In Func0=In 
// State2=T State1=T State0=T 
PORTE=0x00;
DDRE=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
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
// Timer 1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
EMCUCR=0x00;

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

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

DDRC=0;
PORTC=0xFF;

/*
#define aileron PINC.0
#define elevator PINC.1
#define pitch PINC.1

*/

DDRC.2=1;
DDRC.3=1;
DDRC.4=1;


cc=0;//nilai ini coba dirubah2
pc=0;//nilai ini coba dirubah2

while (1)
      {
while (!aileron){;ram_aileron=0;}

while (aileron){ram_aileron++;delay_us(1);}

while (!elevator){ram_elevator=0;}

while (elevator){ram_elevator++;delay_us(1);}

while (!pitch){;ram_pitch=0;}

while (pitch){ram_pitch++;delay_us(1);}         

a = cc * ( ram_elevator * cos (alpha + 0) + ram_aileron * cos (alpha + 90) ) + pc * ram_pitch;
b = cc * ( ram_elevator * cos (alpha + 120) + ram_aileron * cos (alpha + 210) ) + pc * ram_pitch;
c = cc * ( ram_elevator * cos (alpha + 240) + ram_aileron * cos (alpha + 330) ) + pc * ram_pitch ;

kirim_servo(a,0);
kirim_servo(b,1);
kirim_servo(c,2);

      };
}