Login▼/회원가입
BASIC4MCU 후원업체 최신 정보
BASIC4MCU | 질문게시판 | 답변 : 답변 : 모터 제어 질문
페이지 정보
작성자 뽕보 작성일2018-07-11 15:58 조회230회 댓글1건관련링크
본문
#include <stdlib.h>
#include <delay.h>
#include <mega128.h>
#include <math.h>
#include <stdio.h>
#include <string.h>#define GYROSCOPE_SENSITIVITY 250/32768
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, z_gTmp1, z_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, yaw=0;///////////////////////////////////////////////////////////
int toward=0;
int button_2=0;
int speed_flag=0;
////////////////////////////////////////////////////////////volatile int avr_pit=0, avr_rol=0, avr_yaw=0;
unsigned char buffer[12];
//
//void MotorB(void)
//{
// if (toward==0)
// {
//
// OCR1AH = 0x00;
// OCR1AL = 0xFF;
// delay_ms(1);
//
// OCR1AH = 0x01;
// OCR1AL = 0xFF;
// delay_ms(1);
//
// OCR1AH = 0x02;
// OCR1AL = 0xFF;
// delay_ms(1);
//
// OCR1AH = 0x03;
// OCR1AL = 0xFF;
// delay_ms(1);
//
//
// //toward=1;
// }
//
// if (toward==1)
// {
//
// OCR1AH = 0x00;
// OCR1AL = 0xFF;
// delay_ms(1);
//
// OCR1AH = 0x01;
// OCR1AL = 0xFF;
// delay_ms(1);
//
// OCR1AH = 0x02;
// OCR1AL = 0xFF;
// delay_ms(1);
//
// OCR1AH = 0x03;
// OCR1AL = 0xFF;
// delay_ms(1);
// }
//
//}
//
//
//void MotorA_1(void)
//{
// OCR0 = 0x3F;
//}
//
//void MotorA_2(void)
//{
//
// OCR0 = 0x7F;
//}void Interrupt_init(void)
{
EICRB = 0x02;
EIMSK = 0x10;
DDRE.4=0;
SREG |= 0x80;}
interrupt [EXT_INT4] void ext_int4_isr(void)
{
speed_flag++;}
// speed_flag=~speed_flag;void Port_Init()
{
DDRB = 0xFF; //모터를 구동시키기 위해 포트B의 8비트 모두 출력으로 설정. MAIN 모터
DDRC = 0xFF; // 서브 모터 제
PORTB.1=1;
DDRF = 0xf0; //버튼
TCCR0 = 0b01111010; //fast PWM mode : 3,6(WGM01,00) - 11, 5,4(COM ) - 11, 2-0(CS) - 010
TCCR1A |= (1<<COM1A1) | (1<<WGM11) | (1<<WGM10);
TCCR1B |= (1<<WGM12) | (1<<CS11) | (1<<CS10) ;
TIMSK = 1<<TOIE1;
}void Init_USART0()// USART0 초기화
{
UCSR0A = 0x00; // 1배속 전송모드 사용, 송수신 상태 모두 클리어
UCSR0B = (1<<RXCIE0) | (1<<RXEN0) | (1<<TXEN0); // 폴링방식 송수신모드 및 송신 인터럽트 허용
UCSR0C = 0x06; // 비동기모드, 패리티 사용 안함, 정지비트: 1비트, 전송 문자 비트수: 8비트UBRR0H = 0x00;
UBRR0L = 0x67; // 시스템 클럭 16MHZ의 전송속도 115200bps 설정
printf("STARTING SYSTEM ... \n\r"); // USART0 초기화 시 시스템 동작을 표시\
}unsigned char MPU6050_read(unsigned char addr)
{
unsigned char dat;
TWCR = 0xA4;// ((1<<TWINT)|(1<<TWSTA)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWSTA는 마스터로서의 동작, 버스사용이 가능하면 START 조건 출력 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x08)));// TWCR레지스터의 TWINT가 0 이거나 TWSR의 TWS3비트가 1이 아니라면 계속 반복
TWDR = 0xD0;
TWCR = 0x84;// ((1<<TWINT)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x18)));// TWCR레지스터의 TWINT가 0 이거나
TWDR = addr;
TWCR = 0x84;// ((1<<TWINT)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x28)));// TWCR레지스터의 TWINT가 0 이거나
TWCR = 0xA4;// ((1<<TWINT)|(1<<TWSTA)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWSTA는 마스터로서의 동작, 버스사용이 가능하면 START 조건 출력 / TWINT TW인터럽트
//-------------------------------------------------------------
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x10)));// TWCR레지스터의 TWINT가 0 이거나
TWDR = 0xD1;
TWCR = 0x84;// ((1<<TWINT)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x40)));// TWCR레지스터의 TWINT가 0 이거나
TWCR = 0x84;// ((1<<TWINT)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x58)));// TWCR레지스터의 TWINT가 0 이거나
dat = TWDR;
TWCR = 0x94;// ((1<<TWINT)|(1<<TWSTA)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWSTO는 마스터에선 TWI STOP, 슬레이브에선 SCL과 SDA 신호선을 High-Z상태로 하여 에러상태 해제 / TWINT TW인터럽트
return dat;
}void MPU6050_write(unsigned char addr, char data)
{
TWCR = 0xA4;// ((1<<TWINT)|(1<<TWSTA)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWSTA는 마스터로서의 동작, 버스사용이 가능하면 START 조건 출력 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x08)));// TWCR레지스터의 TWINT가 0 이거나
TWDR = 0xD0;
TWCR = 0x84;// ((1<<TWINT)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x18)));// TWCR레지스터의 TWINT가 0 이거나
TWDR = addr; // addr = 0x43
TWCR = 0x84;// ((1<<TWINT)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x28)));// TWCR레지스터의 TWINT가 0 이거나
//-------------------------------------------------------------
TWDR = data;
TWCR = 0x84;// ((1<<TWINT)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWINT TW인터럽트
while(((TWCR & 0x80) == 0x00 || ((TWSR & 0xF8) != 0x28)));// TWCR레지스터의 TWINT가 0 이거나
TWCR = 0x94;// ((1<<TWINT)|(1<<TWSTA)|(1<<TWEN)) -> TWEN으로 TWI 허가 / TWSTO는 마스터에선 TWI STOP, 슬레이브에선 SCL과 SDA 신호선을 High-Z상태로 하여 에러상태 해제 / TWINT TW인터럽트
delay_us(50);
}void getRawData()
{
buffer[0] = MPU6050_read(0x3B);// ax-H
buffer[1] = MPU6050_read(0x3C);// ax-L
buffer[2] = MPU6050_read(0x3D);// ay-H
buffer[3] = MPU6050_read(0x3E);// ay-L
buffer[4] = MPU6050_read(0x3F);// az-H
buffer[5] = MPU6050_read(0x40);// ax-L
buffer[6] = MPU6050_read(0x43);// gx-H
buffer[7] = MPU6050_read(0x44);// gx-L
buffer[8] = MPU6050_read(0x45);// gy-H
buffer[9] = MPU6050_read(0x46);// gy-L
buffer[10] = MPU6050_read(0x47);//gz-H
buffer[11] = MPU6050_read(0x48);//gz-L
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;
z_gTmp1=(float)gz/65536;
x_gTmp1=x_gTmp1*1.8;
y_gTmp1=y_gTmp1*1.8;
z_gTmp1=z_gTmp1*1.8;
x_gResult=x_gTmp1;
y_gResult=y_gTmp1;
z_gResult=z_gTmp1;
}void compFilter(void)
{
xTmp1=(-y_aResult)+(float)xFilterAngle;
xIntTmp1=(float)xIntTmp1+(xTmp1*0.01);
xTmp2=(-kp*xTmp1)+(-ki*(float)xIntTmp1)+x_gResult;
xFilterAngle=xFilterAngle+(xTmp2*0.01);
pitch=(int)(xFilterAngle*180/PI);
yTmp1=(-x_aResult)+(float)yFilterAngle;
yIntTmp1=(float)yIntTmp1+(yTmp1*0.01);
yTmp2=(-kp*yTmp1)+(-ki*(float)yIntTmp1)+y_gResult;
yFilterAngle+=yTmp2*0.01;
roll=(int)(yFilterAngle*180/PI);
yaw=(int)(0.976*(yaw+((gz/131.)*0.001)))+(0.024*(atan2(ax, ay)*180/PI));// 0.95와 0.05의 비중 바꿔보기!
}void main(void)
{
int i;
TWSR = 0x00;
TWBR = 12;// 16 MHz 시스템의 보드에서의 SCL주파수를 400 kHz로 설정
Init_USART0();
Port_Init();
Interrupt_init();
SREG |= 0x80;
MPU6050_write(0x6B, 0x00);
MPU6050_write(0x6C, 0x00);
speed_flag=0;
while(1)
{
getRawData();
getAcclDegree();
getGyroDegree();
compFilter();
printf("pitch : %d\rroll : %d\ryaw : %d\r", pitch, roll, yaw);
if(pitch>20)//움직일 때
{
PORTB.0 = 1; //시계방
for(i = 1; i<4; i++)
{
OCR1AH = 0x00;
OCR1AL = 0xFF;
delay_ms(1);
OCR1AH = 0x01;
OCR1AL = 0xFF;
delay_ms(1);
OCR1AH = 0x02;
OCR1AL = 0xFF;
delay_ms(1);
OCR1AH = 0x03;
OCR1AL = 0xFF;
delay_ms(1);
}
}
else //안움직일 때
{
PORTB.0 = 0; //반시계방
for(i = 1; i<4; i++)
{
OCR1AH = 0x00;
OCR1AL = 0xFF;
delay_ms(1);
OCR1AH = 0x01;
OCR1AL = 0xFF;
delay_ms(1);
OCR1AH = 0x02;
OCR1AL = 0xFF;
delay_ms(1);
OCR1AH = 0x03;
OCR1AL = 0xFF;
delay_ms(1);
}
}
/////////////////바퀴제어/////////////////////////////
// if (speed_flag==1)
// {
// MotorA_1();
// delay_ms(1);
//
// }
//
// else if(speed_flag==2)
// {
// MotorA_2();
// delay_ms(1);
// }
//
// else
// {
// OCR0 = 0xFF;
// speed_flag=0;
// }
//
//
delay_ms(1);
}
}
- 다른 코드를 올려서 죄송합니다 ㅜㅜ MASTER 님.
단순히 자이로 센서로 스탭모터를 돌리는 코드를 구현했는데 MASTER님이 수정하신 부분을 이 코드에서도 수정을 해서 해 봤는데 for문에서 시계방향으로 일정 각만큼 돌고 반시계방향으로도 일정각만큼 돌게 하고 싶은데 시계 방향으로 계속 돌아 갑니다
- BASIC4MCU 작성글 SNS에 공유하기
댓글 1
조회수 230master님의 댓글
master 작성일
파워서플라이 규격(또는 판매처링크)
모터 판매처링크
모터 드라이버모듈 판매처링크
회로도
첨부해보세요