질문게시판 > 답변 : 답변 : 모터 제어 질문

TODAY375 TOTAL182,575
사이트 이용안내
Login▼/회원가입
포럼 동영상강좌 회원가입


BASIC4MCU | 질문게시판 | 답변 : 답변 : 모터 제어 질문

페이지 정보

작성자 뽕보 작성일2018-07-11 15:58 조회58회 댓글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

조회수 58

master님의 댓글

master 작성일

파워서플라이 규격(또는 판매처링크)
모터 판매처링크
모터 드라이버모듈 판매처링크
회로도
첨부해보세요

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

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

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

Privacy Policy
MCU BASIC ⓒ 2017