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

TODAY631 TOTAL398,152
사이트 이용안내
Login▼/회원가입
최신글보기 질문게시판 기술자료 동영상강좌
아두이노 센서 ATMEGA128 PWM LED 초음파 AVR 블루투스 LCD UART 모터 적외선

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

페이지 정보

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

조회수 620

master님의 댓글

master 작성일

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

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

MCU, AVR, 아두이노 등 전자공학에 관련된 질문을 무료회원가입 후 작성해주시면 전문가가 답변해드립니다.
ATMEGA128PWMLED초음파
아두이노AVR블루투스LCD
UART모터적외선ATMEGA
전체 스위치 센서
질문게시판 목록
제목 작성자 작성일 조회
공지 MCU, AVR, 아두이노 등 전자공학에 관련된 질문은 질문게시판에서만 작성 가능합니다. 스태프 19-01-15 81
공지 사이트 이용 안내 댓글[5]개 master 17-10-29 4833
질문 ATmega128 압력센서 소스에대해서 질문드리겠습니다. 새글 YDyd 19-02-15 7
답변 답변글 답변 : ATmega128 압력센서 소스에대해서 질문드리겠습니다. 새글 master 19-02-15 7
질문 어떤분이 올린 소스를 활용하려고 하는데요. 질문좀 드려요 스캉이 19-02-14 10
답변 답변글 답변 : 어떤분이 올린 소스를 활용하려고 하는데요. 질문좀 드려요 댓글[1]개 master 19-02-15 10
질문 안녕하세요. 질문이 있습니다 댓글[1]개 qkr영우우우 19-02-14 15
질문 마스터님 스텝모터 질문좀드립니다. 댓글[1]개 잘하고싶다 19-02-14 16
질문 마이크로칩 PIC 댓글[5]개 c린이 19-02-14 15
질문 부저 코드에서 핀을 바꾸고 싶습니다. 댓글[3]개 이미지첨부파일 스캉이 19-02-14 12
질문 keil 과 ST-LINK Utility 사용도중 문제점 댓글[1]개 이미지첨부파일 공대생 19-02-13 21
질문 아두이노 서보모터 질문 (1회실행,속도조절) master 19-02-12 20
질문 아두이노 관련 질문드립니다. 댓글[1]개 박승규 19-02-12 24
질문 마스터님 메가128a 납땜후 질문좀드립니다. 잘하고싶다 19-02-12 16
답변 답변글 답변 : 마스터님 메가128a 납땜후 질문좀드립니다. master 19-02-12 10
질문 아두이노 피에조 부저 코드 관련 고기민 19-02-11 20
답변 답변글 답변 : 아두이노 피에조 부저 코드 관련 master 19-02-12 17
답변 답변글 답변 : 답변 : 아두이노 피에조 부저 코드 관련 댓글[2]개 고기민 19-02-12 16
질문 atmel studio 코딩 관련 질문드립니다!! 첨부파일 전기초보 19-02-11 15
답변 답변글 답변 : atmel studio 코딩 관련 질문드립니다!! master 19-02-11 12
질문 아두이노 맨땅에 헤딩 master 19-02-11 23
질문 아두이노 컴파일 오류 고기민 19-02-11 19
답변 답변글 답변 : 아두이노 컴파일 오류 댓글[1]개 master 19-02-11 14
질문 아두이노 소스 합치기 열심히하자 19-02-10 25
답변 답변글 답변 : 아두이노 소스 합치기 댓글[1]개 master 19-02-11 30
질문 아두이노 led바 질문 master 19-02-10 24
질문 아두이노 스위치와 서보모터 관련 질문 master 19-02-10 20
질문 아두이노 lcd 댓글[7]개 패드 19-02-10 35
질문 조도센서 댓글[5]개 이미지첨부파일 엘도라도 19-02-09 40
질문 딜레이 회로 댓글[1]개 이미지첨부파일 동지 19-02-09 28
게시물 검색
2019년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2018년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
Privacy Policy
MCU BASIC ⓒ 2017
모바일버전으로보기