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

TODAY633 TOTAL398,154
사이트 이용안내
Login▼/회원가입
최신글보기 질문게시판 기술자료 동영상강좌

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


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

페이지 정보

작성자 master 작성일2018-07-11 15:40 조회579회 댓글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

조회수 579

등록된 댓글이 없습니다.

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

MCU, AVR, 아두이노 등 전자공학에 관련된 질문을 무료회원가입 후 작성해주시면 전문가가 답변해드립니다.
ATMEGA128PWMLED초음파
아두이노AVR블루투스LCD
UART모터적외선ATMEGA
전체 스위치 센서
질문게시판 목록
제목 작성자 작성일 조회
공지 MCU, AVR, 아두이노 등 전자공학에 관련된 질문은 질문게시판에서만 작성 가능합니다. 스태프 19-01-15 81
공지 사이트 이용 안내 댓글[5] master 17-10-29 4834
질문 ATmega128 압력센서 소스에대해서 질문드리겠습니다. 새글 YDyd 19-02-15 8
답변 답변글 답변 : ATmega128 압력센서 소스에대해서 질문드리겠습니다. 새글 master 19-02-15 8
질문 어떤분이 올린 소스를 활용하려고 하는데요. 질문좀 드려요 스캉이 19-02-14 10
답변 답변글 답변 : 어떤분이 올린 소스를 활용하려고 하는데요. 질문좀 드려요 댓글[1] master 19-02-15 12
질문 안녕하세요. 질문이 있습니다 댓글[1] qkr영우우우 19-02-14 16
질문 마스터님 스텝모터 질문좀드립니다. 댓글[1] 잘하고싶다 19-02-14 17
질문 마이크로칩 PIC 댓글[5] c린이 19-02-14 20
질문 부저 코드에서 핀을 바꾸고 싶습니다. 댓글[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 41
질문 딜레이 회로 댓글[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
모바일버전으로보기