질문게시판 목록

1 아두이노로 리니어 모터를 동작시키려고 합니…
2 아두이노를 이용한 RGB LED 전등 제어
3 ATMEGA128 interrupt에서 에…
4 답변 : 답변 : avreeprom 질문입…
5 아트메가128 송수신 의사코드 질문..
6 답변 : 초음파센서 dc모터 관련 질문입니…
7 답변 : avreeprom 질문입니다
8 아두이노 라이다lite-v3
9 답변 : ATMEGA128 interrup…
10 답변 : avr4stdio 사용 블루투스 …

▼ BASIC4MCU 후원업체 신제품 정보 ▼

▲ BASIC4MCU 후원업체 신제품 정보 ▲

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

페이지 정보

작성자 뽕보 작성일2018-07-11 15:58 조회378회 댓글1건

https://www.basic4mcu.com/bbs/board.php?bo_table=gac&wr_id=2428

본문

#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에 공유하기

master님의 댓글

master 작성일 18-07-11 16:48

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

댓글쓰기

비밀글사용
내용

	제목	작성자	작성일	조회
공지	[무료 공개] 소스코드 하이라이트 v2.0 beta [2013.02.07] 댓글[1]개	master	18-01-23	1838
공지	[공지] 질문글은 삭제하지 못합니다. 댓글[1]개	master	18-01-16	1257
질문	네오픽셀 관련 문의 입니다. 댓글[2]개	손문일	04:11	4
질문	atmeag128 codevision timer관련 질문있습니다 댓글[3]개	헤헿헿	00:11	13
답변	답변 : atmeag128 codevision timer관련 질문있습니다	master	01:51	4
질문	블루투스 송 수신 질문 입니다.	도도리	18-11-14	8
답변	답변 : 블루투스 송 수신 질문 입니다.	master	01:30	6
질문	codevision atmega128a 재질문 댓글[3]개	프로미스애인	18-11-14	16
질문	서보모터가 180만 회전해서... 댓글[1]개	아두이노정복	18-11-14	7
질문	아두이노 gnd핀 2개인이유? 댓글[1]개	공도리공	18-11-14	6
질문	아두이노 우노 보드를 이용한 유모차 제작중입니다	심슨	18-11-14	8
답변	답변 : 아두이노 우노 보드를 이용한 유모차 제작중입니다	master	18-11-14	12
질문	불꽃감지센서,기울기센서,노크센서,충격센서,온도센서 5개코드 합치는것좀 제발 도와주세요 ㅜㅠ	안절부절	18-11-14	6
답변	답변 : 불꽃감지센서,기울기센서,노크센서,충격센서,온도센서 5개코드 합치는것좀 제발 도와주세요 ㅜㅠ	master	18-11-14	8
질문	가변저항 초음파센서로 팬과 부저 제어	calicsray	18-11-14	7

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

가장 많이 본 질문,답변 (30일) 더보기 >

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

페이지 정보

관련링크

본문

댓글 1

master님의 댓글

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

ATMEGA128	PWM	LED	초음파
아두이노	AVR	블루투스	LCD
UART	모터	적외선	ATMEGA
전체	스위치	센서