// Sourcecode – car
/*******************************************************
This program was created by the CodeWizardAVR V3.23a
Automatic Program Generator
?Copyright 1998-2015 Pavel Haiduc, HP InfoTech s.r.l.

http://www.hpinfotech.com

Project :
Version :
Date : 2015-11-17
Author :
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 <mega128.h>
#include <delay.h>
//char tc3_spH;
//char tc3_spL;
int stop_speed=0, cnt0=0, cnt1=0;

interrupt [TIM0_OVF] void timer0_overflow(void)
{
cnt0++;
if(cnt0 == 2000)
{
cnt0=0;
cnt1++;
if(cnt1 == 100)
{
cnt1=0;
cnt0=0;
//speed += 100;
//speed = 500;
}
}
}
// UART 통신 함수
void init_UART()
{
UCSR1A=0×0;
UCSR1B=0b00011000;
UCSR1C=0b00000110;
UBRR1H=0;
UBRR1L=16; //57600
}
//모터 제어 함수
void init_timer0(void)
{
TCCR0 = 0×01;
TIMSK |= 0×01;
}
void init_timer1(void)
{
TCCR1A = 0xA3;
TCCR1B = 0×01;
}
void forward(unsigned char motor_speed)
{
//tc3_spH=(char)(8>>(0xff00&pwmsignal));
//tc3_spL=(char)(0x00ff&pwmsignal);
PORTA=0×03;
OCR1A=motor_speed*4;
OCR1B=motor_speed*4;

}
void reverse(unsigned char motor_speed)
{
PORTA=0×00;
OCR1A=motor_speed*4;
OCR1B=motor_speed*4;
}
void turn_left(unsigned char motor_speed)
{
PORTA=0×01;
OCR1A=motor_speed*4;
OCR1B=motor_speed*2; //low reverse
}
void turn_right(unsigned char motor_speed)
{
PORTA=0×02;
OCR1A=motor_speed*2; // low reverse
OCR1B=motor_speed*4;
}
void forward_right(unsigned char motor_speed)
{
PORTA=0×03;
OCR1A=motor_speed*2;
OCR1B=motor_speed*4;
}
void forward_left(unsigned char motor_speed)
{
PORTA=0×03;
OCR1A=motor_speed*4;
OCR1B=motor_speed*2;
}
void reverse_right(unsigned char motor_speed)
{
PORTA=0×00;
OCR1A=motor_speed*2;
OCR1B=motor_speed*4;
}
void reverse_left(unsigned char motor_speed)
{
PORTA=0×00;
OCR1A=motor_speed*4;
OCR1B=motor_speed*2;
}
void stop()
{
PORTA=0×03;
OCR1A=stop_speed;
OCR1B=stop_speed;
}
void main(void)
{
unsigned char ch=0; // 데이터 받는 변수
init_timer0();
init_timer1();
init_UART();

DDRB=0xff;
DDRA=0×03;
#asm(“sei”)
PORTA=0×03;

while(1){
while(UCSR1A&0×80==0×0);
ch=UDR1;
if(ch%10==1) forward(ch-1);
else if(ch%10==2) reverse(ch-2);
else if(ch%10==3) turn_right(ch-3);
else if(ch%10==4) turn_left(ch-4);
else if(ch%10==5) forward_right(ch-5);
else if(ch%10==6) forward_left(ch-6);
else if(ch%10==7) reverse_right(ch-7);
else if(ch%10==8) reverse_left(ch-8);
else stop();
}
}

//Sourcecode – controller
/*****************************************************
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 : 2015-11-25
Author : NeVaDa
Company :
Comments:
8방향 제어
Chip type : ATmega128
Program type : Application
AVR Core Clock frequency: 8.000000 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 1024
*****************************************************/
#include <mega128.h>
#include <delay.h>
#include <stdio.h>
#include <stdlib.h>
///LCD define
#define ENABLE PORTD.7 // ENABLE 신호
#define FUNCSET 0×28 // Function Set
#define ENTMODE 0×06 // Entry Mode Set
#define ALLCLR 0×01 // All Clear
#define HOME 0×02 // HOME
#define DISPON 0x0c // Display On
#define LINE2 0xC0 // 2nd Line Move

///gyro define
#define MPU6050_ACCEL_XOUT_H 0x3B // R
#define MPU6050_TEMP_OUT_H 0×41 // R
#define MPU6050_GYRO_XOUT_H 0×43 // R
#define MPU6050_PWR_MGMT_1 0x6B // R/W
#define MPU6050_I2C_ADDR 0×68 // 디바이스 어드레스

//speed control
#define HIGH_SPEED 0×70;
#define MID_SPEED 0×50;
#define LOW_SPEED 0×30;
typedef unsigned char u_char;
u_char mpudata[14]; //mpu buffer
u_char buf;
u_char rdata;
///gyro func
int TWI_Write(int, u_char); // TWI 쓰기
int TWI_Read(int); // TWI 읽기
int TWI_Comm(int); // TWI 공통 함수
void error_out(u_char); // 에러 LED 출력함수
void MPU6050_read(void); // mpu read
void Putch(char data);
void main(void)
{
//LCD_init(); // lcd 초기화
UCSR1A=0×0;
UCSR1B=0b00011000;
UCSR1C=0b00000110;
UBRR1H=0;
UBRR1L=16;

TWBR=12; //TWI설정
TWSR=0×00;
while(1)
{
MPU6050_read(); //mpu read
delay_ms(200);
}
}
//출력함수
int TWI_Write(int addr, u_char data)
{
TWI_Comm(addr); // Start Condition, SLA + W, RA 전송

// Data
TWDR = data; // 전송 데이터
TWCR = 0×84; // TWINT, TWEN 셋
while((TWCR & 0×80) != 0×80);// TWINT 비트가 1이 될 때까지
if((TWSR & 0xF8)!= 0×28) return(0xEF); // 에러 체크

// STOP 조건 발생
TWCR = 0×94;

return(0);
}

int TWI_Read(int st_addr)
{
int err;

err = TWI_Comm(st_addr); // Start Condition, SLA + W, RA 전송
if(err != 0) return(err);

// 반복 START 조건 발생(Read 모드로 바꿈)
TWCR = 0xA4; // TWINT, TWSTA, TWEN 셋
while((TWCR & 0×80) != 0×80); // TWINT 비트가 1이 될 때까지
// ACK 수신
if((TWSR & 0xF8)!= 0×10) return(0x0F); // 에러 체크

// SLA + R
TWDR = (MPU6050_I2C_ADDR << 1) | 0×01; // R/W 비트 셋
TWCR = 0×84; // TWINT, TWEN 셋
while((TWCR & 0×80) != 0×80); // TWINT 비트가 1이 될 때까지
if((TWSR & 0xF8)!= 0×40) return(0x7f);

// Data Read
TWCR = 0×84; // TWINT, TWEN 셋
while((TWCR & 0×80) != 0×80); // TWINT 비트가 1이 될 때까지
// NACK 송신
if((TWSR & 0xF8)!= 0×58) return(0xF0);
rdata = TWDR;

// STOP 조건 발생
TWCR = 0×94;

return(0);
}
int TWI_Comm(int reg_addr)
{
// START 조건 발생
TWCR = 0xA4; // TWINT, TWSTA, TWEN 셋
while((TWCR & 0×80) != 0×80); // TWINT 비트가 1이 될 때까지
if((TWSR & 0xF8)!= 0×08) return(TWSR & 0xf8); // 에러 체크

// SLA + W
TWDR = MPU6050_I2C_ADDR << 1;
TWCR = 0b10000100; // TWINT, TWEN 셋
while((TWCR & 0×80) != 0×80); // TWINT 비트가 1이 될 때까지
// ACK 수신
if((TWSR & 0xF8)!= 0×18) return(0xFD); // 에러 체크

// Register Address(RA)
TWDR = reg_addr;
TWCR = 0b10000100;
while((TWCR & 0×80) != 0×80); // TWINT 비트가 1이 될 때까지
// ACK 수신
if((TWSR & 0xF8)!= 0×28) return(0xFB); // 에러 체크
return(0);
}
void error_out(u_char err)
{
PORTA = ~err;
//delay_ms(100);
}
void MPU6050_read(void)
{
int error,addr,x_accel,y_accel,n; //gyro변수
unsigned char temp=0;
error=TWI_Write(MPU6050_PWR_MGMT_1,0); //sleep mode 0
if(error!=0) error_out(error);
addr=MPU6050_ACCEL_XOUT_H;
for(n=0; n<14; n++)
{
error=TWI_Read(addr++);
if(error!=0) error_out(error);
mpudata[n]=rdata;
}
x_accel = (((int)mpudata[2] << 8) | ((int)mpudata[3] & 0x0FF));
y_accel = (((int)mpudata[4] << 8) | ((int)mpudata[5] & 0x0FF));

if(x_accel/200>25 && x_accel/200<45)
{
temp=0×51; //1단계 속도 전진
}
else if(x_accel/200>=45 && x_accel/200<65)
{
temp=0xA1; // 2단계 속도 전진
}
else if(x_accel/200>=65)
{
temp=0xf1;
}
else if(x_accel/200<-25 && x_accel/200>-45)
{
temp=0×52; //1단계 속도 후진
}
else if(x_accel/200<=-45 && x_accel/200>-65)
{
temp=0xA2; //2단계 속도 후진
}
else if(x_accel/200<=-65)
{
temp=0xf2;
}
else if(y_accel/200>25 && y_accel/200<45 )
{
temp=0×53; //좌회전 1단계
}
else if(y_accel/200>=45 && y_accel/200<65)
{
temp=0xA3; //좌회전 2단계
}
else if(y_accel/200>=65)
{
temp=0xf3;
}
else if(y_accel/200<-25 && y_accel/200>-45 )
{
temp=0×54; //우회전 1단계
}
else if(y_accel/200<=-45 && y_accel/200>-65)
{
temp=0xA4; //우회전 2단계
}
else if(y_accel/200<=-65)
{
temp=0xf4;
}
else
{
temp=0;
}
buf=temp;
Putch(buf);
}
void Putch(char data)
{
while(!(UCSR1A & (1<<UDRE1)));
UDR1=data;
}