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

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


BASIC4MCU 후원업체 최신 정보

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

페이지 정보

작성자 master 작성일2018-07-11 15:40 조회204회 댓글0건

본문

	

 

// DateTime : 2018-07-11 오후 3:41:30
// by Ok-Hyun Park
//
#include <mega128.h>
#include <delay.h>
#include <stdlib.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=0x00OCR1AL=0xFFdelay_ms(1);
    OCR1AH=0x01OCR1AL=0xFFdelay_ms(1);
    OCR1AH=0x02OCR1AL=0xFFdelay_ms(1);
    OCR1AH=0x03OCR1AL=0xFFdelay_ms(1);
    //toward=1;
  }
  if(toward==1){
    OCR1AH=0x00OCR1AL=0xFFdelay_ms(1);
    OCR1AH=0x01OCR1AL=0xFFdelay_ms(1);
    OCR1AH=0x02OCR1AL=0xFFdelay_ms(1);
    OCR1AH=0x03OCR1AL=0xFFdelay_ms(1);
  }
}
//
void MotorA_1(void){ OCR0=0x3F; }
//
void MotorA_2(void){ OCR0=0x7F; }
//
void Interrupt_init(void){
  EICRB=0x02EIMSK=0x10DDRE.4=0SREG|=0x80;
}
//
interrupt[EXT_INT4]void ext_int4_isr(void){ speed_flag++; }
//    speed_flag=~speed_flag;
void Port_Init(){
  DDRB=0xFF// 모터
  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 초기화
  UCSR0B=0x98UBRR0L=0x67// 16MHZ 9600bps
  printf("STARTING SYSTEM...\n\r");
}
//
unsigned char MPU6050_read(unsigned char addr){
  unsigned char dat;
  TWCR=0xA4while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x08)));
  TWDR=0xD0;
  TWCR=0x84while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x18)));
  TWDR=addr;
  TWCR=0x84while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x28)));
  TWCR=0xA4while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x10)));
  TWDR=0xD1;
  TWCR=0x84while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x40)));
  TWCR=0x84while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x58)));
  dat=TWDR;
  TWCR=0x94
  return dat;
}
//
void MPU6050_write(unsigned char addr,char data){
  TWCR=0xA4while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x08)));
  TWDR=0xD0;
  TWCR=0x84while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x18)));
  TWDR=addr// addr=0x43
  TWCR=0x84while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x28)));
  TWDR=data;
  TWCR=0x84while(((TWCR&0x80)==0x00||((TWSR&0xF8)!=0x28)));
  TWCR=0x94
  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); x_aTmp2=sqrt((float)x_aTmp1); x_aResult=atan((float)ax/x_aTmp2);
  y_aTmp1=((long)ax*(long)ax)+((long)az*(long)az); y_aTmp2=sqrt((float)y_aTmp1); y_aResult=atan((float)ay/y_aTmp2);
  z_aTmp1=((long)ay*(long)ay)+((long)az*(long)az); z_aTmp2=sqrt((float)z_aTmp1); z_aResult=atan(z_aTmp2/(float)az);
}
//
void getGyroDegree(void){ // 자이로 측정값들을 라디안 단위로
  x_gTmp1=(float)gx/65536x_gTmp1=x_gTmp1*1.8x_gResult=x_gTmp1;
  y_gTmp1=(float)gy/65536y_gTmp1=y_gTmp1*1.8y_gResult=y_gTmp1;
  z_gTmp1=(float)gz/65536z_gTmp1=z_gTmp1*1.8z_gResult=z_gTmp1;
}
//
void compFilter(void){
  xTmp1=(-y_aResult)+(float)xFilterAnglexIntTmp1=(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)yFilterAngleyIntTmp1=(float)yIntTmp1+(yTmp1*0.01); yTmp2=(-kp*yTmp1)+(-ki*(float)yIntTmp1)+y_gResult;
  yFilterAngle+=yTmp2*0.01roll=(int)(yFilterAngle*180/PI);
  yaw=(int)(0.976*(yaw+((gz/131.)*0.001)))+(0.024*(atan2(ax,ay)*180/PI));
}
//
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=1for(i=1i<2i++)MotorB(); toward=1button_2=0; } //앞을 보고 있을 때 //시계방향
      else if(toward==0){ PORTB.0=1//시계방향
        if(PINF.1==0){ for(i=1i<2i++)MotorB(); toward=1button_2=0; } //스위치를 누를 때
      }
      else// toward==1 뒤를 보고 있을 때
        PORTB.0=0//반시계방향
        if(PINF.1==0){ for(i=1i<2i++)MotorB(); toward=0button_2=1; }
      }
    }
    else//안움직일 때
      PORTB.0=0//반시계방향
      if(toward==1){ //뒤를 보고 있을 때
        if(PINF.1==0){ for(i=0i<2i++)MotorB(); toward=0button_2=0; }
      }
      else// toward==0 앞을 보고 있을 때
        PORTB.0=1//시계방향
        if(PINF.1==0){ for(i=0i<2i++)MotorB(); toward=1button_2=0; }
      }
    }
    //------------------바퀴제어------------------
    if     (speed_flag==1){ MotorA_1(); delay_ms(1); }
    else if(speed_flag==2){ MotorA_2(); delay_ms(1); }
    else                  { OCR0=0xFFspeed_flag=0; }
    delay_ms(1);
  }
}
//
-인터럽트 4번을 이용하여 버튼을 누르면 dc 모터가 속도가 달라지면서 굴러가고
자이로 센서의 값에 따라 스탭모터가 원하는 각만큼 for문을 이용하여 돌게 구성 했습니다.
그런데 스탭 모터가 돌아갈때 for문에 걸리지 않고 계속 돌아가고 드드득 소리가 나면서 돌아갑니다.어떻게 해야 될까요?

 

소스코드를 정리해드리면

다음번 질문할 때는 정리해드린 소스로 질문해주세요

 

스텝모터에서 드드득 소리가 나면 탈조하는겁니다.

탈조 원인은 여러가지가 있는데요

1. 스텝딜레이가 빠른 경우

2. 스텝신호가 맞지 않는 경우

3. 부하를 구동하기 위한 모터 토크가 부족한 경우

4. 전원장치 전류가 부족할 때

5. 드라이버모듈 전류 설정이 부족할 때

등 여러가지 이유가 있을 수 있습니다.

 

잘 체크해보세요

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

댓글 0

조회수 204

등록된 댓글이 없습니다.

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

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

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

Privacy Policy
MCU BASIC ⓒ 2017