KoreanEnglish ROBOONE Korea Official Site Homesitemapmailadmin
모임소개일정관리공지사항게시판자료실대회현황ROBO-ONE Japan
 
자료실 > 강좌자료
게시물 번호    12 - 1 작 성 일    2003-03-27  01:53:21
글 쓴 이    메가로보틱스 Homepage     http://www.megarobotics.com

조회 : 3473 다수의 AIMOTOR를 싱크로 제어(인터럽트 이용) 수정하기 삭제하기   
첨부된 이미지 : 1human3-2.jpg


제목의 말은 다수의 모터를 제어할 때 각 모터의 위치변위가 다르게 되지만 같은 시간에 각기 다른 변위를 움직이게 되면 시작과 끝(시간)이 같게 움직일 수 있다. 이때 각모터의 변위값과 속도값은 반비례하게 될 것이다.

다음의 프로그램은 타임상수만큼의 시간동안에 각기 다른 목표변위를 갖는 두모터를 싱크로(동기화)시키는 것으로 타이머 인터럽트를 이용했으니 코드를 잘 분석하여 실제 로봇을 제어하는데 이용해 보기 바란다.

#include <mega32.h>

#include <delay.h>

#include <stdio.h>





#define RXB8 1

#define TXB8 0

#define OVR 3

#define FE 4

#define UDRE 5

#define RXC 7



#define RXCIE 7

#define TXCIE 6

#define UDRIE 5

#define TXEN 3

#define RXEN 4

#define CHR9 2





// Frequency = 7.3728MHz

/*

#define BR9600 47

// 클럭 7.3728 MHz일때의 보레이트 설정 상수 (에러 = 0%)

#define BR19200 23

#define BR38400 11

#define BR57600 7

#define BR115200 3

*/



// Frequency = 14.7456MHz

#define BR9600 95

// 클럭 14.7456 MHz일때의 보레이트 설정 상수 (에러 = 0%)

#define BR19200 47

#define BR38400 23

#define BR57600 11

#define BR115200 7



unsigned char G_ID=0; // 아이디 전역 변수







void Motor_Test1(void);



void main(void)

{

// Declare your local variables here







// 포트 설정

// Input/Output Ports initialization

// Port A initialization

// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In

// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T

PORTA=0x00;

DDRA=0x00;



// Port B initialization

// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In

// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T

PORTB=0x00;

DDRB=0x00;



// Port C initialization

// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In

// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T

PORTC=0x00;

DDRC=0x00;



// Port D initialization

// Func0=In Func1=Out Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In

// State0=P State1=1 State2=P State3=P State4=T State5=T State6=T State7=T

PORTD=0x0C;

DDRD=0x00;



// 타이머 설정

// Timer/Counter 0 initialization

// Clock source: System Clock

// Clock value: Timer 0 Stopped

// Mode: Normal top=FFh

// OC0 output: Disconnected

TCCR0=0x00;

TCNT0=0x00;

OCR0=0x00;



// Timer/Counter 1 initialization

// Clock source: System Clock

// Clock value: Timer 1 Stopped

// Mode: Normal top=FFFFh

// OC1A output: Discon.

// OC1B output: Discon.

// Noise Canceler: Off

// Input Capture on Falling Edge

TCCR1A=0x00;

TCCR1B=0x00;

TCNT1H=0x00;

TCNT1L=0x00;

OCR1AH=0x00;

OCR1AL=0x00;

OCR1BH=0x00;

OCR1BL=0x00;



// Timer/Counter 2 initialization

// Clock source: System Clock

// Clock value: Timer 2 Stopped

// Mode: Normal top=FFh

// OC2 output: Disconnected

ASSR=0x00;

TCCR2=0x00;

TCNT2=0x00;

OCR2=0x00;



// 외부 인터럽트와 칩 설정

// External Interrupt(s) initialization

// INT0: off

// INT1: off

// INT2: Off

GICR=0x00;

MCUCR=0x0F;

MCUCSR=0x00;

GIFR=0x00;



// 타이머 인터럽트 설정

// Timer(s)/Counter(s) Interrupt(s) initialization

TIMSK=0x00;



// 시리얼 포트 설정

// USART initialization

// Communication Parameters: 8 Data, 1 Stop, No Parity

// USART Receiver: On

// USART Transmitter: On

// USART Mode: Asynchronous

// USART Baud rate: 115200

UCSRA=0x00;

UCSRB=0x18;

UCSRC=0x86;

UBRRL=0x07;

UBRRH=0x00;



// Analog Comparator initialization

// Analog Comparator: Off

// Analog Comparator Input Capture by Timer/Counter 1: Off

// Analog Comparator Output: Off

ACSR=0x80;

SFIOR=0x00;



delay_ms(1000);

Motor_Test1();



}



// 시리얼 포트로 한 문자를 전송하기 위한 함수

void sciTxData(unsigned char td)

{

while(!(UCSRA&(1<<UDRE)));

// 이전의 전송이 완료될때까지 대기

UDR=td;

}



// 시리얼 포트로 한 문자를 받을때까지 대기하기 위한 함수

unsigned char sciRxReady(void)

{

while(!(UCSRA&(1<<RXC)) );

// 한 문자가 수신될때까지 대기

return UDR;

}



// 모터에 위치, 회전, 파워 절약 모드를 제어할때 모터의 프로토콜로 전송하는 함수

void Send2MotorCMD(unsigned char mode,unsigned char id,unsigned char data)

{

// id : 0 ~ 30

// 1.Position Control Mode : mode = ( 0 ~ 4 ) -> speed , data = ( 0 ~ 254 )

// 2.Position Read Mode : mode = ( 5 ) , data = ( x )

// 3.Act Down Mode : mode = ( 6 ) , data = ( 0x10 )

// 4.Power Down Mode : mode = ( 6 ) , data = ( 0x20 )

// 5.Wheel Mode : mode = ( 6 ) , data = ( 0x30 : CCW , 0x40 : CW ) | ( 0x00 ~ 0x0F : Speed )



unsigned char modid=0;

modid=((mode<<5)&0xe0) | id;

// 7 ~ 5 비트는 모드 , 4 ~ 0 비트는 아이디

sciTxData(0xff);

// 모터의 수신 초기화 값 전송

delay_us(5);

sciTxData(modid);

// 모드 | 아이디 전송

delay_us(5);

sciTxData(data);

// 위치 또는 제어값 전송

delay_us(5);

sciTxData( (modid^data)&0x7f ); // 체크섬 전송

delay_us(5);

}



// 모터의 세팅 모드를 제어할때 모터의 프로토콜로 전송하는 함수

void Send2MotorSetCMD(unsigned char mode,unsigned char id,unsigned char setmode,unsigned char setdata1,unsigned char setdata2)

{

// id : 0 ~ 30

// mode : 7 ( all Mode )

// x is any value

// 1.Baud Rate Setting Mode : setmode = ( 8 ) , setdata1 = ( Baud Rate Constant ) , setdata = ( x )

// 2.Gain Setting Mode : setmode = ( 9 ) , setdata1 = ( pGain Constant ) , setdata = ( dGain Constant )

// 3.ID Setting Mode : setmode = ( 10 ) , setdata1 = ( ID Constant ) , setdata = ( x )

// 4.Auto Gain Setting Mode : setmode = ( 11 ) , setdata1 = ( x ) , setdata = ( x )

// 5.Gain Read Mode : setdata1 = setmode = ( 12 ) , setdata1 = ( x ) , setdata = ( x )

// 6.Resolution Setting Mode : setmode = ( 13 ) , setdata1 = ( Resolution Constant : 0 or 1 ) , setdata = ( x )

// 7.Resolution Read Mode : setmode = ( 14 ) , setdata1 = ( x ) , setdata = ( x )

// 8.Over Current Setting Mode : setmode = ( 15 ) , setdata1 = ( Current Constant ) , setdata = ( x )

// 9.Over Current Read Mode : setmode = ( 16 ) , setdata1 = ( x ) , setdata = ( x )



unsigned char modid=0;

modid=((mode<<5)&0xe0) | id;

// 7 ~ 5 비트는 모드 , 4 ~ 0 비트는 아이디

sciTxData(0xff);

// 모터의 수신 초기화 값 전송

delay_us(5);

sciTxData(modid);

// 모드 | 아이디 전송

delay_us(5);

sciTxData(setmode);

// 세팅 모드 값 전송

delay_us(5);

sciTxData(setdata1);

// 첫번째 세팅 값 전송

delay_us(5);

sciTxData(setdata2);

// 두번째 세팅 값 전송

delay_us(5);

sciTxData( (modid^setmode^setdata1^setdata2)&0x7f ); // 체크섬 전송

delay_us(5);

}



// 위치 제어 함수

unsigned char Position_Control(unsigned char id,unsigned char position,unsigned char spd)

{

#define MOT_SPEED0 0

// 위치 제어 모드일때의 모터의 회전 속도 상수

#define MOT_SPEED1 1

#define MOT_SPEED2 2

#define MOT_SPEED3 3

#define MOT_SPEED4 4

unsigned char torque;

Send2MotorCMD( spd, id, position );

torque=sciRxReady();

sciRxReady();

return torque;





}



// 위치 확인 함수

unsigned char Position_Read(unsigned char id)

{

unsigned char pos;

Send2MotorCMD( 0x05, id, 0 );

sciRxReady();

pos=sciRxReady();;

return pos;



//sciRxReady();

// 첫번째 피드백 토크값 스킵

//return sciRxReady();

// 두번째 피드백 위치값 반환



}



// 모터의 전류 흐름을 차단하는 함수

void ActDown_Mode(unsigned char id)

{

#define ACT_DOWN 0x10

Send2MotorCMD( 0x06, id, ACT_DOWN );

}



// 파워 절약 모드 함수

void PwrDown_Mode(unsigned char id)

{

#define PWR_DOWN 0x20

Send2MotorCMD( 0x06, id, PWR_DOWN );

}



// 회전 모드 제어 함수

void Wheel_Mode(unsigned char id, unsigned char dir, unsigned char spd)

{

// id : 모터의 아이디

// dir : 모터의 방향 ( CW or CCW )

// spd : 모터의 회전 속도 ( 0 ~ 15 )



#define WHEEL_CW 0x40

#define WHEEL_CCW 0x30



#define WHEEL_STOP 0x00

// 방향 | 속도

Send2MotorCMD( 0x06, id , dir | spd );

}



// 보레이트 설정 함수

void BaudSet_Mode(unsigned char id, unsigned char Baud)

{

// Baud : 보레이트 상수

// Frequency = 14.7456MHz

#define BR9600 95

#define BR19200 47

#define BR38400 23

#define BR57600 11

#define BR115200 7



#define BAUD_SET 0x08

Send2MotorSetCMD( 0x07, id , BAUD_SET, Baud, Baud );



}



// 게인 설정 함수

void GainSet_Mode(unsigned char id, unsigned char pgain, unsigned char dgain)

{

#define GAIN_SET 0x09

Send2MotorSetCMD( 0x07, id , GAIN_SET, pgain, dgain );



}



// 아이디 설정 함수

void IDSet_Mode(unsigned char id, unsigned char newid)

{

// newid : 0 ~ 30



#define ID_SET 0x0A

Send2MotorSetCMD( 0x07, id , ID_SET, newid, newid );

}



void AutoGainSet_Mode(unsigned char id, unsigned char pgain, unsigned char dgain)

{

#define AGAIN_SET 0x0B

Send2MotorSetCMD( 0x07, id , AGAIN_SET, 0, 0 );



}



// 현재의 게인값 확인 함수

void GainRead_Mode(unsigned char id, unsigned char *pgain, unsigned char *dgain)

{

#define GAIN_READ 0x0C

Send2MotorSetCMD( 0x07, id , GAIN_READ, 0, 0 );

*pgain=sciRxReady();

*dgain=sciRxReady();

}



// 분해능 설정 함수

void ResolutionSet_Mode(unsigned char id, unsigned char resol)

{

// resol : 0 or 1



#define RESOL_SET 0x0D



#define RESOL_LOW 0

#define RESOL_HIGH 1



Send2MotorSetCMD( 0x07, id , RESOL_SET, resol, resol );

}



// 현재의 분해능 확인 함수

unsigned char ResolutionRead_Mode(unsigned char id)

{

#define RESOL_READ 0x0E

Send2MotorSetCMD( 0x07, id , RESOL_READ, 0, 0 );

return sciRxReady();





}



// 과전류 차단 설정 함수

void OverCurrentSet_Mode(unsigned char id, unsigned char current)

{



// current : 전류 상수 값



#define OVERCUR_SET 0x0F



// 전류 설정 상수

#define OC_400MA 20 // 400mA

#define OC_450MA 23

#define OC_500MA 25

#define OC_550MA 28

#define OC_600MA 30

#define OC_650MA 33

#define OC_700MA 35

#define OC_750MA 38

#define OC_800MA 40

#define OC_850MA 43

#define OC_900MA 46

#define OC_950MA 48

#define OC_1000MA 51 // 1000mA



Send2MotorSetCMD( 0x07, id , OVERCUR_SET, current, current );

}



// 현재 차단 전류값 확인 함수

unsigned char OverCurrentRead_Mode(unsigned char id)

{

#define OVERCUR_READ 0x10



Send2MotorSetCMD( 0x07, id , OVERCUR_READ, 0 , 0 );

return sciRxReady();



}



unsigned int Interval;

unsigned char Src_Pos_0,Src_Pos_1;

unsigned char Dest_Pos_0,Dest_Pos_1;

unsigned char Sample_Num,Count;





interrupt [TIM1_OVF] void Timer1_Int(void)

{

float D0,D1;



D0=((Dest_Pos_0-Src_Pos_0)/Sample_Num)*Count;

D1=((Dest_Pos_1-Src_Pos_1)/Sample_Num)*Count;



Position_Control( 0 , Src_Pos_0+(unsigned char)D0 , MOT_SPEED0 );

Position_Control( 18 , Src_Pos_1+(unsigned char)D1 , MOT_SPEED0 );





TCNT1=65535-Interval*10;

if( Count++ >= Sample_Num )TCCR1B=0x00; // TIMER1 STOP

}



void Motor_Test1(void)

{

TIMSK=0x04; // TIMER1 INTERRUPT ENABLE

TCCR1B=0x00; // TIMER1 STOP



Src_Pos_0=80;

Src_Pos_1=80;



Dest_Pos_0=180;

Dest_Pos_1=240;



Sample_Num=10;



Interval=100;



Count=1;





Position_Control( 0 , Src_Pos_0 , MOT_SPEED0 );

Position_Control( 18 , Src_Pos_1 , MOT_SPEED0 );

#asm("sei"); //gloval interupt enable

TCCR1B=0x05; // Prescale CLK/1024 & Timer1 Start

while(1){};

}

 

답변하기 목록보기 이전글 다음글

[ 현재 페이지 전체 목록 ]
    제                  목 글 쓴 이 등 록 일 조회 받음
20 파일받기:pyro.dwg(98K) 제 로봇 쇠돌이(Pyro)의 설계도입니다. 김낙현 2003-07-30 5560 159
19   R/C 서보 모터와 AI Motor 비교 메가로보틱스 2003-07-02 4873
18   AI Motor관련 매니아와 일문일답 메가로보틱스 2003-06-25 4345
17   AI Motor의 가격비밀 메가로보틱스 2003-06-25 5323
16   [자료] AIMOTOR로 만든 2 족 로봇 사진 3-2 메가로보틱스 2003-04-28 4901
15   [TIP4] AIMOTOR 에 메탈기어를 사용하... 메가로보틱스 2003-04-21 3966
14   [Tip3] AIMOTOR 로 이족 로봇을 만들때... 메가로보틱스 2003-04-14 5139
13 파일받기:1슈퍼짱 로봇 VER1.0.pdf(223K) TMS320F240 DSP 김진범 2003-04-10 3516 414
12   다수의 AIMOTOR를 싱크로 제어(인터럽트 이용) 메가로보틱스 2003-03-27 3473
11   AIMOTOR의 포지션리드 테스트 메가로보틱스 2003-03-27 3622
 
Copyright ⓒ Since 2005 www.robo-one.or.kr All rights reservd. Designed by SaehanDataSystem
이메일주소 무단수집 거부