Login▼/회원가입BASIC4MCU | 질문게시판 | atmega128 소스결합 오류
페이지 정보
작성자 일본인호ㅈ두 작성일2019-08-21 15:29 조회8,145회 댓글1건첨부파일
본문
atmega128을 사용해서 작품을 만들고있는도중 소스를 따로따로 돌렸을때는 작동이 되었는데 소스들을 조금식 참고해서 결합시키니 센서들과 부품들이 마음데로 작동이 안됩니다 ㅠㅠ 레지스터에서 겹치는게 있는지 봐도 잘 모르겠네요. 그리고 자이로센서 코딩은 받은거라서 뭐를 건드려야할지도 잘모르겠네요ㅜ
사용하는 센서는 자이로센서(MPU-6050) , 블루투스(HC-06)와 서보모터(HS-311),360도 서보모터,dc모터(모터드라이브를 납땜하여 만들어 사용했습니다)
자이로센서 PD0,PD1
블루투스 PE0,PE1
dc모터(드라이브) :PB0,BP1,PB2,PB3,PB4(PWM)
서보모터 :PB5,PB6
#include <avr/io.h>
#include <stdlib.h>
#include <util/delay.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#define F_CPU 16000000UL
#include<avr/signal.h>
unsigned char led;
unsigned char T = 0;
int a = 0;
int i;
int x=15;
typedef unsigned char byte;
//블루투스
volatile unsigned char flag;
ISR(USART0_RX_vect)
{
flag=UDR0;
}
//서보모터
int d; int c;
void RC_Motor1(int angle){ // angle : -90도 ~ +90도
int i;
i=(angle)*18+3000;
OCR1A=i; //PB5
}
void RC_Motor(int angle){ // angle : -90도 ~ +90도
int w;
w=(angle)*18+3000;
OCR1B=w; //PB6
}
byte MPU6050_read(byte addr);
void MPU6050_write(byte addr, char data);
void TX_CHAR(char data);
void TX_STR(char *s);
char str[100];void getRawData();
void getAcclDegree();
void getGyroDegree();
void compFilter();
void gyro1(byte roll, byte pitch);volatile double dt = 0.000;
volatile int gx = 0, gy = 0, gz = 0, ax = 0, ay = 0, az = 0;
volatile long x_aTmp1, y_aTmp1, z_aTmp1;
volatile float x_aTmp2, y_aTmp2, z_aTmp2, x_aResult, y_aResult, z_aResult;
volatile float x_gTmp1, y_gTmp1, x_gResult, y_gResult;float kp = 12.0f, ki = 1.0f;
volatile float xTmp1, yTmp1, xTmp2, yTmp2, xIntTmp1, yIntTmp1;
volatile float xFilterAngle = 0.0f, yFilterAngle = 0.0f;
volatile int pitch = 0, roll = 0;
byte buffer[12];
//블루투스
void init()
{
DDRA=0xff;
UCSR0A=0x00;
UCSR0B=0x98;
UCSR0C=0x06;
UBRR0H=0;
UBRR0L=103;
SREG=0x80;
}
//=====================================================================//
int main(void){
DDRB=0xFF; // PB5,6 out
TCCR1A=0xAA; TCCR1B=0x1A; OCR1A=3000; OCR1B=3000; ICR1=47999;
//dc모터 (속도제어)
TIMSK = 0x01;
TCCR0 = 0x7F;
OCR0 = 172;
//자이로 센서
UBRR0H=0; UBRR0L=103; UCSR0B=0x08;
DDRA=0xff;
DDRE = 0xFF;
DDRD=0x0f;
DDRB=0xFF;
SREG = 0x00;TWSR = 0x00;
TWBR = 0x12;
SREG = 0x80;//printf("\n\r start!! ");
MPU6050_write(0x6B, 0x00);
MPU6050_write(0x6C, 0x00);//_delay_ms(100);
//printf("\n\rqqqqq");init();
while(1){getRawData();
getAcclDegree();
getGyroDegree();
compFilter();
// 자이로 센서를 이용한 서보모터 각도제어
RC_Motor(d);
RC_Motor1(c);
if(roll>5){
d=d+1;
if(d>35){
d=35;
}
}
if(roll<-5){
d=d-1;
if(d<-40){
d=-40;
}
//블루투스를 이용한 dc모터 제어
}if(flag=='U')
{
PORTB=0x05;
}
if(flag=='D')
{
PORTB=0x0A;
}
if(flag=='S')
{
PORTB=0x00 ;
}
if(flag=='R')
{
PORTB=0x09;
}
if(flag=='L')
{
PORTB=0x06;
//블루투스를 이용한 360도 서보모터
}if(flag=='r')
{
for(c=0; c<31; c++)
_delay_ms(10);
}
if(flag=='l')
{
for(c=0; c>-31; c--)
_delay_ms(10);
}
if(flag=='s')
{
c=0;
}}
}
void TX_CHAR(char data) //1바이트 송신
{
while(!(UCSR0A&0x20));
UDR0=data;
}void TX_STR(char *s)
{
while(*s)TX_CHAR(*s++);
_delay_ms(5);
}
// 센서값 읽
byte MPU6050_read(byte addr)
{
byte dat;
TWCR = 0xA4;
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x08)));
TWDR = 0xD0;
TWCR = 0x84;
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x18)));TWDR = addr;
TWCR = 0x84;while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x28)));
TWCR = 0xA4;//-------------------------------------------------------------
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x10)));
TWDR = 0xD1;
TWCR = 0x84;
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x40)));
TWCR = 0x84;
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x58)));
dat = TWDR;
TWCR = 0x94;
_delay_us(10);
return dat;
}
//센서값에 며령어 전
void MPU6050_write(byte addr, char data)
{
_delay_us(50);
printf("\n\r 6050 write in !!");////////////////////
TWCR = 0xA4;
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x08)));
TWDR = 0xD0;
TWCR = 0x84;
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x18)));
TWDR = addr; // addr = 0x43
TWCR = 0x84;while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x28)));
//-------------------------------------------------------------
TWDR = data;
TWCR = 0x84;
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x28)));
TWCR = 0x94;
_delay_us(50);
printf("\n\r 6050 write out !!");///////////////
}
// 가속도 각속도 센서값 읽기
void getRawData()
{
buffer[0] = MPU6050_read(0x3B);
buffer[1] = MPU6050_read(0x3C);
buffer[2] = MPU6050_read(0x3D);
buffer[3] = MPU6050_read(0x3E);
buffer[4] = MPU6050_read(0x3F);
buffer[5] = MPU6050_read(0x40);
buffer[6] = MPU6050_read(0x43);
buffer[7] = MPU6050_read(0x44);
buffer[8] = MPU6050_read(0x45);
buffer[9] = MPU6050_read(0x46);
buffer[10] = MPU6050_read(0x47);
buffer[11] = MPU6050_read(0x48);ax = (int)buffer[0] << 8 | (int)buffer[1];
ay = (int)buffer[2] << 8 | (int)buffer[3];
az = (int)buffer[4] << 8 | (int)buffer[5];
gx = (int)buffer[6] << 8 | (int)buffer[7];
gy = (int)buffer[8] << 8 | (int)buffer[9];
gz = (int)buffer[10] << 8 | (int)buffer[11];}
//각속도void getAcclDegree(void)
{
x_aTmp1 = ((long)ay * (long)ay) + ((long)az * (long)az);
y_aTmp1 = ((long)ax * (long)ax) + ((long)az * (long)az);
z_aTmp1 = ((long)ay * (long)ay) + ((long)az * (long)az);
x_aTmp2 = sqrt((float)x_aTmp1);
y_aTmp2 = sqrt((float)y_aTmp1);
z_aTmp2 = sqrt((float)z_aTmp1);
x_aResult = atan((float)ax / x_aTmp2);
y_aResult = atan((float)ay / y_aTmp2);
z_aResult = atan(z_aTmp2 / (float)az);}
// 가속도 값void getGyroDegree(void)
{x_gTmp1 = (float)gx / 65536;
y_gTmp1 = (float)gy / 65536;
x_gTmp1 = x_gTmp1 * 1.8;
y_gTmp1 = y_gTmp1 * 1.8;
x_gResult = x_gTmp1;
y_gResult = y_gTmp1;}
// 센서값 보정 필터void compFilter(void)
{xTmp1 = (-y_aResult) + (float)xFilterAngle;
xIntTmp1 = (float)xIntTmp1 + (xTmp1 / 100);
xTmp2 = (-kp * xTmp1) + (-ki * (float)xIntTmp1) + x_gResult;
xFilterAngle = xFilterAngle + (xTmp2 / 100);
pitch = (int)(xFilterAngle * 180 / 3.14);yTmp1 = (-x_aResult) + (float)yFilterAngle;
yIntTmp1 = (float)yIntTmp1 + (yTmp1 / 100);
yTmp2 = (-kp * yTmp1) + (-ki * (float)yIntTmp1) + y_gResult;
yFilterAngle = yFilterAngle + (yTmp2 / 100);
roll = (yFilterAngle * 180 / 3.14);
}
- BASIC4MCU 작성글 SNS에 공유하기
댓글 1
조회수 8,145master님의 댓글
master 작성일
가장 어려운 소스를 기준으로
쉬운 것을 하나씩 차례로 붙이면서 동작시키면 이해하기 좋습니다.
완전히 이해하지 못하면 원하는대로 수정하지 못합니다.