질문게시판 > 모터 제어 질문

TODAY704 TOTAL241,219
사이트 이용안내
Login▼/회원가입
포럼 동영상강좌 회원가입


BASIC4MCU 후원업체 최신 정보

BASIC4MCU | 질문게시판 | 모터 제어 질문

페이지 정보

작성자 뽕보 작성일2018-07-11 15:11 조회215회 댓글0건

본문

	

#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; // 서브 모터 제
  
    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) ;
    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();
    
    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)//움직일 때
        {         
            if(toward == 0 && button_2 !=1) //앞을 보고 있을 때
            {  
                PORTB.0 = 1; //시계방향
                for(i=1;i<2;i++)
                {
                    MotorB(); //뒤로 돌린
                }
                toward=1;
                button_2 = 0;
               
            }
            else if (toward == 0)
            {
                PORTB.0 = 1; //시계방향
                if(PINF.1 == 0) //스위치를 누를 때
                {
                    for(i=1;i<2;i++)
                    {
                        MotorB(); //뒤로 돌린다
                    }
                    toward=1;
                    button_2 = 0;   
                }   
            }
            else // toward ==1 뒤를 보고 있을 때
            {    
                PORTB.0 = 0; //반시계방향
                if(PINF.1 == 0) //스위치를 누를 때
                {
                    for(i=1;i<2;i++)
                    {
                        MotorB(); //앞으로 돌린다
                    }
                    toward=0;
                    button_2 = 1;   
                }
            }
        }
        else //안움직일 때
        {    
            PORTB.0 = 0; //반시계방향
            if(toward == 1) //뒤를 보고 있을 때
            {
                if(PINF.1 == 0) //스위치를 누를 때
                {
                    for(i=0;i<2;i++)
                    {
                        MotorB();
                    }
                    toward=0;
                    button_2 = 0;
                }
            }
            else // toward ==0 앞을 보고 있을 때
            {   
                PORTB.0 = 1;//시계방향
                if(PINF.1 == 0) //스위치를 누를 때
                {
                    for(i=0;i<2;i++)
                    {
                        MotorB();
                    }
                    toward=1;
                    button_2 = 0;
                }
            }
        }  
        /////////////////바퀴제어/////////////////////////////  
        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);
    }
}

 

-인터럽트 4번을 이용하여 버튼을 누르면 dc 모터가 속도가 달라지면서 굴러가고

자이로 센서의 값에 따라 스탭모터가 원하는 각만큼 for문을 이용하여 돌게 구성 했습니다.

그런데 스탭 모터가 돌아갈때 for문에 걸리지 않고 계속 돌아가고 드드득 소리가 나면서 돌아갑니다 .어떻게 해야 될까요?

 

 

  • BASIC4MCU 작성글 SNS에 공유하기
  • 페이스북으로 보내기
  • 트위터로 보내기
  • 구글플러스로 보내기

댓글 0

조회수 215

등록된 댓글이 없습니다.

질문게시판HOME > 질문게시판 목록

MCU, AVR, 아두이노 등 전자공학에 관련된 질문을 무료회원가입 후 작성해주시면 전문가가 답변해드립니다.

ATMEGA128PWMLED초음파
아두이노AVR블루투스LCD
UART모터적외선ATMEGA
전체 스위치 센서
게시물 검색

Privacy Policy
MCU BASIC ⓒ 2017